11
#define M_PI 3.14159265358979323846264338327950288
24
#define RDK_SIZE_JOINTS_MAX 12
28
#define RDK_SIZE_MAX_CONFIG 4
36
typedef
doubletXYZWPR[6];
39
typedef
doubletXYZ[3];
56
typedef
doubletConfig[RDK_SIZE_MAX_CONFIG];
61
#define DOT(v,q) ((v)[0]*(q)[0] + (v)[1]*(q)[1] + (v)[2]*(q)[2])
64
#define NORM(v) (sqrt((v)[0]*(v)[0] + (v)[1]*(v)[1] + (v)[2]*(v)[2]))
67
#define NORMALIZE(inout){\
69
norm = sqrt((inout)[0]*(inout)[0] + (inout)[1]*(inout)[1] + (inout)[2]*(inout)[2]);\
70
(inout)[0] = (inout)[0]/norm;\
71
(inout)[1] = (inout)[1]/norm;\
72
(inout)[2] = (inout)[2]/norm;}
75
#define CROSS(out,a,b) \
76
(out)[0] = (a)[1]*(b)[2] - (b)[1]*(a)[2]; \
77
(出)[1]= (a) (b) [2] * [0] - (b)[2] *[0](一个);\
78
(out)[2] = (a)[0]*(b)[1] - (b)[0]*(a)[1];
81
#define COPY3(out,in)\
87
#define MULT_MAT(out,inA,inB)\
88
(out)[0] = (inA)[0]*(inB)[0] + (inA)[4]*(inB)[1] + (inA)[8]*(inB)[2];\
89
(out)[1] = (inA)[1]*(inB)[0] + (inA)[5]*(inB)[1] + (inA)[9]*(inB)[2];\
90
(out)[2] = (inA)[2]*(inB)[0] + (inA)[6]*(inB)[1] + (inA)[10]*(inB)[2];\
92
(out)[4] = (inA)[0]*(inB)[4] + (inA)[4]*(inB)[5] + (inA)[8]*(inB)[6];\
93
(out)[5] = (inA)[1]*(inB)[4] + (inA)[5]*(inB)[5] + (inA)[9]*(inB)[6];\
94
(out)[6] = (inA)[2]*(inB)[4] + (inA)[6]*(inB)[5] + (inA)[10]*(inB)[6];\
96
(out)[8] = (inA)[0]*(inB)[8] + (inA)[4]*(inB)[9] + (inA)[8]*(inB)[10];\
97
(out)[9] = (inA)[1]*(inB)[8] + (inA)[5]*(inB)[9] + (inA)[9]*(inB)[10];\
98
(out)[10] = (inA)[2]*(inB)[8] + (inA)[6]*(inB)[9] + (inA)[10]*(inB)[10];\
100
(out)[12] = (inA)[0]*(inB)[12] + (inA)[4]*(inB)[13] + (inA)[8]*(inB)[14] + (inA)[12];\
101
(out)[13] = (inA)[1]*(inB)[12] + (inA)[5]*(inB)[13] + (inA)[9]*(inB)[14] + (inA)[13];\
102
(out)[14] = (inA)[2]*(inB)[12] + (inA)[6]*(inB)[13] + (inA)[10]*(inB)[14] + (inA)[14];\
106
#define MULT_MAT_VECTOR(out,H,p)\
107
(out)[0] = (H)[0]*(p)[0] + (H)[4]*(p)[1] + (H)[8]*(p)[2];\
108
(out)[1] = (H)[1]*(p)[0] + (H)[5]*(p)[1] + (H)[9]*(p)[2];\
109
(out)[2] = (H)[2]*(p)[0] + (H)[6]*(p)[1] + (H)[10]*(p)[2];
112
#define MULT_MAT_POINT(out,H,p)\
113
(out)[0] = (H)[0]*(p)[0] + (H)[4]*(p)[1] + (H)[8]*(p)[2] + (H)[12];\
114
(out)[1] = (H)[1]*(p)[0] + (H)[5]*(p)[1] + (H)[9]*(p)[2] + (H)[13];\
115
(out)[2] = (H)[2]*(p)[0] + (H)[6]*(p)[1] + (H)[10]*(p)[2] + (H)[14];
179
voidMatrix2D_Set_Size(
tMatrix2D*mat,
introws,
intcols);
184
intMatrix2D_Size(
const
tMatrix2D*mat,
intdim);
189
intMatrix2D_Get_ncols(
const
tMatrix2D*var);
194
intMatrix2D_Get_nrows(
const
tMatrix2D*var);
199
doubleMatrix2D_Get_ij(
const
tMatrix2D*var,
inti,
intj);
206
voidMatrix2D_Set_ij(
const
tMatrix2D*var,
inti,
intj,
doublevalue);
213
double* Matrix2D_Get_col(
const
tMatrix2D*var,
intcol);
220
voidDebug_Array(
const
double*array,
intarraysize);
227
voidMatrix2D_Save(QDataStream *st,
tMatrix2D*emx);
230
voidMatrix2D_Save(QTextStream *st,
tMatrix2D*emx,
boolcsv=
false);
233
voidMatrix2D_Load(QDataStream *st,
tMatrix2D**emx);
249
tJoints(
const
double*joints,
intndofs = 0);
254
tJoints(
const
float*joints,
intndofs = 0);
281
#ifdef ROBODK_API_FLOATS
284
const
float*
Values()
const;
288
const
double*
Values()
const;
293
doubleCompare(
const
tJoints&other)
const;
319
void
SetValues(
const
double*joints,
intndofs = -1);
324
void
SetValues(
const
float*joints,
intndofs = -1);
330QString
ToString(
constQString &separator=
", ",
intprecision = 3)
const;
361
class
Mat:
publicQMatrix4x4 {
372
Mat(
constQMatrix4x4 &matrix);
398
Mat(
doublenx,
doubleox,
doubleax,
doubletx,
doubleny,
doubleoy,
doubleay,
doublety,
doublenz,
doubleoz,
doubleaz,
doubletz);
407
Mat(
const
doublevalues[16]);
417
Mat(
const
floatvalues[16]);
432
Mat(
doublex,
doubley,
doublez);
440
void
setVX(
doublex,
doubley,
doublez);
443
void
setVY(
doublex,
doubley,
doublez);
446
void
setVZ(
doublex,
doubley,
doublez);
449
void
setPos(
doublex,
doubley,
doublez);
452
void
setVX(
doublexyz[3]);
455
void
setVY(
doublexyz[3]);
458
void
setVZ(
doublexyz[3]);
461
void
setPos(
doublexyz[3]);
467
void
VX(tXYZ xyz)
const;
470
void
VY(tXYZ xyz)
const;
473
void
VZ(tXYZ xyz)
const;
476
void
Pos(tXYZ xyz)
const;
482
void
Set(
intr,
intc,
doublevalue);
488
double
Get(
intr,
intc)
const;
514QString
ToString(
constQString &separator=
", ",
intprecision = 3,
boolxyzrpw_only =
false)
const;
535
static
Mat
XYZRPW_2_Mat(
doublex,
doubley,
doublez,
doubler,
doublep,
doublew);
544
#ifdef ROBODK_API_FLOATS
546
const
float*
Values()
const;
549
const
double*
Values()
const;
553
void
Values(
doublevalues[16])
const;
556
void
Values(
floatvalues[16])
const;
574
static
Mat
transl(
doublex,
doubley,
doublez);
627
Mattransl(
doublex,
doubley,
doublez);
639
inlineQDebug operator<<(QDebug dbg,
const
Mat&m){
returndbg.noquote() << m.
ToString(); }
640
inlineQDebug operator<<(QDebug dbg,
const
tJoints&jnts){
returndbg.noquote() << jnts.
ToString(); }
642
inlineQDebug operator<<(QDebug dbg,
const
Mat*m){
returndbg.noquote() << (m ==
nullptr?
"Mat(null)": m->
ToString()); }
643
inlineQDebug operator<<(QDebug dbg,
const
tJoints*jnts){
returndbg.noquote() << (jnts ==
nullptr?
"tJoints(null)": jnts->
ToString()); }
The Item class represents an item in RoboDK station. An item can be a robot, a frame,...
This class is the iterface to the RoboDK API. With the RoboDK API you can automate certain tasks and ...
The Mat class represents a 4x4 pose matrix. The main purpose of this object is to represent a pose in...
QString ToString(const QString &separator=", ", int precision=3, bool xyzrpw_only=false) const
Retrieve a string representation of the pose.
Mat()
Create the identity matrix.
void VX(tXYZ xyz) const
Get the X vector (N vector)
void setVX(double x, double y, double z)
Set the X vector values (N vector)
void FromXYZRPW(tXYZWPR xyzwpr)
Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) The result is the same ...
void VY(tXYZ xyz) const
Get the Y vector (O vector)
static Mat transl(double x, double y, double z)
Return a translation matrix.
void ToXYZRPW(tXYZWPR xyzwpr) const
Calculates the equivalent position and euler angles ([x,y,z,r,p,w] vector) of the given pose Note: tr...
void setVZ(double x, double y, double z)
Set the Z vector values (A vector)
double Get(int r, int c) const
Get a matrix value.
bool Valid() const
Check if the matrix is valid.
void setVY(double x, double y, double z)
Set the Y vector values (O vector)
static Mat roty(double ry)
Return a Y-axis rotation matrix
const double * Values() const
Get a pointer to the 16-digit array (doubles or floats if ROBODK_API_FLOATS is defined).
void setValues(double pose[16])
Set the pose values.
void Pos(tXYZ xyz) const
Get the position (T position), in mm.
bool MakeHomogeneous()
Forces 4x4 matrix to be homogeneous (vx,vy,vz must be unitary vectors and respect: vx x vy = vz)....
static Mat rotz(double rz)
Return a Z-axis rotation matrix.
const double * ValuesD() const
Get a pointer to the 16-digit double array.
double _ddata16[16]
Copy of the data as a double array.
bool FromString(const QString &str)
Set the matrix given a XYZRPW string array (6-values)
double _valid
Flags if a matrix is not valid.
Mat inv() const
Invert the pose (homogeneous matrix assumed)
void setPos(double x, double y, double z)
Set the position (T position) in mm.
void VZ(tXYZ xyz) const
Get the Z vector (A vector)
bool isHomogeneous() const
Returns true if the matrix is homogeneous, otherwise it returns false.
const float * ValuesF() const
Get a pointer to the 16-digit array as an array of floats.
void Set(int r, int c, double value)
Set a matrix value.
static Mat rotx(double rx)
Return the X-axis rotation matrix.
static Mat XYZRPW_2_Mat(double x, double y, double z, double r, double p, double w)
Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) The result is the same ...
The tJoints class represents a joint position of a robot (robot axes).
int GetValues(double *joints)
GetValues.
tJoints(int ndofs=0)
tJoints
QString ToString(const QString &separator=", ", int precision=3) const
Retrieve a string representation of the joint values.
int Length() const
Number of joint axes of the robot (or degrees of freedom)
const double * Values() const
Joint values.
bool Valid()
Check if the joints are valid. For example, when we request the Inverse kinematics and there is no so...
int _nDOFs
number of degrees of freedom
const double * ValuesD() const
Joint values.
double _Values[RDK_SIZE_JOINTS_MAX]
joint values (doubles, used to store the joint values)
bool FromString(const QString &str)
Set the joint values given a comma-separated string. Tabs and spaces are also allowed.
void setLength(int new_length)
Set the length of the array (only shrinking the array is allowed)
float _ValuesF[RDK_SIZE_JOINTS_MAX]
joint values (floats, used to return a copy as a float pointer)
const float * ValuesF() const
Joint values.
无效的setvalue (const双*关节,int ndofs = 1)
Set the joint values in deg or mm. You can also important provide the number of degrees of freedom (6...
The Color struct represents an RGBA color (each color component should be in the range [0-1])
float a
Alpha value (0 = transparent; 1 = opaque)
The tMatrix2D struct represents a variable size 2d Matrix. Use the Matrix2D_... functions to oeprate ...
double * data
Pointer to the data.
int * size
Pointer to the size array.
int numDimensions
Number of dimensions (usually 2)