1#include "robodktypes.h"
9 _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
10 for (
int i=0; i<
_nDOFs; i++){
21 int ndofs_ok = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
22 double jnts[RDK_SIZE_JOINTS_MAX];
23 for (
int i=0; i<ndofs_ok; i++){
29 if (Matrix2D_Size(mat2d, 2) >= column){
31 qDebug()<<
"Warning: tMatrix2D column outside range when creating joints";
34 ndofs = Matrix2D_Size(mat2d, 1);
36 _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
38 double *ptr = Matrix2D_Get_col(mat2d, column);
50 for (
int i=0; i<RDK_SIZE_JOINTS_MAX; i++){
55#ifdef ROBODK_API_FLOATS
65double tJoints::Compare(
const tJoints &other)
const {
66 double sum_diff = 0.0;
80 _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
82 for (
int i=0; i<
_nDOFs; i++){
89 _nDOFs = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
91 for (
int i=0; i<
_nDOFs; i++){
96 for (
int i=0; i<
_nDOFs; i++){
106 values.append(QString::number(
_Values[0],
'f',precision));
107 for (
int i=1; i<
_nDOFs; i++){
108 values.append(separator);
109 values.append(QString::number(
_Values[i],
'f',precision));
114 QStringList jnts_list = QString(str).replace(
";",
",").replace(
"\t",
",").split(
",", QString::SkipEmptyParts);
115 _nDOFs = qMin(jnts_list.length(), RDK_SIZE_JOINTS_MAX);
116 for (
int i=0; i<
_nDOFs; i++){
117 QString stri(jnts_list.at(i));
118 _Values[i] = stri.trimmed().toDouble();
127 if (new_length >= 0 && new_length <
_nDOFs){
142Mat transl(
double x,
double y,
double z){
167Mat::Mat(
const QMatrix4x4 &matrix) : QMatrix4x4(matrix) {
176Mat::Mat(
double nx,
double ox,
double ax,
double tx,
double ny,
double oy,
double ay,
double ty,
double nz,
double oz,
double az,
double tz) :
177 QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0,0,0,1)
182 QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15])
187 QMatrix4x4(v[0], v[4], v[8], v[12], v[1], v[5], v[9], v[13], v[2], v[6], v[10], v[14], v[3], v[7], v[11], v[15])
193 QMatrix4x4(1.0, 0.0, 0.0, x, 0.0, 1.0, 0.0, y, 0.0, 0.0, 1.0, z, 0.0,0.0,0.0,1.0)
291 QVector4D rw(this->row(i));
306 return this->inverted();
311 const bool debug_info =
false;
313 const double tol = 1e-7;
317 if (fabs((
double) DOT(vx,vy)) > tol){
319 qDebug() <<
"Vector X and Y are not perpendicular!";
322 }
else if (fabs((
double) DOT(vx,vz)) > tol){
324 qDebug() <<
"Vector X and Z are not perpendicular!";
327 }
else if (fabs((
double) DOT(vy,vz)) > tol){
329 qDebug() <<
"Vector Y and Z are not perpendicular!";
332 }
else if (fabs((
double) (NORM(vx)-1.0)) > tol){
334 qDebug() <<
"Vector X is not unitary! " << NORM(vx);
337 }
else if (fabs((
double) (NORM(vy)-1.0)) > tol){
339 qDebug() <<
"Vector Y is not unitary! " << NORM(vy);
342 }
else if (fabs((
double) (NORM(vz)-1.0)) > tol){
344 qDebug() <<
"Vector Z is not unitary! " << NORM(vz);
373 return !is_homogeneous;
385 if (
Get(2,0) > (1.0 - 1e-6)){
388 w = atan2(-
Get(1,2),
Get(1,1));
389 }
else if (
Get(2,0) < -1.0 + 1e-6){
392 w = atan2(
Get(1,2),
Get(1,1));
394 p = atan2(-
Get(2, 0), sqrt(
Get(0, 0) *
Get(0, 0) +
Get(1, 0) *
Get(1, 0)));
395 w = atan2(
Get(1, 0),
Get(0, 0));
396 r = atan2(
Get(2, 1),
Get(2, 2));
401 xyzwpr[3] = r*180.0/M_PI;
402 xyzwpr[4] = p*180.0/M_PI;
403 xyzwpr[5] = w*180.0/M_PI;
405QString
Mat::ToString(
const QString &separator,
int precision,
bool xyzwpr_only)
const {
407 return "Mat(Invalid)";
411 str.append(
"Warning!! Pose is not homogeneous! Use Mat::MakeHomogeneous() to make this matrix homogeneous\n");
413 str.append(
"Mat(XYZRPW_2_Mat(");
417 str.append(QString::number(xyzwpr[0],
'f',precision));
418 for (
int i=1; i<6; i++){
419 str.append(separator);
420 str.append(QString::number(xyzwpr[i],
'f',precision));
428 for (
int i=0; i<4; i++){
430 for (
int j=0; j<4; j++){
431 str.append(QString::number(row(i)[j],
'f', precision));
433 str.append(separator);
441 QStringList values_list = QString(pose_str).replace(
";",
",").replace(
"\t",
",").split(
",", QString::SkipEmptyParts);
442 int nvalues = qMin(values_list.length(), 6);
444 for (
int i=0; i<6; i++){
451 for (
int i=0; i<nvalues; i++){
452 QString stri(values_list.at(i));
453 xyzwpr[i] = stri.trimmed().toDouble();
460 double a = r * M_PI / 180.0;
461 double b = p * M_PI / 180.0;
462 double c = w * M_PI / 180.0;
469 return Mat(cb * cc, cc * sa * sb - ca * sc, sa * sc + ca * cc * sb, x, cb * sc, ca * cc + sa * sb * sc, ca * sb * sc - cc * sa, y, -sb, cb * sa, ca * cb, z);
472 return XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
476 Mat newmat =
Mat::XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
477 for (
int i=0; i<4; i++){
478 for (
int j=0; j<4; j++){
479 this->
Set(i,j, newmat.
Get(i,j));
485 double* _ddata16_non_const = (
double*)
_ddata16;
486 for(
int i=0; i<16; ++i){
487 _ddata16_non_const[i] = constData()[i];
495#ifdef ROBODK_API_FLOATS
509 for(
int i=0; i<16; ++i){
510 data[i] = constData()[i];
514 for(
int i=0; i<16; ++i){
515 data[i] = constData()[i];
532 return Mat(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0);
538 return Mat(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0);
544 return Mat(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0);
562void emxInit_real_T(
tMatrix2D **pEmxArray,
int numDimensions){
566 emxArray = *pEmxArray;
567 emxArray->
data = (
double *)NULL;
569 emxArray->
size = (
int *)malloc((
unsigned int)(
sizeof(int) * numDimensions));
571 emxArray->canFreeData =
true;
572 for (i = 0; i < numDimensions; i++) {
573 emxArray->
size[i] = 0;
579 emxInit_real_T((
tMatrix2D**)(&matrix), 2);
584void emxFree_real_T(
tMatrix2D **pEmxArray){
586 if (((*pEmxArray)->data != (
double *)NULL) && (*pEmxArray)->canFreeData) {
587 free((
void *)(*pEmxArray)->data);
589 free((
void *)(*pEmxArray)->size);
590 free((
void *)*pEmxArray);
601void emxEnsureCapacity(
tMatrix2D *emxArray,
int oldNumel,
unsigned int elementSize){
610 newNumel *= emxArray->
size[i];
617 while (i < newNumel) {
618 if (i > 1073741823) {
624 newData = (
double*) calloc((
unsigned int)i, elementSize);
625 if (emxArray->
data != NULL) {
626 memcpy(newData, emxArray->
data, elementSize * oldNumel);
627 if (emxArray->canFreeData) {
628 free(emxArray->
data);
631 emxArray->
data = newData;
633 emxArray->canFreeData =
true;
637void Matrix2D_Set_Size(
tMatrix2D *mat,
int rows,
int cols) {
640 old_numel = mat->
size[0] * mat->
size[1];
644 emxEnsureCapacity(mat, old_numel,
sizeof(
double));
650int Matrix2D_Size(
const tMatrix2D *var,
int dim) {
652 return var->
size[dim - 1];
658int Matrix2D_Get_ncols(
const tMatrix2D *var) {
659 return Matrix2D_Size(var, 2);
661int Matrix2D_Get_nrows(
const tMatrix2D *var) {
662 return Matrix2D_Size(var, 1);
664double Matrix2D_Get_ij(
const tMatrix2D *var,
int i,
int j) {
665 return var->
data[var->
size[0] * j + i];
667void Matrix2D_Set_ij(
const tMatrix2D *var,
int i,
int j,
double value) {
668 var->
data[var->
size[0] * j + i] = value;
671double *Matrix2D_Get_col(
const tMatrix2D *var,
int col) {
672 return (var->
data + var->
size[0] * col);
676 Matrix2D_Set_Size(to, 0,0);
679 int sz1 = Matrix2D_Size(from,1);
680 int sz2 = Matrix2D_Size(from,2);
681 Matrix2D_Set_Size(to, sz1, sz2);
683 for (
int i=0; i<numel; i++){
691void Matrix2D_Add(
tMatrix2D *var,
const double *array,
int numel){
693 int size1 = var->
size[0];
694 int size2 = var->
size[1];
695 oldnumel = size1*size2;
696 var->
size[1] = size2 + 1;
697 emxEnsureCapacity(var, oldnumel, (
int)
sizeof(
double));
698 numel = qMin(numel, size1);
699 for (
int i=0; i<numel; i++){
700 var->
data[size1*size2 + i] = array[i];
706 int size1 = var->
size[0];
707 int size2 = var->
size[1];
708 int size1_ap = varadd->
size[0];
709 int size2_ap = varadd->
size[1];
710 int numel = size1_ap*size2_ap;
711 if (size1 != size1_ap){
714 oldnumel = size1*size2;
715 var->
size[1] = size2 + size2_ap;
716 emxEnsureCapacity(var, oldnumel, (
int)
sizeof(
double));
717 for (
int i=0; i<numel; i++){
718 var->
data[size1*size2 + i] = varadd->
data[i];
722void Debug_Array(
const double *array,
int arraysize) {
725 for (i = 0; i < arraysize; i++) {
726 strout.append(QString::number(array[i],
'f', 3));
727 if (i < arraysize - 1) {
728 strout.append(
" , ");
731 qDebug().noquote() << strout;
734void Debug_Matrix2D(
const tMatrix2D *emx) {
739 size1 = Matrix2D_Get_nrows(emx);
740 size2 = Matrix2D_Get_ncols(emx);
741 qDebug().noquote() <<
"Matrix size = " << size1 <<
" x " << size2;
742 if (size1*size2 == 0){
return; }
743 for (j = 0; j<size2; j++) {
744 column = Matrix2D_Get_col(emx, j);
745 Debug_Array(column, size1);
749void Matrix2D_Save(QDataStream *st,
tMatrix2D *emx){
754 qint32 sizei = emx->
size[i];
755 size_values = size_values * sizei;
758 for (i=0; i<size_values; i++){
762void Matrix2D_Save(QTextStream *st,
tMatrix2D *emx,
bool csv){
767 size1 = Matrix2D_Get_nrows(emx);
768 size2 = Matrix2D_Get_ncols(emx);
770 if (size1*size2 == 0){
return; }
772 for (j = 0; j<size2; j++) {
773 column = Matrix2D_Get_col(emx, j);
774 for (
int i = 0; i < size1; i++) {
775 *st << QString::number(column[i],
'f', 8) <<
", ";
780 for (j = 0; j<size2; j++) {
781 column = Matrix2D_Get_col(emx, j);
783 for (
int i = 0; i < size1; i++) {
784 *st << QString::number(column[i],
'f', 8) <<
" ";
791void Matrix2D_Load(QDataStream *st,
tMatrix2D **emx){
793 qDebug() <<
"No data to read";
796 if (*emx !=
nullptr){
797 Matrix2D_Delete(emx);
803 qDebug() <<
"Loading matrix of dimensions: " << ndim;
804 emxInit_real_T(emx, ndim);
806 for (i=0; i<ndim; i++){
808 size_values = size_values * sizei;
809 (*emx)->size[i] = sizei;
812 emxEnsureCapacity(*emx, 0,
sizeof(
double));
814 for (i=0; i<size_values; i++){
816 (*emx)->data[i] = value;
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.
void SetValues(const double *joints, int ndofs=-1)
Set the joint values in deg or mm. You can also important provide the number of degrees of freedom (6...
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 allocatedSize
Allocated size.
int numDimensions
Number of dimensions (usually 2)