RoboDK Plug-In Interface
robodktypes.cpp
1 #include "robodktypes.h"
2 #include
3 #include
4
5
6
7 //----------------------------------- tJoints class ------------------------
8 tJoints::tJoints(intndofs){
9 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
10 for(inti=0; i<_nDOFs; i++){
11 _Values[i] = 0.0;
12}
13}
14 tJoints::tJoints(const tJoints©){
15 SetValues(copy._Values, copy._nDOFs);
16}
17 tJoints::tJoints(const double*joints,intndofs){
18 SetValues(joints, ndofs);
19}
20 tJoints::tJoints(const float*joints,intndofs){
21 intndofs_ok = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
22 doublejnts[RDK_SIZE_JOINTS_MAX];
23 for(inti=0; i
24jnts[i] = joints[i];
25}
26 SetValues(jnts, ndofs_ok);
27}
28 tJoints::tJoints(const tMatrix2D*mat2d,intcolumn,intndofs){
29 if(Matrix2D_Size(mat2d, 2) >= column){
30 _nDOFs= 0;
31qDebug()<<"Warning: tMatrix2D column outside range when creating joints";
32}
33 if(ndofs < 0){
34ndofs = Matrix2D_Size(mat2d, 1);
35}
36 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
37
38 double*ptr = Matrix2D_Get_col(mat2d, column);
39 SetValues(ptr,_nDOFs);
40}
41 tJoints::tJoints(constQString &str){
42 _nDOFs= 0;
43 FromString(str);
44}
45
46 const double*tJoints::ValuesD()const{
47 return _Values;
48}
49 const float*tJoints::ValuesF()const{
50 for(inti=0; i
51((float*)_ValuesF)[i] =_Values[i];
52}
53 return _ValuesF;
54}
55 # ifdef ROBODK_API_FLOATS
56 const float*tJoints::Values()const{
57 return ValuesF();
58}
59 #else
60 const double*tJoints::Values()const{
61 return _Values;
62}
63 #endif
64
65 doubletJoints::Compare(const tJoints&other)const{
66 doublesum_diff = 0.0;
67 for(inti=0;i_nDOFs, other.Length()); i++){
68sum_diff += qAbs(_Values[i] - other.Values()[i]);
69}
70 returnsum_diff;
71}
72
73 double*tJoints::Data（）{
74 return _Values;
75}
76
77
78 void tJoints::SetValues(const double*values,intndofs){
79 if(ndofs >= 0){
80 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
81}
82 for(inti=0; i<_nDOFs; i++){
83 _Values[i] = values[i];
84}
85}
86
87 void tJoints::SetValues(const float*values,intndofs){
88 if(ndofs >= 0){
89 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
90}
91 for(inti=0; i<_nDOFs; i++){
92 _Values[i] = values[i];
93}
94}
95 int tJoints::GetValues(double*values){
96 for(inti=0; i<_nDOFs; i++){
97values[i] =_Values[i];
98}
99 return _nDOFs;
100}
101QStringtJoints::ToString(constQString &separator,intprecision)const{
102QString values;
103 if(_nDOFs<= 0){
104 returnvalues;
105}
106values.append(QString::number(_Values[0],'f',precision));
107 for(inti=1; i<_nDOFs; i++){
108values.append(separator);
109values.append(QString::number(_Values[i],'f',precision));
110}
111 returnvalues;
112}
113 bool tJoints::FromString(constQString &str){
114QStringList jnts_list = QString(str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts);
115 _nDOFs= qMin(jnts_list.length(), RDK_SIZE_JOINTS_MAX);
116 for(inti=0; i<_nDOFs; i++){
117QString stri(jnts_list.at(i));
118 _Values[i] = stri.trimmed().toDouble();
119}
120 return true;
121}
122
123 int tJoints::Length()const{
124 return _nDOFs;
125}
126 void tJoints: setLength(intnew_length) {
127 if(new_length >= 0 && new_length <_nDOFs){
128 _nDOFs= new_length;
129}
130}
131 bool tJoints::Valid（）{
132 return _nDOFs> 0;
133}
134 //---------------------------------------------------------------------
135
136
137
138
139
140 //----------------------------------- Mat class ------------------------
141
142 Mattransl(doublex,doubley,doublez){
143 return Mat::transl(x,y,z);
144}
145
146 Matrotx(doublerx){
147 return Mat::rotx(rx);
148}
149
150 Matroty(doublery){
151 return Mat::roty(ry);
152}
153
154 Matrotz(doublerz){
155 return Mat::rotz(rz);
156}
157
158 Mat::Mat() : QMatrix4x4() {
159 _valid=true;
160setToIdentity();
161}
162 Mat::Mat(boolvalid) : QMatrix4x4() {
163 _valid= valid;
164setToIdentity();
165}
166
167 Mat::Mat(constQMatrix4x4 &matrix) : QMatrix4x4(matrix) {
168 // just copy
169 _valid=true;
170}
171 Mat::Mat(const Mat&matrix) : QMatrix4x4(matrix) {
172 // just copy
173 _valid= matrix._valid;
174}
175
176 Mat::Mat(doublenx,doubleox,doubleax,doubletx,doubleny,doubleoy,doubleay,doublety,doublenz,doubleoz,doubleaz,doubletz) :
177QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0,0,0,1)
178{
179 _valid=true;
180}
181 Mat::Mat(const doublev[16]) :
182QMatrix4x4(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])
183{
184 _valid=true;
185}
186 Mat::Mat(const floatv[16]) :
187QMatrix4x4(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])
188{
189 _valid=true;
190}
191
192 Mat::Mat(doublex,doubley,doublez):
193QMatrix4x4(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)
194{
195 _valid=true;
196}
197
198
199Mat::~Mat(){
200
201}
202
203 void Mat::VX(tXYZ xyz)const{
204xyz[0] =Get(0, 0);
205xyz[1] =Get(1, 0);
206xyz[2] =Get(2, 0);
207}
208 void Mat::VY(tXYZ xyz)const{
209xyz[0] =Get(0, 1);
210xyz[1] =Get(1, 1);
211xyz[2] =Get(2, 1);
212}
213 void Mat::VZ(tXYZ xyz)const{
214xyz[0] =Get(0, 2);
215xyz[1] =Get(1, 2);
216xyz[2] =Get(2, 2);
217}
218 void Mat::Pos(tXYZ xyz)const{
219xyz[0] =Get(0, 3);
220xyz[1] =Get(1, 3);
221xyz[2] =Get(2, 3);
222}
223
224 void Mat::setVX(doublex,doubley,doublez){
225 Set(0,0, x);
226 Set(1,0, y);
227 Set(2,0, z);
228}
229 void Mat::setVY(doublex,doubley,doublez){
230 Set(0,1, x);
231 Set(1,1, y);
232 Set(2,1, z);
233}
234 void Mat::setVZ(doublex,doubley,doublez){
235 Set(0,2, x);
236 Set(1,2, y);
237 Set(2,2, z);
238}
239 void Mat::setPos(doublex,doubley,doublez){
240 Set(0,3, x);
241 Set(1,3, y);
242 Set(2,3, z);
243}
244
245 void Mat::setVX(doublexyz[3]){
246 Set(0,0, xyz[0]);
247 Set(1,0, xyz[1]);
248 Set(2,0, xyz[2]);
249}
250 void Mat::setVY(doublexyz[3]){
251 Set(0,1, xyz[0]);
252 Set(1,1, xyz[1]);
253 Set(2,1, xyz[2]);
254}
255 void Mat::setVZ(doublexyz[3]){
256 Set(0,2, xyz[0]);
257 Set(1,2, xyz[1]);
258 Set(2,2, xyz[2]);
259}
260 void Mat::setPos(doublexyz[3]){
261 Set(0,3, xyz[0]);
262 Set(1,3, xyz[1]);
263 Set(2,3, xyz[2]);
264}
265
266
267 void Mat::setValues(doublepose[16]){
268 Set(0,0, pose[0]);
269 Set(1,0, pose[1]);
270 Set(2,0, pose[2]);
271 Set(3,0, pose[3]);
272
273 Set(0,1, pose[4]);
274 Set(1,1, pose[5]);
275 Set(2,1, pose[6]);
276 Set(3,1, pose[7]);
277
278 Set(0,2, pose[8]);
279 Set(1,2, pose[9]);
280 Set(2,2, pose[10]);
281 Set(3,2, pose[11]);
282
283 Set(0,3, pose[12]);
284 Set(1,3, pose[13]);
285 Set(2,3, pose[14]);
286 Set(3,3, pose[15]);
287}
288
289
290 void Mat::Set(inti,intj,doublevalue){
291QVector4D rw(this->row(i));
292rw[j] = value;
293setRow(i, rw);
294 // the following should not crash!!
295 //float **dt_ok = (float**) data();
296 //dt_ok[i][j] = value;
297}
298
299 double Mat::Get(inti,intj)const{
300 returnrow(i)[j];
301 // the following hsould be allowed!!
302 //return ((const float**)data())[i][j];
303}
304
305 Mat Mat::inv()const{
306 returnthis->inverted();
307}
308
309
310 bool Mat::isHomogeneous()const{
311 const booldebug_info =false;
312tXYZ vx, vy, vz;
313 const doubletol = 1e-7;
314 VX(vx);
315 VY(vy);
316 VZ(vz);
317 if(fabs((double) DOT(vx,vy)) > tol){
318 if(debug_info){
319qDebug() <<"Vector X and Y are not perpendicular!";
320}
321 return false;
322}else if(fabs((double) DOT(vx,vz)) > tol){
323 if(debug_info){
324qDebug() <<"Vector X and Z are not perpendicular!";
325}
326 return false;
327}else if(fabs((double) DOT(vy,vz)) > tol){
328 if(debug_info){
329qDebug() <<"Vector Y and Z are not perpendicular!";
330}
331 return false;
332}else if(fabs((double) (NORM(vx)-1.0)) > tol){
333 if(debug_info){
334qDebug() <<"Vector X is not unitary! "<< NORM(vx);
335}
336 return false;
337}else if(fabs((double) (NORM(vy)-1.0)) > tol){
338 if(debug_info){
339qDebug() <<"Vector Y is not unitary! "<< NORM(vy);
340}
341 return false;
342}else if(fabs((double) (NORM(vz)-1.0)) > tol){
343 if(debug_info){
344qDebug() <<"Vector Z is not unitary! "<< NORM(vz);
345}
346 return false;
347}
348 return true;
349}
350
351 bool Mat::MakeHomogeneous（）{
352tXYZ vx, vy, vz;
353 VX(vx);
354 VY(vy);
355 VZ(vz);
356 boolis_homogeneous =isHomogeneous();
357 //if (is_homogeneous){
358 // return false;
359 //}
360
361NORMALIZE(vx);
362CROSS(vz, vx, vy);
363NORMALIZE(vz);
364CROSS(vy, vz, vx);
365NORMALIZE(vy);
366 setVX(vx);
367 setVY(vy);
368 setVZ(vz);
369 Set(3,0, 0.0);
370 Set(3,1, 0.0);
371 Set(3,2, 0.0);
372 Set(3,3, 1.0);
373 return!is_homogeneous;
374}
375
376
377
378 //----------------------------------------------------
379
380 void Mat::ToXYZRPW(tXYZWPR xyzwpr)const{
381 doublex =Get(0,3);
382 doubley =Get(1,3);
383 doublez =Get(2,3);
384 doublew, p, r;
385 if(Get(2,0) > (1.0 - 1e-6)){
386p = -M_PI * 0.5;
387r = 0;
388w = atan2(-Get(1,2),Get(1,1));
389}else if(Get(2,0) < -1.0 + 1e-6){
390p = 0.5*M_PI;
391r = 0;
392w = atan2(Get(1,2),Get(1,1));
393}else{
394p = atan2(-Get(2, 0), sqrt(Get(0, 0) *Get(0, 0) +Get(1, 0) *Get(1, 0)));
395w = atan2(Get(1, 0),Get(0, 0));
396r = atan2(Get(2, 1),Get(2, 2));
397}
398xyzwpr[0] = x;
399xyzwpr[1] = y;
400xyzwpr[2] = z;
401xyzwpr[3] = r*180.0/M_PI;
402xyzwpr[4] = p*180.0/M_PI;
403xyzwpr[5] = w*180.0/M_PI;
404}
405QStringMat::ToString(constQString &separator,intprecision,boolxyzwpr_only)const{
406 if(!Valid()){
407 return "Mat(Invalid)";
408}
409QString str;
410 if(!isHomogeneous()){
411str.append("Warning!! Pose is not homogeneous! Use Mat::MakeHomogeneous() to make this matrix homogeneous\n");
412}
413str.append("Mat(XYZRPW_2_Mat(");
414
415tXYZWPR xyzwpr;
416 ToXYZRPW(xyzwpr);
417str.append(QString::number(xyzwpr[0],'f',precision));
418 for(inti=1; i<6; i++){
419str.append(separator);
420str.append(QString::number(xyzwpr[i],'f',precision));
421}
422str.append("))");
423
424 if(xyzwpr_only){
425 returnstr;
426}
427str.append("\n");
428 for(inti=0; i<4; i++){
429str.append("[");
430 for(intj=0; j<4; j++){
431str.append(QString::number(row(i)[j],'f', precision));
432 if(j < 3){
433str.append(separator);
434}
435}
436str.append("];\n");
437}
438 returnstr;
439}
440 bool Mat::FromString(constQString &pose_str){
441QStringList values_list = QString(pose_str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts);
442 intnvalues = qMin(values_list.length(), 6);
443tXYZWPR xyzwpr;
444 for(inti=0; i<6; i++){
445xyzwpr[i] = 0.0;
446}
447 if(nvalues < 6){
448 FromXYZRPW(xyzwpr);
449 return false;
450}
451 for(inti=0; i
452QString stri(values_list.at(i));
453xyzwpr[i] = stri.trimmed().toDouble();
454}
455 FromXYZRPW(xyzwpr);
456 return true;
457}
458
459 Mat Mat::XYZRPW_2_Mat(doublex,doubley,doublez,doubler,doublep,doublew){
460 doublea = r * M_PI / 180.0;
461 doubleb = p * M_PI / 180.0;
462 doublec = w * M_PI / 180.0;
463 doubleca = cos(a);
464 doublesa = sin(a);
465 doublecb = cos(b);
466 doublesb = sin(b);
467 doublecc = cos(c);
468 doublesc = sin(c);
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);
470}
471 Mat Mat::XYZRPW_2_Mat(tXYZWPR xyzwpr){
472 return XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
473}
474
475 void Mat::FromXYZRPW(tXYZWPR xyzwpr){
476 Matnewmat =Mat::XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
477 for(inti=0; i<4; i++){
478 for(intj=0; j<4; j++){
479this->Set(i, j newmat。Get(i,j));
480}
481}
482}
483
484 const double*Mat::ValuesD()const{
485 double* _ddata16_non_const = (double*)_ddata16;
486 for(inti=0; i<16; ++i){
487_ddata16_non_const[i] = constData()[i];
488}
489 return _ddata16;
490}
491 const float*Mat::ValuesF()const{
492 returnconstData();
493}
494
495 # ifdef ROBODK_API_FLOATS
496 const float*Mat::Values()const{
497 returnconstData();
498}
499 #else
500 const double*Mat::Values()const{
501 return ValuesD();
502}
503
504 #endif
505
506
507
508 void Mat::Values(doubledata[16])const{
509 for(inti=0; i<16; ++i){
510data[i] = constData()[i];
511}
512}
513 void Mat::Values(floatdata[16])const{
514 for(inti=0; i<16; ++i){
515data[i] = constData()[i];
516}
517}
518 bool Mat::Valid()const{
519 return _valid;
520}
521
522 Mat Mat::transl(doublex,doubley,doublez){
523 Matmat;
524mat.setToIdentity();
525mat.setPos(x, y, z);
526 returnmat;
527}
528
529 Mat Mat::rotx(doublerx){
530 doublecx = cos(rx);
531 doublesx = sin(rx);
532 return Mat(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0);
533}
534
535 Mat Mat::roty(doublery){
536 doublecy = cos(ry);
537 doublesy =罪(办法);
538 return Mat(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0);
539}
540
541 Mat Mat::rotz(doublerz){
542 doublecz = cos(rz);
543 doublesz = sin(rz);
544 return Mat(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0);
545}
546
547
548
549
550
551
552
553 //---------------------------------------------------------------------------------------------------
554 //---------------------------------------------------------------------------------------------------
555 //---------------------------------------------------------------------------------------------------
556 //---------------------------------------------------------------------------------------------------
557 //---------------------------------------------------------------------------------------------------
558 //---------------------------------------------------------------------------------------------------
560 // 2D matrix functions
562 voidemxInit_real_T(tMatrix2D**pEmxArray,intnumDimensions){
563 tMatrix2D*emxArray;
564 inti;
565*pEmxArray = (tMatrix2D*)malloc(运算符(tMatrix2D));
566emxArray = *pEmxArray;
567emxArray->data= (double*)NULL;
568emxArray->numDimensions= numDimensions;
569emxArray->size= (int*)malloc((unsigned int)(运算符(int) * numDimensions));
570emxArray->allocatedSize= 0;
571emxArray->canFreeData =true;
572 for(i = 0; i < numDimensions; i++) {
573emxArray->size[i] = 0;
574}
575}
577 tMatrix2D* Matrix2D_Create() {
578 tMatrix2D*matrix;
579emxInit_real_T((tMatrix2D**)(&matrix), 2);
580 returnmatrix;
581}
582
583
584 voidemxFree_real_T(tMatrix2D**pEmxArray){
585 if(*pEmxArray != (tMatrix2D*)NULL) {
586 if(((*pEmxArray)->data != (double*)NULL) && (*pEmxArray)->canFreeData) {
587free((void*)(*pEmxArray)->data);
588}
589free((void*)(*pEmxArray)->size);
590free((void*)*pEmxArray);
591*pEmxArray = (tMatrix2D*)NULL;
592}
593}
594
595 voidMatrix2D_Delete(tMatrix2D**mat) {
596emxFree_real_T((tMatrix2D**)(mat));
597}
598
599
600
601 voidemxEnsureCapacity(tMatrix2D*emxArray,intoldNumel,unsigned intelementSize){
602 intnewNumel;
603 inti;
604 double*newData;
605 if(oldNumel < 0) {
606oldNumel = 0;
607}
608newNumel = 1;
609 for(i = 0; i < emxArray->numDimensions; i++) {
610newNumel *= emxArray->size[i];
611}
612 if(newNumel > emxArray->allocatedSize) {
613i = emxArray->allocatedSize;
614 if(i < 16) {
615i = 16;
616}
617 while(i < newNumel) {
618 if(i > 1073741823) {
619i =(2147483647);//MAX_int32_T;
620}else{
621i <<= 1;
622}
623}
624newData = (double*) calloc((unsigned int)i, elementSize);
625 if(emxArray->data!= NULL) {
626memcpy(newData, emxArray->data, elementSize * oldNumel);
627 if(emxArray->canFreeData) {
628free(emxArray->data);
629}
630}
631emxArray->data= newData;
632emxArray->allocatedSize= i;
633emxArray->canFreeData =true;
634}
635}
636
637 voidMatrix2D_Set_Size(tMatrix2D*mat,introws,intcols) {
638 intold_numel;
639 intnumbel;
640old_numel = mat->size[0] * mat->size[1];
641mat->size[0] = rows;
642mat->size[1] = cols;
643numbel = rows*cols;
644emxEnsureCapacity(mat, old_numel,运算符(double));
645 /*for (i=0; i
646 mat->data[i] = 0.0;
647 }*/
648}
649
650 intMatrix2D_Size(const tMatrix2D*var,intdim) {// ONE BASED!!
651 if(var->numDimensions>= dim) {
652 returnvar->size[dim - 1];
653}
654 else{
655 return0;
656}
657}
658 intMatrix2D_Get_ncols(const tMatrix2D*var) {
659 returnMatrix2D_Size(var, 2);
660}
661 intMatrix2D_Get_nrows(const tMatrix2D*var) {
662 returnMatrix2D_Size(var, 1);
663}
664 doubleMatrix2D_Get_ij(const tMatrix2D*var,inti,intj) {// ZERO BASED!!
665 returnvar->data[var->size[0] * j + i];
666}
667 voidMatrix2D_Set_ij(const tMatrix2D*var,inti,intj,doublevalue) {// ZERO BASED!!
668var->data[var->size[0] * j + i] = value;
669}
670
671 double*Matrix2D_Get_col(const tMatrix2D*var,intcol) {// ZERO BASED!!
672 return(var->data+ var->size[0] * col);
673}
674 boolMatrix2D_Copy(const tMatrix2D*from,tMatrix2D*to){
675 if(from->numDimensions!= 2 || to->numDimensions!= 2){
676Matrix2D_Set_Size(to, 0,0);
677 return false;
678}
679 intsz1 = Matrix2D_Size(from,1);
680 intsz2 = Matrix2D_Size(from,2);
681Matrix2D_Set_Size(to, sz1, sz2);
682 intnumel = sz1*sz2;
683 for(inti=0; i
684to->data[i] = from->data[i];
685}
686 //to->canFreeData = from->canFreeData;
687 return true;
688}
689
690
691 voidMatrix2D_Add(tMatrix2D*var,const double*array,intnumel){
692 intoldnumel;
693 intsize1 = var->size[0];
694 intsize2 = var->size[1];
695oldnumel = size1*size2;
696var->size[1] = size2 + 1;
697emxEnsureCapacity(var, oldnumel, (int)运算符(double));
698numel = qMin(numel, size1);
699 for(inti=0; i
700var->data[size1*size2 + i] = array[i];
701}
702}
703
704 voidMatrix2D_Add(tMatrix2D*var,const tMatrix2D*varadd){
705 intoldnumel;
706 intsize1 = var->size[0];
707 intsize2 = var->size[1];
708 intsize1_ap = varadd->size[0];
709 intsize2_ap = varadd->size[1];
710 intnumel = size1_ap*size2_ap;
711 if(size1 != size1_ap){
712 return;
713}
714oldnumel = size1*size2;
715var->size[1] = size2 + size2_ap;
716emxEnsureCapacity(var, oldnumel, (int)运算符(double));
717 for(inti=0; i
718var->data[size1*size2 + i] = varadd->data[i];
719}
720}
721
722 voidDebug_Array(const double*array,intarraysize) {
723 inti;
724QString strout;
725 for(i = 0; i < arraysize; i++) {
726strout.append(QString::number(array[i],'f', 3));
727 if(i < arraysize - 1) {
728strout.append(" , ");
729}
730}
731qDebug().noquote() << strout;
732}
733
734 voidDebug_Matrix2D(const tMatrix2D*emx) {
735 intsize1;
736 intsize2;
737 intj;
738 double*column;
739size1 = Matrix2D_Get_nrows(emx);
740size2 = Matrix2D_Get_ncols(emx);
741qDebug().noquote() <<"Matrix size = "<< size1 <<" x "<< size2;
742 if(size1*size2 == 0){return; }
743 for(j = 0; j
744column = Matrix2D_Get_col(emx, j);
745Debug_Array(column, size1);
746}
747}
748
749 voidMatrix2D_Save(QDataStream *st,tMatrix2D*emx){
750 inti;
751*st << emx->numDimensions;
752 intsize_values = 1;
753 for(i=0; i< emx->numDimensions; i++){
754qint32 sizei = emx->size[i];
755size_values = size_values * sizei;
756*st << sizei;
757}
758 for(i=0; i
759*st << emx->data[i];
760}
761}
762 voidMatrix2D_Save(QTextStream *st,tMatrix2D*emx,boolcsv){
763 intsize1;
764 intsize2;
765 intj;
766 double*column;
767size1 = Matrix2D_Get_nrows(emx);
768size2 = Matrix2D_Get_ncols(emx);
769 //*st << "% Matrix size = " << size1 << " x " << size2;
770 if(size1*size2 == 0){return; }
771 if(csv){
772 for(j = 0; j
773column = Matrix2D_Get_col(emx, j);
774 for(inti = 0; i < size1; i++) {
775*st << QString::number(column[i],'f', 8) <<", ";
776}
777*st <<"\n";
778}
779}else{
780 for(j = 0; j
781column = Matrix2D_Get_col(emx, j);
782*st <<"[";
783 for(inti = 0; i < size1; i++) {
784*st << QString::number(column[i],'f', 8) <<" ";
785}
786*st <<"];\n";
787}
788}
789}
790
791 voidMatrix2D_Load(QDataStream *st,tMatrix2D**emx){
792 if(st->atEnd()){
793qDebug() <<"No data to read";
794 return;
795}
796 if(*emx !=nullptr){
797Matrix2D_Delete(emx);
798}
799 inti;
800qint32 ndim;
801qint32 sizei;
802*st >> ndim;
803qDebug() <<"Loading matrix of dimensions: "<< ndim;
804emxInit_real_T(emx, ndim);
805 intsize_values = 1;
806 for(i=0; i
807*st >> sizei;
808size_values = size_values * sizei;
809(*emx)->size[i] = sizei;
810}
811 //emxEnsureCapacity((emxArray__common *) *emx, 0, (int32_T)sizeof(real_T));
812emxEnsureCapacity(*emx, 0,运算符(double));
813 doublevalue;
814 for(i=0; i
815*st >> value;
816(*emx)->data[i] = value;
817}
818}
819
820
821 /*
822 void Debug_Mat(Mat pose, char show_full_pose) {
823 tMatrix4x4 pose_tr;
824 double xyzwpr[6];
825 int j;
826 if (show_full_pose > 0) {
827 POSE_TR(pose_tr, pose);
828 printf("Pose size = [4x4]\n");
829 //std::cout << "Pose size = [4x4]\n";
830 for (j = 0; j < 4; j++) {
831 Debug_Array(pose_tr + j * 4, 4);
832 printf("\n");
833 //std::cout << "\n";
834 }
835 }
836 else {
837 POSE_2_XYZWPR(xyzwpr, pose);
838 //std::cout << "XYZWPR = [ ";
839 printf("XYZWPR = [ ");
840 Debug_Array(xyzwpr, 6);
841 printf(" ]\n");
842 //std::cout << " ]\n";
843 }
844 }
845 */
846
Mat
The Mat class represents a 4x4 pose matrix. The main purpose of this object is to represent a pose in...
Definition: robodktypes.h:361
Mat::ToString
QString ToString(const QString &separator=", ", int precision=3, bool xyzrpw_only=false) const
Retrieve a string representation of the pose.
Definition: robodktypes.cpp:405
Mat::Mat
Mat()
Create the identity matrix.
Definition: robodktypes.cpp:158
Mat::VX
void VX(tXYZ xyz) const
Get the X vector (N vector)
Definition: robodktypes.cpp:203
Mat::setVX
void setVX(double x, double y, double z)
Set the X vector values (N vector)
Definition: robodktypes.cpp:224
Mat::FromXYZRPW
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 ...
Definition: robodktypes.cpp:475
Mat::VY
void VY(tXYZ xyz) const
Get the Y vector (O vector)
Definition: robodktypes.cpp:208
Mat::transl
static Mat transl(double x, double y, double z)
Return a translation matrix.
Definition: robodktypes.cpp:522
Mat::ToXYZRPW
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...
Definition: robodktypes.cpp:380
Mat::setVZ
void setVZ(double x, double y, double z)
Set the Z vector values (A vector)
Definition: robodktypes.cpp:234
Mat::Get
double Get(int r, int c) const
Get a matrix value.
Definition: robodktypes.cpp:299
Mat::Valid
bool Valid() const
Check if the matrix is valid.
Definition: robodktypes.cpp:518
Mat::setVY
void setVY(double x, double y, double z)
Set the Y vector values (O vector)
Definition: robodktypes.cpp:229
Mat::roty
static Mat roty(double ry)
Return a Y-axis rotation matrix
Definition: robodktypes.cpp:535
Mat::Values
const double * Values() const
Get a pointer to the 16-digit array (doubles or floats if ROBODK_API_FLOATS is defined).
Definition: robodktypes.cpp:500
Mat::setValues
void setValues(double pose[16])
Set the pose values.
Definition: robodktypes.cpp:267
Mat::Pos
void Pos(tXYZ xyz) const
Get the position (T position), in mm.
Definition: robodktypes.cpp:218
Mat::MakeHomogeneous
bool MakeHomogeneous()
Forces 4x4 matrix to be homogeneous (vx,vy,vz must be unitary vectors and respect: vx x vy = vz)....
Definition: robodktypes.cpp:351
Mat::rotz
static Mat rotz(double rz)
Return a Z-axis rotation matrix.
Definition: robodktypes.cpp:541
Mat::ValuesD
const double * ValuesD() const
Get a pointer to the 16-digit double array.
Definition: robodktypes.cpp:484
Mat::_ddata16
double _ddata16[16]
Copy of the data as a double array.
Definition: robodktypes.h:622
Mat::FromString
bool FromString(const QString &str)
Set the matrix given a XYZRPW string array (6-values)
Definition: robodktypes.cpp:440
Mat::_valid
double _valid
Flags if a matrix is not valid.
Definition: robodktypes.h:617
Mat::inv
Mat inv() const
Invert the pose (homogeneous matrix assumed)
Definition: robodktypes.cpp:305
Mat::setPos
void setPos(double x, double y, double z)
Set the position (T position) in mm.
Definition: robodktypes.cpp:239
Mat::VZ
void VZ(tXYZ xyz) const
Get the Z vector (A vector)
Definition: robodktypes.cpp:213
Mat::isHomogeneous
bool isHomogeneous() const
Returns true if the matrix is homogeneous, otherwise it returns false.
Definition: robodktypes.cpp:310
Mat::ValuesF
const float * ValuesF() const
Get a pointer to the 16-digit array as an array of floats.
Definition: robodktypes.cpp:491
Mat::Set
void Set(int r, int c, double value)
Set a matrix value.
Definition: robodktypes.cpp:290
Mat::rotx
static Mat rotx(double rx)
Return the X-axis rotation matrix.
Definition: robodktypes.cpp:529
Mat::XYZRPW_2_Mat
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 ...
Definition: robodktypes.cpp:459
tJoints
The tJoints class represents a joint position of a robot (robot axes).
Definition: robodktypes.h:239
tJoints::GetValues
int GetValues(double *joints)
GetValues.
Definition: robodktypes.cpp:95
tJoints::tJoints
tJoints(int ndofs=0)
tJoints
Definition: robodktypes.cpp:8
tJoints::ToString
QString ToString(const QString &separator=", ", int precision=3) const
Retrieve a string representation of the joint values.
Definition: robodktypes.cpp:101
tJoints::Length
int Length() const
机器人的关节轴数(或度freedom)
Definition: robodktypes.cpp:123
tJoints::Values
const double * Values() const
联合values.
Definition: robodktypes.cpp:60
tJoints::Valid
bool Valid()
Check if the joints are valid. For example, when we request the Inverse kinematics and there is no so...
Definition: robodktypes.cpp:131
tJoints::_nDOFs
int _nDOFs
number of degrees of freedom
Definition: robodktypes.h:340
tJoints::Data
double * Data()
Definition: robodktypes.cpp:73
tJoints::ValuesD
const double * ValuesD() const
联合values.
Definition: robodktypes.cpp:46
tJoints::_Values
double _Values[RDK_SIZE_JOINTS_MAX]
joint values (doubles, used to store the joint values)
Definition: robodktypes.h:343
tJoints::FromString
bool FromString(const QString &str)
Set the joint values given a comma-separated string. Tabs and spaces are also allowed.
Definition: robodktypes.cpp:113
tJoints: setLength
void setLength(int new_length)
Set the length of the array (only shrinking the array is allowed)
Definition: robodktypes.cpp:126
tJoints::_ValuesF
float _ValuesF[RDK_SIZE_JOINTS_MAX]
joint values (floats, used to return a copy as a float pointer)
Definition: robodktypes.h:346
tJoints::ValuesF
const float * ValuesF() const
联合values.
Definition: robodktypes.cpp:49
tJoints::SetValues
无效的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...
Definition: robodktypes.cpp:78
tMatrix2D
The tMatrix2D struct represents a variable size 2d Matrix. Use the Matrix2D_... functions to oeprate ...
Definition: robodktypes.h:145
tMatrix2D::data
double * data
Pointer to the data.
Definition: robodktypes.h:147
tMatrix2D::size
int * size
Pointer to the size array.
Definition: robodktypes.h:150
tMatrix2D::allocatedSize
int allocatedSize
Allocated size.
Definition: robodktypes.h:153
tMatrix2D::numDimensions
int numDimensions
Number of dimensions (usually 2)
Definition: robodktypes.h:156