RoboDKAPI - Documentation
robodk_api.cpp
1 #include "robodk_api.h"
2 #include
3 #include
4 #include
5 #include
6 #include
7
8
9 #ifdef _WIN32
10 // Default path on Windows:
11 #define ROBODK_DEFAULT_PATH_BIN "C:/RoboDK/bin/RoboDK.exe"
12
13 #elif __APPLE__
14 // Default Install Path on Mac
15 #define ROBODK_DEFAULT_PATH_BIN "~/RoboDK/Applications/RoboDK.app/Contents/MacOS/RoboDK"
16
17 #else
18
19 // Default Install Path on Linux:
20 #define ROBODK_DEFAULT_PATH_BIN "~/RoboDK/bin/RoboDK"
21
22 #endif
23
24 #define ROBODK_DEFAULT_PORT 20500
25
26 #define ROBODK_API_TIMEOUT 1000 // communication timeout. Raise this value for slow computers
27 #define ROBODK_API_START_STRING "CMD_START"
28 #define ROBODK_API_READY_STRING "READY"
29 #define ROBODK_API_LF "\n"
30
31
32
33 #define M_PI 3.14159265358979323846264338327950288
34
35
36 #ifndef RDK_SKIP_NAMESPACE
37 namespace RoboDK_API{
38 #endif
39
40
41 //----------------------------------- Joints class ------------------------
42 tJoints::tJoints(intndofs){
43 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
44 for(inti=0; i<_nDOFs; i++){
45 _Values[i] = 0.0;
46}
47}
49 SetValues(copy._Values, copy._nDOFs);
50}
51 tJoints::tJoints(const double*joints,intndofs){
52 SetValues(joints, ndofs);
53}
54 tJoints::tJoints(const float*joints,intndofs){
55 intndofs_ok = qMin(ndofs, RDK_SIZE_JOINTS_MAX);
56 doublejnts[RDK_SIZE_JOINTS_MAX];
57 for(inti=0; i
58jnts[i] = joints[i];
59}
60 SetValues(jnts, ndofs_ok);
61}
62 tJoints::tJoints(const tMatrix2D*mat2d,int列,intndofs){
63 intncols =Matrix2D_Size(mat2d, 2);
64 if(column >= ncols){
65 _nDOFs= 0;
66qDebug()<<"Warning: tMatrix2D column outside range when creating joints";
67}
68 if(ndofs < 0){
69ndofs =Matrix2D_Size(mat2d, 1);
70}
71 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
72 double*ptr =Matrix2D_Get_col(mat2d, column);
73 SetValues(ptr,_nDOFs);
74}
75 tJoints::tJoints(constQString &str){
76 _nDOFs= 0;
77 FromString(str);
78}
79
80 const double*tJoints::ValuesD()const{
81 return _Values;
82}
83 const float*tJoints::ValuesF()const{
84 for(inti=0; i
85((float*)_ValuesF)[i] =_Values[i];
86}
87 return _ValuesF;
88}
89 #ifdef ROBODK_API_FLOATS
90 const float*tJoints::Values()const{
91 return ValuesF();
92}
93 #else
94 const double*tJoints::Values()const{
95 return _Values;
96}
97 #endif
98
99 double*tJoints::Data(){
100 return _Values;
101}
102
103
104 void tJoints::SetValues(const double*values,intndofs){
105 if(ndofs >= 0){
106 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
107}
108 for(inti=0; i<_nDOFs; i++){
109 _Values[i] = values[i];
110}
111}
112
113 void tJoints::SetValues(const float*values,intndofs){
114 if(ndofs >= 0){
115 _nDOFs= qMin(ndofs, RDK_SIZE_JOINTS_MAX);
116}
117 for(inti=0; i<_nDOFs; i++){
118 _Values[i] = values[i];
119}
120}
121 int tJoints::GetValues(double*values){
122 for(inti=0; i<_nDOFs; i++){
123values[i] =_Values[i];
124}
125 return _nDOFs;
126}
127QStringtJoints::ToString(constQString &separator,intprecision)const{
128 if(!Valid()){
129 return "tJoints(Invalid)";
130}
131QString values("tJoints({");
132 if(_nDOFs<= 0){
133 returnvalues;
134}
135values.append(QString::number(_Values[0],'f',precision));
136 for(inti=1; i<_nDOFs; i++){
137values.append(separator);
138values.append(QString::number(_Values[i],'f',precision));
139}
140values.append("} , "+ QString::number(_nDOFs) +")");
141 returnvalues;
142}
143 bool tJoints: FromString(constQString &str){
144QStringList jnts_list = QString(str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts);
145 _nDOFs= qMin(jnts_list.length(), RDK_SIZE_JOINTS_MAX);
146 for(inti=0; i<_nDOFs; i++){
147QString stri(jnts_list.at(i));
148 _Values[i] = stri.trimmed().toDouble();
149}
150 return true;
151}
152
153 int tJoints::Length()const{
154 return _nDOFs;
155}
156
157 void tJoints::setLength(intnew_length) {
158 if(new_length >= 0 && new_length <_nDOFs){
159 _nDOFs= new_length;
160}
161}
162
163 bool tJoints::Valid()const{
164 return _nDOFs> 0;
165}
166 //---------------------------------------------------------------------
167
168
169
170
171
172
173 Mat transl(doublex,doubley,doublez){
174 return Mat::transl(x,y,z);
175}
176
177 Mat rotx(doublerx){
178 return Mat::rotx(rx);
179}
180
181 Mat roty(doublery){
182 return Mat::roty(ry);
183}
184
185 Mat rotz(doublerz){
186 return Mat::rotz(rz);
187}
188
189 Mat::Mat() : QMatrix4x4() {
190 _valid=true;
191setToIdentity();
192}
193 Mat::Mat(boolvalid) : QMatrix4x4() {
194 _valid= valid;
195setToIdentity();
196}
197
198 Mat::Mat(constQMatrix4x4 &matrix) : QMatrix4x4(matrix) {
199 // just copy
200 _valid=true;
201}
202 Mat::Mat(const Mat&matrix) : QMatrix4x4(matrix) {
203 // just copy
204 _valid= matrix._valid;
205}
206
207 Mat::Mat(doublenx,doubleox,doubleax,doubletx,doubleny,doubleoy,doubleay,doublety,doublenz,doubleoz,doubleaz,doubletz) :
208QMatrix4x4(nx, ox, ax, tx, ny, oy, ay, ty, nz, oz, az, tz, 0,0,0,1)
209{
210 _valid=true;
211}
212 Mat::Mat(const doublev[16]) :
213QMatrix4x4(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])
214{
215 _valid=true;
216}
217 Mat::Mat(const floatv[16]) :
218QMatrix4x4(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])
219{
220 _valid=true;
221}
222
223
224
225Mat::~Mat(){
226
227}
228
229
230 void Mat::VX(tXYZxyz)const{
231xyz[0] =Get(0, 0);
232xyz[1] =Get(1, 0);
233xyz[2] =Get(2, 0);
234}
235 void Mat::VY(tXYZxyz)const{
236xyz[0] =Get(0, 1);
237xyz[1] =Get(1, 1);
238xyz[2] =Get(2, 1);
239}
240 void Mat::VZ(tXYZxyz)const{
241xyz[0] =Get(0, 2);
242xyz[1] =Get(1, 2);
243xyz[2] =Get(2, 2);
244}
245 void Mat::Pos(tXYZxyz)const{
246xyz[0] =Get(0, 3);
247xyz[1] =Get(1, 3);
248xyz[2] =Get(2, 3);
249}
250 void Mat::setVX(doublex,doubley,doublez){
251 Set(0,0, x);
252 Set(1,0, y);
253 Set(2,0, z);
254}
255 void Mat::setVY(doublex,doubley,doublez){
256 Set(0,1, x);
257 Set(1,1, y);
258 Set(2,1, z);
259}
260
261 void Mat::setVZ(doublex,doubley,doublez){
262 Set(0,2, x);
263 Set(1,2, y);
264 Set(2,2, z);
265}
266
267 void Mat::setPos(doublex,doubley,doublez){
268 Set(0,3, x);
269 Set(1,3, y);
270 Set(2,3, z);
271}
272 void Mat::setVX(doublexyz[3]){
273 Set(0,0, xyz[0]);
274 Set(1,0, xyz[1]);
275 Set(2,0, xyz[2]);
276}
277 void Mat::setVY(doublexyz[3]){
278 Set(0,1, xyz[0]);
279 Set(1,1, xyz[1]);
280 Set(2,1, xyz[2]);
281}
282 void Mat::setVZ(doublexyz[3]){
283 Set(0, 2, xyz [0]);
284 Set(1,2, xyz[1]);
285 Set(2,2, xyz[2]);
286}
287 void Mat::setPos(doublexyz[3]){
288 Set(0,3, xyz[0]);
289 Set(1,3, xyz[1]);
290 Set(2,3, xyz[2]);
291}
292
293 void Mat::Set(inti,intj,doublevalue){
294QVector4D rw(this->row(i));
295rw[j] = value;
296setRow(i, rw);
297 // the following should not crash!!
298 //float **dt_ok = (float**) data();
299 //dt_ok[i][j] = value;
300}
301
302 double Mat::Get(inti,intj)const{
303 returnrow(i)[j];
304 // the following hsould be allowed!!
305 //return ((const float**)data())[i][j];
306}
307
308
309 Mat Mat::inv()const{
310 returnthis->inverted();
311}
312
313 bool Mat::isHomogeneous()const{
314 const booldebug_info =false;
315 tXYZvx, vy, vz;
316 const doubletol = 1e-7;
317 VX(vx);
318 VY(vy);
319 VZ(vz);
320 if(fabs((double) DOT(vx,vy)) > tol){
321 if(debug_info){
322qDebug() <<"Vector X and Y are not perpendicular!";
323}
324 return false;
325}else if(fabs((double) DOT(vx,vz)) > tol){
326 if(debug_info){
327qDebug() <<"Vector X and Z are not perpendicular!";
328}
329 return false;
330}else if(fabs((double) DOT(vy,vz)) > tol){
331 if(debug_info){
332qDebug() <<"Vector Y and Z are not perpendicular!";
333}
334 return false;
335}else if(fabs((double) (NORM(vx)-1.0)) > tol){
336 if(debug_info){
337qDebug() <<"Vector X is not unitary! "<< NORM(vx);
338}
339 return false;
340}else if(fabs((double) (NORM(vy)-1.0)) > tol){
341 if(debug_info){
342qDebug() <<"Vector Y is not unitary! "<< NORM(vy);
343}
344 return false;
345}else if(fabs((double) (NORM(vz)-1.0)) > tol){
346 if(debug_info){
347qDebug() <<"Vector Z is not unitary! "<< NORM(vz);
348}
349 return false;
350}
351 return true;
352}
353
355 tXYZvx, vy, vz;
356 VX(vx);
357 VY(vy);
358 VZ(vz);
359 boolis_homogeneous =isHomogeneous();
360 //if (is_homogeneous){
361 // return false;
362 //}
363
364NORMALIZE(vx);
365CROSS(vz, vx, vy);
366NORMALIZE(vz);
367CROSS(vy, vz, vx);
368NORMALIZE(vy);
369 setVX(vx);
370 setVY(vy);
371 setVZ(vz);
372 Set(3,0, 0.0);
373 Set(3、1、0.0);
374 Set(3,2, 0.0);
375 Set(3,3, 1.0);
376 return!is_homogeneous;
377}
378
379
380 //----------------------------------------------------
381
382 void Mat::ToXYZRPW(tXYZWPRxyzwpr)const{
383 doublex =Get(0,3);
384 doubley =Get(1,3);
385 doublez =Get(2,3);
386 doublew, p, r;
387 if(Get(2,0) > (1.0 - 1e-6)){
388p = -M_PI*0.5;
389r = 0;
390w = atan2(-Get(1,2),Get(1,1));
391}else if(Get(2,0) < -1.0 + 1e-6){
392p = 0.5*M_PI;
393r = 0;
394w = atan2(Get(1,2),Get(1,1));
395}else{
396p = atan2(-Get(2, 0), sqrt(Get(0, 0) *Get(0, 0) +Get(1, 0) *Get(1, 0)));
397w = atan2(Get(1, 0),Get(0, 0));
398r = atan2(Get(2, 1),Get(2, 2));
399}
400xyzwpr[0] = x;
401xyzwpr[1] = y;
402xyzwpr[2] = z;
403xyzwpr[3] = r*180.0/M_PI;
404xyzwpr[4] = p*180.0/M_PI;
405xyzwpr[5] = w*180.0/M_PI;
406}
407
408QStringMat::ToString(constQString &separator,intprecision,boolxyzwpr_only)const{
409 if(!Valid()){
410 return "Mat(Invalid)";
411}
412QString str;
413 if(!isHomogeneous()){
414str.append("Warning!! Pose is not homogeneous! Use Mat::MakeHomogeneous() to make this matrix homogeneous\n");
415}
416str.append("Mat(XYZRPW_2_Mat(");
417
418 tXYZWPRxyzwpr;
419 ToXYZRPW(xyzwpr);
420str.append(QString::number(xyzwpr[0],'f',precision));
421 for(inti=1; i<6; i++){
422str.append(separator);
423str.append(QString::number(xyzwpr[i],'f',precision));
424}
425str.append("))");
426
427 if(xyzwpr_only){
428 returnstr;
429}
430str.append("\n");
431 for(inti=0; i<4; i++){
432str.append("[");
433 for(intj=0; j<4; j++){
434str.append(QString::number(row(i)[j],'f', precision));
435 if(j < 3){
436str.append(separator);
437}
438}
439str.append("];\n");
440}
441 returnstr;
442}
443
444 bool Mat::FromString(constQString &pose_str){
445QStringList values_list = QString(pose_str).replace(";",",").replace("\t",",").split(",", QString::SkipEmptyParts);
446 intnvalues = qMin(values_list.length(), 6);
447 tXYZWPRxyzwpr;
448 for(inti=0; i<6; i++){
449xyzwpr[i] = 0.0;
450}
451 if(nvalues < 6){
452 FromXYZRPW(xyzwpr);
453 return false;
454}
455 for(inti=0; i
456QString stri(values_list.at(i));
457xyzwpr[i] = stri.trimmed().toDouble();
458}
459 FromXYZRPW(xyzwpr);
460 return true;
461}
462
463
464 Mat Mat::XYZRPW_2_Mat(doublex,doubley,doublez,doubler,doublep,doublew){
465 doublea = r * M_PI / 180.0;
466 doubleb = p * M_PI / 180.0;
467 doublec = w * M_PI / 180.0;
468 doubleca = cos(a);
469 doublesa = sin(a);
470 doublecb = cos(b);
471 doublesb = sin(b);
472 doublecc = cos(c);
473 doublesc = sin(c);
474 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);
475}
477 return XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
478}
479
481 Matnewmat =Mat::XYZRPW_2_Mat(xyzwpr[0], xyzwpr[1], xyzwpr[2], xyzwpr[3], xyzwpr[4], xyzwpr[5]);
482 for(inti=0; i<4; i++){
483 for(intj=0; j<4; j++){
484this->Set(i,j, newmat.Get(i,j));
485}
486}
487}
488
489 const double*Mat::ValuesD()const{
490 double* _ddata16_non_const = (double*)_ddata16;
491 for(inti=0; i<16; ++i){
492_ddata16_non_const[i] = constData()[i];
493}
494 return _ddata16;
495}
496 const float*Mat::ValuesF()const{
497 returnconstData();
498}
499
500 #ifdef ROBODK_API_FLOATS
501 const float*Mat::Values()const{
502 returnconstData();
503}
504 #else
505 const double*Mat::Values()const{
506 return ValuesD();
507}
508
509 #endif
510
511
512
513 void Mat::Values(doubledata[16])const{
514 for(inti=0; i<16; ++i){
515data[i] = constData()[i];
516}
517}
518 void Mat::Values(floatdata[16])const{
519 for(inti=0; i<16; ++i){
520data[i] = constData()[i];
521}
522}
523 bool Mat::Valid()const{
524 return _valid;
525}
526
527 Mat Mat::transl(doublex,doubley,doublez){
528 Matmat;
529mat.setToIdentity();
530mat.setPos(x, y, z);
531 returnmat;
532}
533
534 Mat Mat::rotx(doublerx){
535 doublecx = cos(rx);
536 doublesx = sin(rx);
537 return Mat(1, 0, 0, 0, 0, cx, -sx, 0, 0, sx, cx, 0);
538}
539
540 Mat Mat::roty(doublery){
541 doublecy = cos(ry);
542 doublesy = sin(ry);
543 return Mat(cy, 0, sy, 0, 0, 1, 0, 0, -sy, 0, cy, 0);
544}
545
546 Mat Mat::rotz(doublerz){
547 doublecz = cos(rz);
548 doublesz = sin(rz);
549 return Mat(cz, -sz, 0, 0, sz, cz, 0, 0, 0, 0, 1, 0);
550}
551
552
553
554
555 //---------------------------------------------------------------------------------------------------
556 //---------------------------------------------------------------------------------------------------
557 //---------------------------------------------------------------------------------------------------
558 //---------------------------------------------------------------------------------------------------
560 Item::Item(RoboDK*rdk, quint64 ptr, qint32 type) {
561 _RDK= rdk;
562 _PTR= ptr;
563 _TYPE= type;
564}
565Item::Item(constItem &other) {
566 _RDK= other._RDK;
567 _PTR= other._PTR;
568 _TYPE= other._TYPE;
569}
570Item::~Item(){
571
572}
573
574QString Item::ToString()const{
575 if(this->Valid()){
576 returnQString("Item(&RDK, %1, %2); // %3").arg(_PTR).arg(_TYPE).arg(Name());
577}
578 return "Item(Invalid)";
579}
580
581
587 return _RDK;
588}
589
594 _RDK=new RoboDK();
595}
596
602 int Item::Type()const{
603 _RDK->_check_connection();
604 _RDK->_send_Line("G_Item_Type");
605 _RDK->_send_Item(this);
606 intitemtype =_RDK->_recv_Int();
607 _RDK->_check_status();
608 returnitemtype;
609}
610
612
617 void Item::Save(constQString &filename){
618 _RDK->Save(filename,this);
619}
620
624 void Item::Delete(){
625 _RDK->_check_connection();
626 _RDK->_send_Line("Remove");
627 _RDK->_send_Item(this);
628 _RDK->_check_status();
629 _PTR= 0;
630 _TYPE= -1;
631}
632
637 bool Item::Valid(boolcheck_deleted)const{
638 if(check_deleted){
639 return Type() > 0;
640}
641 return _PTR!= 0;
642}
647 void Item::setParent(Itemparent){
648 _RDK->_check_connection();
649 _RDK->_send_Line("S_Parent");
650 _RDK->_send_Item(this);
651 _RDK->_send_Item(parent);
652 _RDK->_check_status();
653}
654
661 _RDK->_check_connection();
662 _RDK->_send_Line("S_Parent_Static");
663 _RDK->_send_Item(this);
664 _RDK->_send_Item(parent);
665 _RDK->_check_status();
666}
667
673 _RDK->_check_connection();
674 _RDK->_send_Line("Attach_Closest");
675 _RDK->_send_Item(this);
676 Itemitem_attached =_RDK->_recv_Item();
677 _RDK->_check_status();
678 returnitem_attached;
679}
680
686 _RDK->_check_connection();
687 _RDK->_send_Line("Detach_Closest");
688 _RDK->_send_Item(this);
689 _RDK->_send_Item(parent);
690 Itemitem_detached =_RDK->_recv_Item();
691 _RDK->_check_status();
692 returnitem_detached;
693}
694
698 void Item::DetachAll(Itemparent) {
699 _RDK->_check_connection();
700 _RDK->_send_Line("Detach_All");
701 _RDK->_send_Item(this);
702 _RDK->_send_Item(parent);
703 _RDK->_check_status();
704}
705
706
712 _RDK->_check_connection();
713 _RDK->_send_Line("G_Parent");
714 _RDK->_send_Item(this);
715 Itemitm_parent =_RDK->_recv_Item();
716 _RDK->_check_status();
717 returnitm_parent;
718}
719
720
726QListItem::Childs()const{
727 _RDK->_check_connection();
728 _RDK->_send_Line("G_Childs");
729 _RDK->_send_Item(this);
730 intnitems =_RDK->_recv_Int();
731QList itemlist;
732 for(inti = 0; i < nitems; i++)
733{
734itemlist.append(_RDK->_recv_Item());
735}
736 _RDK->_check_status();
737 returnitemlist;
738}
739
744 bool Item::Visible()const{
745 _RDK->_check_connection();
746 _RDK->_send_Line("G_Visible");
747 _RDK->_send_Item(this);
748 intvisible =_RDK->_recv_Int();
749 _RDK->_check_status();
750 return(visible != 0);
751}
757 void Item::setVisible(boolvisible,intvisible_frame){
758 if(visible_frame < 0)
759{
760visible_frame = visible ? 1 : 0;
761}
762 _RDK->_check_connection();
763 _RDK->_send_Line("S_Visible");
764 _RDK->_send_Item(this);
765 _RDK->_send_Int(visible ? 1 : 0);
766 _RDK->_send_Int(visible_frame);
767 _RDK->_check_status();
768}
769
774QStringItem::Name()const{
775 _RDK->_check_connection();
776 _RDK->_send_Line("G_Name");
777 _RDK->_send_Item(this);
778QString name =_RDK->_recv_Line();
779 _RDK->_check_status();
780 returnname;
781}
782
787 void Item::setName(constQString &name){
788 _RDK->_check_connection();
789 _RDK->_send_Line("S_Name");
790 _RDK->_send_Item(this);
791 _RDK->_send_Line(name);
792 _RDK->_check_status();
793}
794
795 // add more methods
796
802 void Item::setPose(const Matpose){
803 _RDK->_check_connection();
804 _RDK->_send_Line("S_Hlocal");
805 _RDK->_send_Item(this);
806 _RDK->_send_Pose(pose);
807 _RDK->_check_status();
808}
809
815 Mat Item::Pose()const{
816 _RDK->_check_connection();
817 _RDK->_send_Line("G_Hlocal");
818 _RDK->_send_Item(this);
819 Matpose =_RDK->_recv_Pose();
820 _RDK->_check_status();
821 returnpose;
822}
823
828 void Item::setGeometryPose(const Matpose){
829 _RDK->_check_connection();
830 _RDK->_send_Line("S_Hgeom");
831 _RDK->_send_Item(this);
832 _RDK->_send_Pose(pose);
833 _RDK->_check_status();
834}
835
841 _RDK->_check_connection();
842 _RDK->_send_Line("G_Hgeom");
843 _RDK->_send_Item(this);
844 Matpose =_RDK->_recv_Pose();
845 _RDK->_check_status();
846 returnpose;
847}
848 /*
853 void Item::setHtool(Mat pose){
854 _RDK->_check_connection();
855 _RDK->_send_Line("S_Htool");
856 _RDK->_send_Item(this);
857 _RDK->_send_Pose(pose);
858 _RDK->_check_status();
859 }
860
866 Mat Item::Htool(){
867 _RDK->_check_connection();
868 _RDK->_send_Line("G_Htool");
869 _RDK->_send_Item(this);
870 Mat pose = _RDK->_recv_Pose();
871 _RDK->_check_status();
872 return pose;
873 }
874 */
880 _RDK->_check_connection();
881 _RDK->_send_Line("G_Tool");
882 _RDK->_send_Item(this);
883 Matpose =_RDK->_recv_Pose();
884 _RDK->_check_status();
885 returnpose;
886}
887
893 _RDK->_check_connection();
894 _RDK->_send_Line("G_Frame");
895 _RDK->_send_Item(this);
896 Matpose =_RDK->_recv_Pose();
897 _RDK->_check_status();
898 returnpose;
899}
900
906 void Item::setPoseFrame(const Matframe_pose){
907 _RDK->_check_connection();
908 _RDK->_send_Line("S_Frame");
909 _RDK->_send_Pose(frame_pose);
910 _RDK->_send_Item(this);
911 _RDK->_check_status();
912}
913
919 void Item::setPoseFrame(const Itemframe_item){
920 _RDK->_check_connection();
921 _RDK->_send_Line("S_Frame_ptr");
922 _RDK->_send_Item(frame_item);
923 _RDK->_send_Item(this);
924 _RDK->_check_status();
925}
926
932 void Item::setPoseTool(const Mattool_pose){
933 _RDK->_check_connection();
934 _RDK->_send_Line("S_Tool");
935 _RDK->_send_Pose(tool_pose);
936 _RDK->_send_Item(this);
937 _RDK->_check_status();
938}
939
945 void Item::setPoseTool(const Itemtool_item){
946 _RDK->_check_connection();
947 _RDK->_send_Line("S_Tool_ptr");
948 _RDK->_send_Item(tool_item);
949 _RDK->_send_Item(this);
950 _RDK->_check_status();
951}
952
957 void Item::setPoseAbs(const Matpose){
958 _RDK->_check_connection();
959 _RDK->_send_Line("S_Hlocal_Abs");
960 _RDK->_send_Item(this);
961 _RDK->_send_Pose(pose);
962 _RDK->_check_status();
963
964}
965
971 _RDK->_check_connection();
972 _RDK->_send_Line("G_Hlocal_Abs");
973 _RDK->_send_Item(this);
974 Matpose =_RDK->_recv_Pose();
975 _RDK->_check_status();
976 returnpose;
977}
978
983 void Item::setColor(doublecolorRGBA[4]){
984 _RDK->_check_connection();
985 _RDK->_send_Line("S_Color");
986 _RDK->_send_Item(this);
987 _RDK->_send_Array(colorRGBA, 4);
988 _RDK->_check_status();
989
990}
991
996
997
1003 void Item::Scale(doublescale){
1004 doublescale_xyz[3];
1005scale_xyz[0] = scale;
1006scale_xyz[1] = scale;
1007scale_xyz[2] = scale;
1008 Scale(scale_xyz);
1009}
1010
1016 void Item::Scale(doublescale_xyz[3]){
1017 _RDK->_check_connection();
1018 _RDK->_send_Line("Scale");
1019 _RDK->_send_Item(this);
1020 _RDK->_send_Array(scale_xyz, 3);
1021 _RDK->_check_status();
1022}
1023
1024
1025
1026
1036 Item Item::setMachiningParameters(QString ncfile,Itempart_obj, QString options)
1037{
1038 _RDK->_check_connection();
1039 _RDK->_send_Line("S_MachiningParams");
1040 _RDK->_send_Item(this);
1041 _RDK->_send_Line(ncfile);
1042 _RDK->_send_Item(part_obj);
1043 _RDK->_send_Line("NO_UPDATE "+ options);
1044 _RDK->_TIMEOUT = 3600 * 1000;
1045 Itemprogram =_RDK->_recv_Item();
1046 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
1047 doublestatus =_RDK->_recv_Int() / 1000.0;
1048 _RDK->_check_status();
1049 returnprogram;
1050}
1051
1056 _RDK->_check_connection();
1057 _RDK->_send_Line("S_Target_As_RT");
1058 _RDK->_send_Item(this);
1059 _RDK->_check_status();
1060}
1061
1066 _RDK->_check_connection();
1067 _RDK->_send_Line("S_Target_As_JT");
1068 _RDK->_send_Item(this);
1069 _RDK->_check_status();
1070}
1071
1076 _RDK->_check_connection();
1077 _RDK->_send_Line("Target_Is_JT");
1078 _RDK->_send_Item(this);
1079 intis_jt =_RDK->_recv_Int();
1080 _RDK->_check_status();
1081 returnis_jt > 0;
1082}
1083
1084 //#####Robot item calls####
1085
1091 tJointsjnts;
1092 _RDK->_check_connection();
1093 _RDK->_send_Line("G_Thetas");
1094 _RDK->_send_Item(this);
1095 _RDK- > _recv_Array (&jnts);
1096 _RDK->_check_status();
1097 returnjnts;
1098}
1099
1100 // add more methods
1101
1107 tJointsjnts;
1108 _RDK->_check_connection();
1109 _RDK->_send_Line("G_Home");
1110 _RDK->_send_Item(this);
1111 _RDK- > _recv_Array (&jnts);
1112 _RDK->_check_status();
1113 returnjnts;
1114}
1115
1116
1121 void Item::setJointsHome(const tJoints&jnts){
1122 _RDK->_check_connection();
1123 _RDK->_send_Line("S_Home");
1124 _RDK->_send_Array(&jnts);
1125 _RDK->_send_Item(this);
1126 _RDK->_check_status();
1127}
1128
1134 Item Item::ObjectLink(intlink_id){
1135 _RDK->_check_connection();
1136 _RDK->_send_Line(“G_LinkObjId”);
1137 _RDK->_send_Item(this);
1138 _RDK->_send_Int(link_id);
1139 Itemitem =_RDK->_recv_Item();
1140 _RDK->_check_status();
1141 returnitem;
1142}
1143
1149 Item Item::getLink(inttype_linked){
1150 _RDK->_check_connection();
1151 _RDK->_send_Line("G_LinkType");
1152 _RDK->_send_Item(this);
1153 _RDK->_send_Int(type_linked);
1154 Itemitem =_RDK->_recv_Item();
1155 _RDK->_check_status();
1156 returnitem;
1157}
1158
1159
1164 void Item::setJoints(const tJoints&jnts){
1165 _RDK->_check_connection();
1166 _RDK->_send_Line("S_Thetas");
1167 _RDK->_send_Array(&jnts);
1168 _RDK->_send_Item(this);
1169 _RDK->_check_status();
1170}
1171
1177 void Item::JointLimits(tJoints*lower_limits,tJoints*upper_limits){
1178 _RDK->_check_connection();
1179 _RDK->_send_Line("G_RobLimits");
1180 _RDK->_send_Item(this);
1181 _RDK->_recv_Array(lower_limits);
1182 _RDK->_recv_Array(upper_limits);
1183 doublejoints_type =_RDK->_recv_Int() / 1000.0;
1184 _RDK->_check_status();
1185}
1186
1192 void Item::setJointLimits(const tJoints&lower_limits,const tJoints&upper_limits){
1193 _RDK->_check_connection();
1194 _RDK->_send_Line("S_RobLimits");
1195 _RDK->_send_Item(this);
1196 _RDK->_send_Array(&lower_limits);
1197 _RDK->_send_Array(&upper_limits);
1198 //double joints_type = _RDK->_recv_Int() / 1000.0;
1199 _RDK->_check_status();
1200}
1201
1207 void Item::setRobot(const Item&robot){
1208 _RDK->_check_connection();
1209 _RDK->_send_Line("S_Robot");
1210 _RDK->_send_Item(this);
1211 _RDK->_send_Item(robot);
1212 _RDK->_check_status();
1213}
1214
1215
1222 Item 项目::AddTool(const Mat&tool_pose,constQString &tool_name){
1223 _RDK->_check_connection();
1224 _RDK->_send_Line("AddToolEmpty");
1225 _RDK->_send_Item(this);
1226 _RDK->_send_Pose(tool_pose);
1227 _RDK->_send_Line(tool_name);
1228 Itemnewtool =_RDK->_recv_Item();
1229 _RDK->_check_status();
1230 returnnewtool;
1231}
1232
1238 Mat 项目::SolveFK(const tJoints&joints,const Mat*tool,const Mat*ref){
1239 _RDK->_check_connection();
1240 _RDK->_send_Line("G_FK");
1241 _RDK->_send_Array(&joints);
1242 _RDK->_send_Item(this);
1243 Matpose =_RDK->_recv_Pose();
1244 Matbase2flange(pose);
1245 if(tool !=nullptr){
1246base2flange = pose*(*tool);
1247}
1248 if(ref !=nullptr){
1249base2flange = ref->inv() * base2flange;
1250}
1251 _RDK->_check_status();
1252 returnbase2flange;
1253}
1254
1260 void Item::JointsConfig(const tJoints&joints,tConfigconfig){
1261 _RDK->_check_connection();
1262 _RDK->_send_Line("G_Thetas_Config");
1263 _RDK->_send_Array(&joints);
1264 _RDK->_send_Item(this);
1265 intsz = RDK_SIZE_MAX_CONFIG;
1266 _RDK->_recv_Array(config, &sz);
1267 _RDK->_check_status();
1268 //return config;
1269}
1270
1278 tJoints Item::SolveIK(const Mat&pose,const Mat*tool,const Mat*ref){
1279 tJointsjnts;
1280 Matbase2flange(pose);
1281 if(tool !=nullptr){
1282base2flange = pose*tool->inv();
1283}
1284 if(ref !=nullptr){
1285base2flange = (*ref) * base2flange;
1286}
1287 _RDK->_check_connection();
1288 _RDK->_send_Line("G_IK");
1289 _RDK->_send_Pose(base2flange);
1290 _RDK->_send_Item(this);
1291 _RDK- > _recv_Array (&jnts);
1292 _RDK->_check_status();
1293 returnjnts;
1294}
1295
1296
1297
1306 tJoints Item::SolveIK(const Matpose,tJointsjoints_approx,const Mat*tool,const Mat*ref){
1307 Matbase2flange(pose);
1308 if(tool !=nullptr){
1309base2flange = pose*tool->inv();
1310}
1311 if(ref !=nullptr){
1312base2flange = (*ref) * base2flange;
1313}
1314 _RDK->_check_connection();
1315 _RDK->_send_Line("G_IK_jnts");
1316 _RDK->_send_Pose(base2flange);
1317 _RDK->_send_Array(&joints_approx);
1318 _RDK->_send_Item(this);
1319 tJointsjnts;
1320 _RDK- > _recv_Array (&jnts);
1321 _RDK->_check_status();
1322 returnjnts;
1323}
1324
1325
1331 tMatrix2D*Item::SolveIK_All_Mat2D(const Mat&pose,const Mat*tool,const Mat*ref){
1332 tMatrix2D*mat2d =nullptr;
1333 Matbase2flange(pose);
1334 if(tool !=nullptr){
1335base2flange = pose*tool->inv();
1336}
1337 if(ref !=nullptr){
1338base2flange = (*ref) * base2flange;
1339}
1340 _RDK->_check_connection();
1341 _RDK->_send_Line("G_IK_cmpl");
1342 _RDK->_send_Pose(base2flange);
1343 _RDK->_send_Item(this);
1344 _RDK->_recv_Matrix2D(&mat2d);
1345 _RDK->_check_status();
1346 returnmat2d;
1347}
1348QListItem::SolveIK_All(const Mat&pose,const Mat*tool,const Mat*ref){
1349 tMatrix2D*mat2d =SolveIK_All_Mat2D(pose, tool, ref);
1350QList < tJoints > jnts_list;
1351 intndofs =Matrix2D_Size(mat2d, 1) - 2;
1352 intnsol =Matrix2D_Size(mat2d, 2);
1353 for(inti=0; i
1354 tJointsjnts =tJoints(mat2d, i);
1355jnts.setLength(jnts.Length() - 2);
1356jnts_list.append(jnts);
1357}
1358 returnjnts_list;
1359}
1360
1366 bool Item::Connect(constQString &robot_ip){
1367 _RDK->_check_connection();
1368 _RDK->_send_Line("Connect");
1369 _RDK->_send_Item(this);
1370 _RDK->_send_Line(robot_ip);
1371 intstatus =_RDK->_recv_Int();
1372 _RDK->_check_status();
1373 returnstatus != 0;
1374}
1375
1381 _RDK->_check_connection();
1382 _RDK->_send_Line("Disconnect");
1383 _RDK->_send_Item(this);
1384 intstatus =_RDK->_recv_Int();
1385 _RDK->_check_status();
1386 returnstatus != 0;
1387}
1388
1394 void Item::MoveJ(const Item&itemtarget,boolblocking){
1396 _RDK->_check_connection();
1397 _RDK->_send_Line("Add_INSMOVE");
1398 _RDK->_send_Item(itemtarget);
1399 _RDK->_send_Item(this);
1400 _RDK->_send_Int(1);
1401 _RDK->_check_status();
1402}else{
1403 _RDK->_moveX(&itemtarget,nullptr,nullptr,this, 1, blocking);
1404}
1405}
1406
1412 void Item::MoveJ(const tJoints&joints,boolblocking){
1413 _RDK->_moveX(nullptr, &joints,nullptr,this, 1, blocking);
1414}
1415
1421 void Item::MoveJ(const Mat&target,boolblocking){
1422 _RDK->_moveX(nullptr,nullptr, &target,this, 1, blocking);
1423}
1424
1430 void Item::MoveL(const Item&itemtarget,boolblocking){
1432 _RDK->_check_connection();
1433 _RDK->_send_Line("Add_INSMOVE");
1434 _RDK->_send_Item(itemtarget);
1435 _RDK->_send_Item(this);
1436 _RDK->_send_Int(2);
1437 _RDK->_check_status();
1438}else{
1439 _RDK->_moveX(&itemtarget,nullptr,nullptr,this, 2, blocking);
1440}
1441}
1442
1448 void Item::MoveL(const tJoints&joints,boolblocking){
1449 _RDK->_moveX(nullptr, &joints,nullptr,this, 2, blocking);
1450}
1451
1457 void Item::MoveL(const Mat&target,boolblocking){
1458 _RDK->_moveX(nullptr,nullptr, &target,this, 2, blocking);
1459}
1460
1467 void Item::MoveC(const Item&itemtarget1,const Item&itemtarget2,boolblocking){
1468 _RDK->_moveC(&itemtarget1,nullptr,nullptr, &itemtarget2,nullptr,nullptr,this, blocking);
1469}
1470
1477 void Item::MoveC(const tJoints&joints1,const tJoints&joints2,boolblocking){
1478 _RDK->_moveC(nullptr, &joints1,nullptr,nullptr, &joints2,nullptr,this, blocking);
1479}
1480
1487 void Item::MoveC(const Mat&target1,const Mat&target2,boolblocking){
1488 _RDK->_moveC(nullptr,nullptr, &target1,nullptr,nullptr, &target2,this, blocking);
1489}
1490
1498 int Item::MoveJ_Test(const tJoints&j1,const tJoints&j2,doubleminstep_deg){
1499 _RDK->_check_connection();
1500 _RDK->_send_Line("CollisionMove");
1501 _RDK->_send_Item(this);
1502 _RDK->_send_Array(&j1);
1503 _RDK->_send_Array(&j2);
1504 _RDK->_send_Int((int)(minstep_deg * 1000.0));
1505 _RDK->_TIMEOUT = 3600 * 1000;
1506 intcollision =_RDK->_recv_Int();
1507 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
1508 _RDK->_check_status();
1509 returncollision;
1510}
1511
1519 int Item::MoveL_Test(const tJoints&j1,const Mat&pose2,doubleminstep_deg){
1520 _RDK->_check_connection();
1521 _RDK->_send_Line("CollisionMoveL");
1522 _RDK->_send_Item(this);
1523 _RDK->_send_Array(&j1);
1524 _RDK->_send_Pose(pose2);
1525 _RDK->_send_Int((int)(minstep_deg * 1000.0));
1526 _RDK->_TIMEOUT = 3600 * 1000;
1527 intcollision =_RDK->_recv_Int();
1528 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
1529 _RDK->_check_status();
1530 returncollision;
1531}
1532
1533
1541 void Item::setSpeed(doublespeed_linear,doublespeed_joints,doubleaccel_linear,doubleaccel_joints){
1542 _RDK->_check_connection();
1543 _RDK->_send_Line(“S_Speed4”);
1544 _RDK->_send_Item(this);
1545 doublespeed_accel[4];
1546speed_accel[0] = speed_linear;
1547speed_accel[1] = speed_joints;
1548speed_accel[2] = accel_linear;
1549speed_accel[3] = accel_joints;
1550 _RDK->_send_Array(speed_accel, 4);
1551 _RDK->_check_status();
1552}
1553
1558 void Item::setRounding(doublezonedata){
1559 _RDK->_check_connection();
1560 _RDK->_send_Line("S_ZoneData");
1561 _RDK->_send_Int((int)(zonedata * 1000.0));
1562 _RDK->_send_Item(this);
1563 _RDK->_check_status();
1564}
1565
1571 _RDK->_check_connection();
1572 _RDK->_send_Line("Show_Seq");
1573 _RDK->_send_Matrix2D(sequence);
1574 _RDK->_send_Item(this);
1575 _RDK->_check_status();
1576}
1577
1578
1583 bool Item::Busy(){
1584 _RDK->_check_connection();
1585 _RDK->_send_Line("IsBusy");
1586 _RDK->_send_Item(this);
1587 intbusy =_RDK->_recv_Int();
1588 _RDK->_check_status();
1589 return(busy > 0);
1590}
1591
1596 void Item::Stop(){
1597 _RDK->_check_connection();
1598 _RDK->_send_Line("Stop");
1599 _RDK->_send_Item(this);
1600 _RDK->_check_status();
1601}
1602
1607 void Item::WaitMove(doubletimeout_sec)const{
1608 _RDK->_check_connection();
1609 _RDK->_send_Line("WaitMove");
1610 _RDK->_send_Item(this);
1611 _RDK->_check_status();
1612 _RDK->_TIMEOUT = (int)(timeout_sec * 1000.0);
1613 _RDK->_check_status();//will wait here;
1614 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
1615 //int isbusy = _RDK->Busy(this);
1616 //while (isbusy)
1617 //{
1618 // busy = _RDK->Busy(item);
1619 //}
1620}
1621
1622
1627 void Item::setAccuracyActive(intaccurate){
1628 _RDK->_check_connection();
1629 _RDK->_send_Line("S_AbsAccOn");
1630 _RDK->_send_Item(this);
1631 _RDK->_send_Int(accurate);
1632 _RDK->_check_status();
1633}
1634
1636
1637
1638 // ---- Program item calls -----
1639
1645 bool Item::MakeProgram(constQString &filename){
1646 _RDK->_check_connection();
1647 _RDK->_send_Line("MakeProg");
1648 _RDK->_send_Item(this);
1649 _RDK->_send_Line(filename);
1650 intprog_status =_RDK->_recv_Int();
1651QString prog_log_str =_RDK->_recv_Line();
1652 _RDK->_check_status();
1653 boolsuccess =false;
1654 if(prog_status > 1) {
1655success =true;
1656}
1657 returnsuccess;// prog_log_str
1658}
1659
1665 void Item::setRunType(intprogram_run_type){
1666 _RDK->_check_connection();
1667 _RDK->_send_Line("S_ProgRunType");
1668 _RDK->_send_Item(this);
1669 _RDK->_send_Int(program_run_type);
1670 _RDK->_check_status();
1671}
1672
1683 _RDK->_check_connection();
1684 _RDK->_send_Line("RunProg");
1685 _RDK->_send_Item(this);
1686 intprog_status =_RDK->_recv_Int();
1687 _RDK->_check_status();
1688 returnprog_status;
1689}
1690
1691
1701 int Item::RunCode(constQString ¶meters){
1702 _RDK->_check_connection();
1703 if(parameters.isEmpty()){
1704 _RDK->_send_Line("RunProg");
1705 _RDK->_send_Item(this);
1706}else{
1707 _RDK->_send_Line("RunProgParam");
1708 _RDK->_send_Item(this);
1709 _RDK->_send_Line(parameters);
1710}
1711 intprogstatus =_RDK->_recv_Int();
1712 _RDK->_check_status();
1713 returnprogstatus;
1714}
1715
1721 int Item::RunInstruction(constQString &code,intrun_type){
1722 _RDK->_check_connection();
1723 _RDK->_send_Line("RunCode2");
1724 _RDK->_send_Item(this);
1725 _RDK->_send_Line(QString(code).replace("\n\n","
"
).replace("\n","
"
));
1726 _RDK->_send_Int(run_type);
1727 intprogstatus =_RDK->_recv_Int();
1728 _RDK->_check_status();
1729 returnprogstatus;
1730}
1731
1736 void Item::Pause(doubletime_ms){
1737 _RDK->_check_connection();
1738 _RDK->_send_Line("RunPause");
1739 _RDK->_send_Item(this);
1740 _RDK->_send_Int((int)(time_ms * 1000.0));
1741 _RDK->_check_status();
1742}
1743
1744
1750 void Item::setDO(constQString &io_var,constQString &io_value){
1751 _RDK->_check_connection();
1752 _RDK->_send_Line("setDO");
1753 _RDK->_send_Item(this);
1754 _RDK->_send_Line(io_var);
1755 _RDK->_send_Line(io_value);
1756 _RDK->_check_status();
1757}
1763 void Item::setAO(constQString &io_var,constQString &io_value){
1764 _RDK->_check_connection();
1765 _RDK->_send_Line("setAO");
1766 _RDK->_send_Item(this);
1767 _RDK->_send_Line(io_var);
1768 _RDK->_send_Line(io_value);
1769 _RDK->_check_status();
1770}
1771
1776QStringItem::getDI(constQString &io_var){
1777 _RDK->_check_connection();
1778 _RDK->_send_Line("getDI");
1779 _RDK->_send_Item(this);
1780 _RDK->_send_Line(io_var);
1781QString io_value(_RDK->_recv_Line());
1782 _RDK->_check_status();
1783 returnio_value;
1784}
1785
1790QStringItem::getAI(constQString &io_var){
1791 _RDK->_check_connection();
1792 _RDK->_send_Line("getAI");
1793 _RDK->_send_Item(this);
1794 _RDK->_send_Line(io_var);
1795QString di_value(_RDK->_recv_Line());
1796 _RDK->_check_status();
1797 returndi_value;
1798}
1799
1806 void Item::waitDI(constQString &io_var,constQString &io_value,doubletimeout_ms){
1807 _RDK->_check_connection();
1808 _RDK->_send_Line("waitDI");
1809 _RDK->_send_Item(this);
1810 _RDK->_send_Line(io_var);
1811 _RDK->_send_Line(io_value);
1812 _RDK->_send_Int((int)(timeout_ms * 1000.0));
1813 _RDK->_check_status();
1814}
1815
1816
1826 void Item::customInstruction(constQString &name,constQString &path_run,constQString &path_icon,boolblocking,constQString &cmd_run_on_robot){
1827 _RDK->_check_connection();
1828 _RDK->_send_Line("InsCustom2");
1829 _RDK->_send_Item(this);
1830 _RDK->_send_Line(name);
1831 _RDK->_send_Line(path_run);
1832 _RDK->_send_Line(path_icon);
1833 _RDK->_send_Line(cmd_run_on_robot);
1834 _RDK->_send_Int(blocking ? 1 : 0);
1835 _RDK->_check_status();
1836}
1837
1838 /*
1844 void Item::addMoveJ(const Item &itemtarget){
1845 _RDK->_check_connection();
1846 _RDK->_send_Line("Add_INSMOVE");
1847 _RDK->_send_Item(itemtarget);
1848 _RDK->_send_Item(this);
1849 _RDK->_send_Int(1);
1850 _RDK->_check_status();
1851 }
1852
1857 void Item::addMoveL(const Item &itemtarget){
1858 _RDK->_check_connection();
1859 _RDK->_send_Line("Add_INSMOVE");
1860 _RDK->_send_Item(itemtarget);
1861 _RDK->_send_Item(this);
1862 _RDK->_send_Int(2);
1863 _RDK->_check_status();
1864 }
1865 */
1866
1871 void Item::ShowInstructions(boolvisible){
1872 _RDK->_check_connection();
1873 _RDK->_send_Line("Prog_ShowIns");
1874 _RDK->_send_Item(this);
1875 _RDK->_send_Int(visible ? 1 : 0);
1876 _RDK->_check_status();
1877}
1878
1883 void Item::ShowTargets(boolvisible){
1884 _RDK->_check_connection();
1885 _RDK->_send_Line("Prog_ShowTargets");
1886 _RDK->_send_Item(this);
1887 _RDK->_send_Int(visible ? 1 : 0);
1888 _RDK->_check_status();
1889}
1890
1891
1898 _RDK->_check_connection();
1899 _RDK->_send_Line("Prog_Nins");
1900 _RDK->_send_Item(this);
1901 intnins =_RDK->_recv_Int();
1902 _RDK->_check_status();
1903 returnnins;
1904}
1905
1916 void Item::Instruction(intins_id, QString &name,int&instype,int&movetype,bool&isjointtarget,Mat&target,tJoints&joints){
1917 _RDK->_check_connection();
1918 _RDK->_send_Line("Prog_GIns");
1919 _RDK->_send_Item(this);
1920 _RDK->_send_Int(ins_id);
1921name =_RDK->_recv_Line();
1922instype =_RDK->_recv_Int();
1923movetype = 0;
1924isjointtarget =false;
1925 //target = null;
1926 //joints = null;
1927 if(instype ==RoboDK::INS_TYPE_MOVE) {
1928movetype =_RDK->_recv_Int();
1929isjointtarget =_RDK->_recv_Int() > 0 ? true :false;
1930target =_RDK->_recv_Pose();
1931 _RDK->_recv_Array(&joints);
1932}
1933 _RDK->_check_status();
1934}
1935
1946 void Item::setInstruction(intins_id,constQString &name,intinstype,intmovetype,boolisjointtarget,const Mat&target,const tJoints&joints){
1947 _RDK->_check_connection();
1948 _RDK->_send_Line("Prog_SIns");
1949 _RDK->_send_Item(this);
1950 _RDK->_send_Int(ins_id);
1951 _RDK->_send_Line(name);
1952 _RDK->_send_Int(instype);
1953 if(instype ==RoboDK::INS_TYPE_MOVE)
1954{
1955 _RDK->_send_Int(movetype);
1956 _RDK->_send_Int(isjointtarget ? 1 : 0);
1957 _RDK->_send_Pose(target);
1958 _RDK->_send_Array(&joints);
1959}
1960 _RDK->_check_status();
1961}
1962
1963
1970 _RDK->_check_connection();
1971 _RDK->_send_Line("G_ProgInsList");
1972 _RDK->_send_Item(this);
1973 _RDK->_recv_Matrix2D(&instructions);
1974 interrors =_RDK->_recv_Int();
1975 _RDK->_check_status();
1976 returnerrors;
1977}
1978
1979
1980
1991 double Item::Update(intcollision_check,inttimeout_sec,double*out_nins_time_dist,doublemm_step,doubledeg_step){
1992 _RDK->_check_connection();
1993 _RDK->_send_Line("Update2");
1994 _RDK->_send_Item(this);
1995 doublevalues[5];
1996values[0] = collision_check;
1997values[1] = mm_step;
1998values[2] = deg_step;
1999 _RDK- > _send_Array(价值s, 3);
2000 _RDK->_TIMEOUT = timeout_sec * 1000;
2001 doublereturn_values[10];
2002 intnvalues = 10;
2003 _RDK->_recv_Array(return_values, &nvalues);
2004 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
2005QString readable_msg =_RDK->_recv_Line();
2006 _RDK->_check_status();
2007 doubleratio_ok = return_values[3];
2008 if(out_nins_time_dist !=nullptr)
2009{
2010out_nins_time_dist[0] = return_values[0];// number of correct instructions
2011out_nins_time_dist[1] = return_values[1];// estimated time to complete the program (cycle time)
2012out_nins_time_dist[2] = return_values[2];// estimated travel distance
2013}
2014 returnratio_ok;
2015}
2016
2017
2030 int Item::InstructionListJoints(QString &error_msg,tMatrix2D**joint_list,doublemm_step,doubledeg_step,constQString &save_to_file,boolcollision_check,intresult_flag,doubletime_step_s){
2031 _RDK->_check_connection();
2032 _RDK->_send_Line("G_ProgJointList");
2033 _RDK->_send_Item(this);
2034 doublestep_mm_deg[5] = { mm_step, deg_step, collision_check ? 1.0 : 0.0, (double) result_flag, time_step_s };
2035 _RDK->_send_Array(step_mm_deg, 5);
2036 _RDK->_TIMEOUT = 3600 * 1000;
2037 //joint_list = save_to_file;
2038 if(save_to_file.isEmpty()) {
2039 _RDK->_send_Line("");
2040 _RDK->_recv_Matrix2D(joint_list);
2041}else{
2042 _RDK->_send_Line(save_to_file);
2043joint_list =nullptr;
2044}
2045 interror_code =_RDK->_recv_Int();
2046 _RDK->_TIMEOUT = ROBODK_API_TIMEOUT;
2047error_msg =_RDK->_recv_Line();
2048 _RDK->_check_status();
2049 returnerror_code;
2050}
2051
2060QStringItem::setParam(constQString ¶m,constQString &value){
2061 _RDK->_check_connection();
2062 _RDK->_send_Line("ICMD");
2063 _RDK->_send_Item(this);
2064 _RDK->_send_Line(param);
2065 _RDK->_send_Line(value);
2066QString result =_RDK->_recv_Line();
2067 _RDK->_check_status();
2068 returnresult;
2069}
2070
2076 _RDK->Finish();
2077 return true;
2078}
2079
2080quint64Item::GetID(){
2081 return _PTR;
2082}
2083
2084 //---------------------------------------- add more
2085
2086
2087
2088
2089
2090
2091
2092
2093
2094
2095
2096
2097 //---------------------------------------------------------------------------------------------------
2098 //---------------------------------------------------------------------------------------------------
2099 //---------------------------------------------------------------------------------------------------
2100 //---------------------------------------------------------------------------------------------------
2102 RoboDK::RoboDK(constQString &robodk_ip,intcom_port,constQString &args,constQString &path) {
2103_COM =nullptr;
2104_IP = robodk_ip;
2105_TIMEOUT = ROBODK_API_TIMEOUT;
2106_PROCESS = 0;
2107_PORT = com_port;
2108_ROBODK_BIN = path;
2109 if(_PORT < 0){
2110_PORT = ROBODK_DEFAULT_PORT;
2111}
2112 if(_ROBODK_BIN.isEmpty()){
2113_ROBODK_BIN = ROBODK_DEFAULT_PATH_BIN;
2114}
2115_ARGUMENTS = args;
2116 if(com_port > 0){
2117_ARGUMENTS.append(" /PORT="+ QString::number(com_port));
2118}
2119_connect_smart();
2120}
2121
2122RoboDK::~RoboDK(){
2123_disconnect();
2124}
2125
2126quint64 RoboDK::ProcessID(){
2127 if(_PROCESS == 0) {
2128QString response =Command("MainProcess_ID");
2129_PROCESS = response.toULongLong();
2130}
2131 return_PROCESS;
2132}
2133
2134quint64 RoboDK::WindowID(){
2135qint64 window_id;
2136QString response =Command("MainWindow_ID");
2137window_id = response.toULongLong();
2138 returnwindow_id;
2139}
2140
2141 boolRoboDK::Connected(){
2142 return_connected();
2143}
2144
2145 boolRoboDK::Connect(){
2146 return_connect();
2147}
2153_disconnect();
2154}
2160 Disconnect();
2161}
2162
2163 // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2164 // public methods
2171 Item RoboDK::getItem(QString name,intitemtype){
2172_check_connection();
2173 if(itemtype < 0){
2174_send_Line("G_Item");
2175_send_Line(name);
2176}else{
2177_send_Line("G_Item2");
2178_send_Line(name);
2179_send_Int(itemtype);
2180}
2181 Itemitem = _recv_Item();
2182_check_status();
2183 returnitem;
2184}
2185
2192QStringListRoboDK::getItemListNames(intfilter){
2193_check_connection();
2194 if(filter < 0) {
2195_send_Line("G_List_Items");
2196}else{
2197_send_Line("G_List_Items_Type");
2198_send_Int(filter);
2199}
2200qint32 numitems = _recv_Int();
2201QStringList listnames;
2202 for(inti = 0; i < numitems; i++) {
2203listnames.append(_recv_Line());
2204}
2205_check_status();
2206 returnlistnames;
2207}
2208
2215QListRoboDK::getItemList(intfilter) {
2216_check_connection();
2217 if(filter < 0) {
2218_send_Line("G_List_Items_ptr");
2219}else{
2220_send_Line("G_List_Items_Type_ptr");
2221_send_Int(filter);
2222}
2223 intnumitems = _recv_Int();
2224QList listitems;
2225 for(inti = 0; i < numitems; i++) {
2226listitems.append(_recv_Item());
2227}
2228_check_status();
2229 returnlistitems;
2230}
2231
2233
2241 Item RoboDK::ItemUserPick(constQString &message,intitemtype) {
2242_check_connection();
2243_send_Line("PickItem");
2244_send_Line(message);
2245_send_Int(itemtype);
2246_TIMEOUT = 3600 * 1000;
2247 Itemitem = _recv_Item();//item);
2248_TIMEOUT = ROBODK_API_TIMEOUT;
2249_check_status();
2250 returnitem;
2251}
2252
2257_check_connection();
2258_send_Line("RAISE");
2259_check_status();
2260}
2261
2266_check_connection();
2267_send_Line("HIDE");
2268_check_status();
2269}
2270
2275_check_connection();
2276_send_Line("QUIT");
2277_check_status();
2278_disconnect();
2279_PROCESS = 0;
2280}
2281
2283{
2284_check_connection();
2285_send_Line("Version");
2286QString appName = _recv_Line();
2287 intbitArch = _recv_Int();
2288QString ver4 = _recv_Line();
2289QString dateBuild = _recv_Line();
2290_check_status();
2291 returnver4;
2292
2293}
2294
2295
2300 void RoboDK::setWindowState(intwindowstate){
2301_check_connection();
2302_send_Line("S_WindowState");
2303_send_Int(windowstate);
2304_check_status();
2305}
2306
2307
2312 void RoboDK::setFlagsRoboDK(intflags){
2313_check_connection();
2314_send_Line("S_RoboDK_Rights");
2315_send_Int(flags);
2316_check_status();
2317}
2318
2324 void RoboDK::setFlagsItem(Itemitem,intflags){
2325_check_connection();
2326_send_Line("S_Item_Rights");
2327_send_Item(item);
2328_send_Int(flags);
2329_check_status();
2330}
2331
2338_check_connection();
2339_send_Line("S_Item_Rights");
2340_send_Item(item);
2341 intflags = _recv_Int();
2342_check_status();
2343 returnflags;
2344}
2345
2351 void RoboDK::ShowMessage(constQString &message,boolpopup){
2352_check_connection();
2353 if(popup)
2354{
2355_send_Line("ShowMessage");
2356_send_Line(message);
2357_TIMEOUT = 3600 * 1000;
2358_check_status();
2359_TIMEOUT = ROBODK_API_TIMEOUT;
2360}
2361 else
2362{
2363_send_Line("ShowMessageStatus");
2364_send_Line(message);
2365_check_status();
2366}
2367
2368}
2369
2374 void RoboDK::Copy(const Item&tocopy){
2375_check_connection();
2376_send_Line("Copy");
2377_send_Item(tocopy);
2378_check_status();
2379}
2380
2386 Item RoboDK::Paste(const Item*paste_to){
2387_check_connection();
2388_send_Line("Paste");
2389_send_Item(paste_to);
2390 Itemnewitem = _recv_Item();
2391_check_status();
2392 returnnewitem;
2393}
2394
2401 Item RoboDK::AddFile(constQString &filename,const Item*parent){
2402_check_connection();
2403_send_Line("Add");
2404_send_Line(filename);
2405_send_Item(parent);
2406_TIMEOUT = 3600 * 1000;
2407 Itemnewitem = _recv_Item();
2408_TIMEOUT = ROBODK_API_TIMEOUT;
2409_check_status();
2410 returnnewitem;
2411}
2412
2418 void RoboDK::Save(constQString &filename,const Item*itemsave){
2419_check_connection();
2420_send_Line("Save");
2421_send_Line(filename);
2422_send_Item(itemsave);
2423_check_status();
2424}
2425
2438 Item RoboDK::AddShape(tMatrix2D*trianglePoints,Item*addTo,boolshapeOverride,Color*color)
2439{
2440 doublecolorArray[4] = {0.6,0.6,0.8,1.0};
2441 if(color !=nullptr){
2442colorArray[0] = color->r;
2443colorArray[1] = color->g;
2444colorArray[2] = color->b;
2445colorArray[3] = color->a;
2446}
2447_check_connection();
2448_send_Line("AddShape3");
2449_send_Matrix2D(特里安glePoints);
2450_send_Item(addTo);
2451_send_Int(shapeOverride? 1 : 0);
2452_send_Array(colorArray,4);
2453 Itemnewitem = _recv_Item();
2454_check_status();
2455 returnnewitem;
2456}
2457
2474 Item RoboDK::AddCurve(tMatrix2D*curvePoints,Item*referenceObject,booladdToRef,intProjectionType)
2475{
2476_check_connection();
2477_send_Line("AddWire");
2478_send_Matrix2D(curvePoints);
2479_send_Item(referenceObject);
2480_send_Int(addToRef ? 1:0);
2481_send_Int(ProjectionType);
2482 Itemnewitem = _recv_Item();
2483_check_status();
2484 returnnewitem;
2485}
2486
2495 Item RoboDK::AddPoints(tMatrix2D*points,Item*referenceObject,booladdToRef,intProjectionType)
2496{
2497_check_connection();
2498_send_Line("AddPoints");
2499_send_Matrix2D(points);
2500_send_Item(referenceObject);
2501_send_Int(addToRef? 1 : 0);
2502_send_Int(ProjectionType);Itemnewitem = _recv_Item();
2503_check_status();
2504 returnnewitem;
2505}
2506
2507 void RoboDK::ProjectPoints(tMatrix2D*points,tMatrix2D**projected,ItemobjectProject,intProjectionType)
2508{
2509_check_connection();
2510_send_Line("ProjectPoints");
2511_send_Matrix2D(points);
2512_send_Item(objectProject);
2513_send_Int(ProjectionType);
2514_recv_Matrix2D(projected);
2515_check_status();
2516}
2517
2523 Item RoboDK::AddStation(constQString &name)
2524{
2525_check_connection();
2526_send_Line("NewStation");
2527_send_Line(name);
2528 Itemnewitem = _recv_Item();
2529_check_status();
2530 returnnewitem;
2531}
2532
2533
2534
2535
2540_check_connection();
2541_send_Line("RemoveStn");
2542_check_status();
2543}
2544
2552 Item RoboDK::AddTarget(constQString &name,Item*itemparent,Item*itemrobot){
2553_check_connection();
2554_send_Line("Add_TARGET");
2555_send_Line(name);
2556_send_Item(itemparent);
2557_send_Item(itemrobot);
2558 Itemnewitem = _recv_Item();
2559_check_status();
2560 returnnewitem;
2561}
2562
2569 Item RoboDK::AddFrame(constQString &name,Item*itemparent){
2570_check_connection();
2571_send_Line("Add_FRAME");
2572_send_Line(name);
2573_send_Item(itemparent);
2574 Itemnewitem = _recv_Item();
2575_check_status();
2576 returnnewitem;
2577}
2578
2585 Item RoboDK::AddProgram(constQString &name,Item*itemrobot){
2586_check_connection();
2587_send_Line("Add_PROG");
2588_send_Line(name);
2589_send_Item(itemrobot);
2590 Itemnewitem = _recv_Item();
2591_check_status();
2592 returnnewitem;
2593}
2594
2603 Item RoboDK::AddMachiningProject(constQString &name,Item*itemrobot)
2604{
2605_check_connection();
2606_send_Line("Add_MACHINING");
2607_send_Line(name);
2608_send_Item(itemrobot);
2609 Itemnewitem = _recv_Item();
2610_check_status();
2611 returnnewitem;
2612}
2613
2614
2615
2617{
2618_check_connection();
2619_send_Line("G_AllStn");
2620 intnstn = _recv_Int();
2621QList *listStn =newQList();
2622 for(inti = 0;i < nstn;i++){
2623 Itemstation = _recv_Item();
2624listStn->append(station);
2625}
2626_check_status();
2627 return*listStn;
2628}
2629
2630
2636_check_connection();
2637_send_Line("G_ActiveStn");
2638 Itemstation = _recv_Item();
2639_check_status();
2640 returnstation;
2641}
2642
2648_check_connection();
2649_send_Line("S_ActiveStn");
2650_send_Item(station);
2651_check_status();
2652}
2653
2654
2655 //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
2661 int RoboDK::RunProgram(constQString &function_w_params){
2662 return RunCode(function_w_params,true);
2663}
2664
2671 int RoboDK::RunCode(constQString &code,boolcode_is_fcn_call){
2672_check_connection();
2673_send_Line("RunCode");
2674_send_Int(code_is_fcn_call ? 1 : 0);
2675_send_Line(code);
2676 intprog_status = _recv_Int();
2677_check_status();
2678 returnprog_status;
2679}
2680
2686 void RoboDK::RunMessage(constQString &message,boolmessage_is_comment){
2687_check_connection();
2688_send_Line("RunMessage");
2689_send_Int(message_is_comment ? 1 : 0);
2690_send_Line(message);
2691_check_status();
2692}
2693
2698 void RoboDK::Render(boolalways_render){
2699 boolauto_render = !always_render;
2700_check_connection();
2701_send_Line("Render");
2702_send_Int(auto_render ? 1 : 0);
2703_check_status();
2704}
2705
2711{
2712_check_connection();
2713_send_Line("Refresh");
2714_send_Int(0);
2715_check_status();
2716}
2717
2724 bool RoboDK::IsInside(Itemobject_inside,Itemobject_parent){
2725_check_connection();
2726_send_Line("IsInside");
2727_send_Item(object_inside);
2728_send_Item(object_parent);
2729 intinside = _recv_Int();
2730_check_status();
2731 returninside > 0;
2732}
2733
2739 int RoboDK::setCollisionActive(intcheck_state){
2740_check_connection();
2741_send_Line("Collision_SetState");
2742_send_Int(check_state);
2743 intncollisions = _recv_Int();
2744_check_status();
2745 returnncollisions;
2746}
2747
2758 bool RoboDK::setCollisionActivePair(intcheck_state,Itemitem1,Itemitem2,intid1,intid2){
2759_check_connection();
2760_send_Line("Collision_SetPair");
2761_send_Item(item1);
2762_send_Item(item2);
2763_send_Int(id1);
2764_send_Int(id2);
2765_send_Int(check_state);
2766 intsuccess = _recv_Int();
2767_check_status();
2768 return成功> 0;
2769}
2770
2776_check_connection();
2777_send_Line("Collisions");
2778 intncollisions = _recv_Int();
2779_check_status();
2780 returnncollisions;
2781}
2782
2789 int RoboDK::Collision(Itemitem1,Itemitem2){
2790_check_connection();
2791_send_Line("Collided");
2792_send_Item(item1);
2793_send_Item(item2);
2794 intncollisions = _recv_Int();
2795_check_status();
2796 returnncollisions;
2797}
2798
2799QListRoboDK::getCollisionItems(QList link_id_list)
2800{
2801_check_connection();
2802_send_Line("Collision Items");
2803 intnitems = _recv_Int();
2804QList itemList = QList();
2805 if(!link_id_list.isEmpty()){
2806link_id_list.clear();
2807}
2808 for(inti = 0; i < nitems; i++){
2809itemList.append(_recv_Item());
2810 intlinkId = _recv_Int();
2811 if(!link_id_list.isEmpty()){
2812link_id_list.append(linkId);
2813}
2814 intcollisionTimes = _recv_Int();
2815}
2816_check_status();
2817 returnitemList;
2818}
2819
2824 void RoboDK::setSimulationSpeed(doublespeed){
2825_check_connection();
2826_send_Line("SimulateSpeed");
2827_send_Int((int)(speed * 1000.0));
2828_check_status();
2829}
2830
2836_check_connection();
2837_send_Line("GetSimulateSpeed");
2838 doublespeed = ((double)_recv_Int()) / 1000.0;
2839_check_status();
2840 returnspeed;
2841}
2852 void RoboDK::setRunMode(intrun_mode){
2853_check_connection();
2854_send_Line(“S_RunMode”);
2855_send_Int(run_mode);
2856_check_status();
2857}
2858
2868_check_connection();
2869_send_Line("G_RunMode");
2870 intrunmode = _recv_Int();
2871_check_status();
2872 returnrunmode;
2873}
2881_check_connection();
2882_send_Line("G_Params");
2883QList> paramlist;
2884 intnparam = _recv_Int();
2885 for(inti = 0; i < nparam; i++) {
2886QString param = _recv_Line();
2887QString value = _recv_Line();
2888paramlist.append(qMakePair(param, value));
2889}
2890_check_status();
2891 returnparamlist;
2892}
2893
2905QStringRoboDK::getParam(constQString ¶m){
2906_check_connection();
2907_send_Line("G_Param");
2908_send_Line(param);
2909QString value = _recv_Line();
2910 if(value.startsWith("UNKNOWN ")) {
2911value ="";
2912}
2913_check_status();
2914 returnvalue;
2915}
2916
2924 void RoboDK::setParam(constQString ¶m,constQString &value){
2925_check_connection();
2926_send_Line("S_Param");
2927_send_Line(param);
2928_send_Line(价值);
2929_check_status();
2930}
2931
2938QStringRoboDK::Command(constQString &cmd,constQString &value){
2939_check_connection();
2940_send_Line("SCMD");
2941_send_Line(cmd);
2942_send_Line(价值);
2943QString answer = _recv_Line();
2944_check_status();
2945 returnanswer;
2946}
2947
2948 bool RoboDK::LaserTrackerMeasure(tXYZxyz,tXYZestimate,boolsearch)
2949{
2950_check_connection();
2951_send_Line("MeasLT");
2952_send_XYZ(estimate);
2953_send_Int(search ? 1 : 0);
2954_recv_XYZ(xyz);
2955_check_status();
2956 if(xyz[0]*xyz[0] + xyz[1]*xyz[1] + xyz[2]*xyz[2] < 0.0001){
2957 return false;
2958}
2959
2960 return true;
2961}
2962
2963 void RoboDK::ShowAsCollided(QList itemList, QList collidedList, QList *robot_link_id)
2964{
2965 intnitems = qMin(itemList.length(),collidedList.length());
2966 if(robot_link_id !=nullptr){
2967nitems = qMin(nitems, robot_link_id->length());
2968}
2969_check_connection();
2970_send_Line("ShowAsCollidedList");
2971_send_Int(nitems);
2972 for(inti = 0; i < nitems; i++){
2973_send_Item(itemList[i]);
2974_send_Int(collidedList[i] ? 1 : 0);
2975 intlink_id = 0;
2976 if(robot_link_id !=nullptr){
2977link_id = robot_link_id->at(i);
2978}
2979_send_Int(link_id);
2980}
2981_check_status();
2982}
2983
2984 //---------------------------------------------- ADD MORE (getParams, setParams, calibrate TCP, calibrate ref...)
2985
2986
2997 void RoboDK::CalibrateTool(tMatrix2D*poses_joints,tXYZtcp_xyz,intformat,intalgorithm,Item*robot,double*error_stats){
2998_check_connection();
2999_send_Line("CalibTCP2");
3000_send_Matrix2D(poses_joints);
3001_send_Int(format);
3002_send_Int(algorithm);
3003_send_Item(robot);
3004 intnxyz = 3;
3005_recv_Array(tcp_xyz, &nxyz);
3006 if(error_stats !=nullptr){
3007_recv_Array(error_stats);
3008}else{
3009 doubleerrors_ignored[20];
3010_recv_Array(errors_ignored);
3011}
3012 tMatrix2D* error_graph =Matrix2D_Create();
3013_recv_Matrix2D(&error_graph);
3014 Matrix2D_Delete(&error_graph);
3015_check_status();
3016}
3017
3026 Mat RoboDK::CalibrateReference(tMatrix2D*poses_joints,intmethod,booluse_joints,Item*robot){
3027_check_connection();
3028_send_Line("CalibFrame");
3029_send_Matrix2D(poses_joints);
3030_send_Int(use_joints ? -1 : 0);
3031_send_Int(method);
3032_send_Item(robot);
3033 Matreference_pose = _recv_Pose();
3034 doubleerror_stats[20];
3035_recv_Array(error_stats);
3036_check_status();
3037 returnreference_pose;
3038}
3039
3040
3041 int RoboDK::ProgramStart(constQString &progname,constQString &defaultfolder,constQString &postprocessor,Item*robot){
3042_check_connection();
3043_send_Line("ProgramStart");
3044_send_Line(progname);
3045_send_Line(defaultfolder);
3046_send_Line(postprocessor);
3047_send_Item(robot);
3048 interrors = _recv_Int();
3049_check_status();
3050 returnerrors;
3051}
3052
3057 void RoboDK::setViewPose(const Mat&pose){
3058_check_connection();
3059_send_Line("S_ViewPose");
3060_send_Pose(pose);
3061_check_status();
3062}
3063
3069_check_connection();
3070_send_Line("G_ViewPose");
3071 Matpose = _recv_Pose();
3072_check_status();
3073 returnpose;
3074}
3075
3076 //INCOMPLETE function!
3077 /*bool RoboDK::SetRobotParams(Item *robot, tMatrix2D dhm, Mat poseBase, Mat poseTool)
3078 {
3079 _check_connection();
3080 _send_Line("S_AbsAccParam");
3081 _send_Item(robot);
3082 Mat r2b;
3083 r2b.setToIdentity();
3084 _send_Pose(r2b);
3085 _send_Pose(poseBase);
3086 _send_Pose(poseTool);
3087 int *ndofs = dhm.size;
3088 _send_Int(*ndofs);
3089 for (int i = 0; i < *ndofs; i++){
3090 _send_Array(dhm);
3091 }
3092
3093 _send_Pose(poseBase);
3094 _send_Pose(poseTool);
3095 _send_Int(*ndofs);
3096 for (int i = 0; i < *ndofs; i++){
3097 _send_Array(dhm[i]);
3098 }
3099
3100 _send_Array(nullptr);
3101 _send_Array(nullptr);
3102 _check_status();
3103 return true;
3104 }*/
3105
3106 Item RoboDK::Cam2D_Add(const Item&item_object,constQString &cam_params,const Item*cam_item){
3107_check_connection();
3108_send_Line("Cam2D_PtrAdd");
3109_send_Item(item_object);
3110_send_Item(cam_item);
3111_send_Line (cam_params);
3112 Itemcam_item_return = _recv_Item();
3113_check_status();
3114 returncam_item_return;
3115}
3116 int RoboDK::Cam2D_Snapshot(constQString &file_save_img,const Item&cam_item,constQString ¶ms){
3117_check_connection();
3118_send_Line("Cam2D_PtrSnapshot");
3119_send_Item(cam_item);
3120_send_Line(file_save_img);
3121_send_Line(params);
3122_TIMEOUT = 3600 * 1000;
3123 intstatus = _recv_Int();
3124_TIMEOUT = ROBODK_API_TIMEOUT;
3125_check_status();
3126 returnstatus;
3127}
3128
3129 int RoboDK::Cam2D_SetParams(constQString ¶ms,const Item&cam_item){
3130_check_connection();
3131_send_Line("Cam2D_PtrSetParams");
3132_send_Item(cam_item);
3133_send_Line(params);
3134 intstatus = _recv_Int();
3135_check_status();
3136 returnstatus;
3137}
3138
3139 Item RoboDK::getCursorXYZ(intx,inty,tXYZxyzStation)
3140{
3141_check_connection();
3142_send_Line("Proj2d3d");
3143_send_Int(x);
3144_send_Int(y);
3145 intselection = _recv_Int();
3146 ItemselectedItem = _recv_Item();
3147 tXYZxyz;
3148_recv_XYZ(xyz);
3149 if(xyzStation !=nullptr){
3150xyzStation[0] = xyz[0];
3151xyzStation[1] = xyz[1];
3152xyzStation[2] = xyz[2];
3153}
3154_check_status();
3155 returnselectedItem;
3156}
3157
3158
3160
3161
3167_check_connection();
3168_send_Line("G_License");
3169QString license = _recv_Line();
3170_check_status();
3171 returnlicense;
3172}
3173
3179_check_connection();
3180_send_Line("G_Selection");
3181 intnitems = _recv_Int();
3182QList list_items;
3183 for(inti = 0; i < nitems; i++) {
3184list_items.append(_recv_Item());
3185}
3186_check_status();
3187 returnlist_items;
3188}
3189
3194 void RoboDK::setSelection(QList list_items){
3195_check_connection();
3196_send_Line("S_Selection");
3197_send_Int(list_items.length());
3198 for(inti = 0; i < list_items.length(); i++) {
3199_send_Item(list_items[i]);
3200}
3201_check_status();
3202}
3203
3204
3208 void RoboDK::PluginLoad(constQString &pluginName,intload){
3209_check_connection();
3210_send_Line("PluginLoad");
3211_send_Line(pluginName);
3212_send_Int(load);
3213_check_status();
3214}
3215
3219QStringRoboDK::PluginCommand(constQString &pluginName,constQString &pluginCommand,constQString &pluginValue){
3220_check_connection();
3221_send_Line("PluginCommand");
3222_send_Line(pluginName);
3223_send_Line(pluginCommand);
3224_send_Line(pluginValue);
3225QString result = _recv_Line();
3226_check_status();
3227 returnresult;
3228}
3229
3230
3235 Item RoboDK::Popup_ISO9283_CubeProgram(Item*robot,tXYZcenter,doubleside,boolblocking){
3236 //_require_build(5177);
3237 Itemiso_program;
3238_check_connection();
3239 if(center ==nullptr){
3240_send_Line("Popup_ProgISO9283");
3241_send_Item(robot);
3242_TIMEOUT = 3600 * 1000;
3243iso_program = _recv_Item();
3244_TIMEOUT = ROBODK_API_TIMEOUT;
3245_check_status();
3246}else{
3247_send_Line("Popup_ProgISO9283_Param");
3248_send_Item(robot);
3249 doublevalues[5];
3250values[0] = center[0];
3251values[1] = center[1];
3252values[2] = center[2];
3253values[3] = side;
3254values[4] = blocking ? 1 : 0;
3255_send_Array(values, 4);
3256 if(blocking){
3257_TIMEOUT = 3600 * 1000;
3258iso_program = _recv_Item();
3259_TIMEOUT = ROBODK_API_TIMEOUT;
3260_check_status();
3261}
3262}
3263 returniso_program;
3264}
3265
3266
3267
3268
3269 bool RoboDK::FileSet(constQString &path_file_local,constQString &file_remote,boolload_file,Item*attach_to){
3270 if(!_check_connection()){return false; }
3271 if(!_send_Line("FileRecvBin")){return false; }
3272QFile文件(path_file_local);
3273 if(!_send_Line(file_remote)){return false; }
3274 intnbytes = file.size();
3275 if(!_send_Int(nbytes)){return false; }
3276 if(!_send_Item(attach_to)){return false; }
3277 if(!_send_Int(load_file ? 1 : 0)){return false; }
3278 if(!_check_status()){return false; }
3279 intsz_sent = 0;
3280 if(!file.open(QFile::ReadOnly)){
3281 return false;
3282}
3283 while(true){
3284QByteArray buffer(file.read(1024));
3285 if(buffer.size() == 0){
3286 break;
3287}
3288 // warning! Nothing guarantees that all bytes are sent
3289sz_sent += _COM->write(buffer);
3290qDebug() <<"Sending file "<< path_file_local << 100*sz_sent/nbytes;
3291}
3292file.close();
3293 return true;
3294}
3295
3296 bool RoboDK::FileGet(constQString &path_file_local,Item*station,constQString path_file_remote){
3297 if(!_check_connection()){return false; }
3298 if(!_send_Line("FileSendBin")){return false; }
3299 if(!_send_Item(station)){return false; }
3300 if(!_send_Line(path_file_remote)){return false; }
3301 intnbytes = _recv_Int();
3302 intremaining = nbytes;
3303QFile文件(path_file_local);
3304 if(!file.open(QFile::WriteOnly)){
3305qDebug() <<"Can not open file for writting "<< path_file_local;
3306 return false;
3307}
3308 while(remaining > 0){
3309QByteArray buffer(_COM->read(qMin(remaining, 1024)));
3310remaining -= buffer.size();
3311file.write(buffer);
3312}
3313file.close();
3314 if(!_check_status()){return false;}
3315 return true;
3316}
3317
3318
3319 boolRoboDK::EmbedWindow(QString window_name, QString docked_name,intsize_w,intsize_h, quint64 pid,intarea_add,intarea_allowed,inttimeout)
3320{
3321 if(!_check_connection()){return false; }
3322 if(docked_name =="") {
3323docked_name = window_name;
3324}
3325_check_connection();
3326_send_Line("WinProcDock");
3327_send_Line(docked_name);
3328_send_Line(window_name);
3329 doublesizeArray[2] = {(double)size_w, (double)size_h};
3330_send_Array(sizeArray,2);
3331_send_Line(QString::number(pid));
3332_send_Int(area_allowed);
3333_send_Int(area_add);
3334_send_Int(timeout);
3335 intresult = _recv_Int();
3336_check_status();
3337 returnresult > 0;
3338}
3339
3340
3341 boolRoboDK::EventsListen()
3342{
3343_COM_EVT =newQTcpSocket();
3344 if(_IP.isEmpty()){
3345_COM_EVT->connectToHost("127.0.0.1", _PORT);//QHostAddress::LocalHost, _PORT);
3346}else{
3347_COM_EVT->connectToHost(_IP, _PORT);
3348}
3349qDebug() << _COM_EVT->state();
3350_COM_EVT->waitForConnected(_TIMEOUT);
3351qDebug() << _COM_EVT->state();
3352_send_Line("RDK_EVT", _COM_EVT);
3353_send_Int(0, _COM_EVT);
3354QString response = _recv_Line(_COM_EVT);
3355qDebug() << response;
3356 intver_evt = _recv_Int(_COM_EVT);
3357 intstatus = _recv_Int(_COM_EVT);
3358 if(response !="RDK_EVT"|| status != 0)
3359{
3360 return false;
3361}
3362 //_COM_EVT.ReceiveTimeout = 3600 * 1000;
3363 //return EventsLoop();
3364 return true;
3365}
3366
3367 boolRoboDK::WaitForEvent(int&evt, Item &itm)
3368{
3369evt = _recv_Int(_COM_EVT);
3370itm = _recv_Item(_COM_EVT);
3371 return true;
3372}
3373
3374 //Receives 24 doubles of data from the event loop
3375 boolRoboDK::Event_Receive_3D_POS(double*data,int*valueCount) {
3376 return_recv_Array(data,valueCount,_COM_EVT);
3377}
3378
3379 //Receives the 3 bytes of mouse data
3380 boolRoboDK::Event_Receive_Mouse_data(int*data) {
3381data[0] = _recv_Int(_COM_EVT);
3382data[1] = _recv_Int(_COM_EVT);
3383data[2] = _recv_Int(_COM_EVT);
3384
3385 return true;
3386}
3387
3388 boolRoboDK::Event_Receive_Event_Moved(Mat *pose_rel_out) {
3389 intnvalues = _recv_Int(_COM_EVT);
3390Mat pose_rel = _recv_Pose(_COM_EVT);
3391*pose_rel_out = pose_rel;
3392 if(nvalues > 16)
3393{
3394 return false;
3395 // future compatibility
3396}
3397
3398 return true;
3399}
3400
3401 boolRoboDK::Event_Connected()
3402{
3403 return_COM_EVT !=nullptr&& _COM_EVT->state() == QTcpSocket::ConnectedState;
3404}
3405
3406 //-------------------------- private ---------------------------------------
3407
3408 boolRoboDK::_connected(){
3409 return_COM !=nullptr&& _COM->state() == QTcpSocket::ConnectedState;
3410}
3411
3412
3413 boolRoboDK::_check_connection(){
3414 if(_connected()){
3415 return true;
3416}
3417 boolconnection_ok = _connect_smart();
3418 //if (!connection_ok){
3419 // throw -1;
3420 //}
3421 returnconnection_ok;
3422}
3423
3424 boolRoboDK::_check_status(){
3425qint32 status = _recv_Int();
3426 if(status == 0) {
3427 // everything is OK
3428 / /状态=状态
3429}else if(status > 0 && status < 10) {
3430QString strproblems("Unknown error");
3431 if(status == 1) {
3432strproblems ="Invalid item provided: The item identifier provided is not valid or it does not exist.";
3433}else if(status == 2) {//output warning only
3434strproblems = _recv_Line();
3435qDebug() <<"RoboDK API WARNING: "<< strproblems;
3436 return0;
3437}else if(status == 3){// output error
3438strproblems = _recv_Line();
3439qDebug() <<"RoboDK API ERROR: "<< strproblems;
3440}else if(status == 9) {
3441qDebug() <<"Invalid RoboDK License";
3442}
3443 //print(strproblems);
3444 //throw new RDKException(strproblems); //raise Exception(strproblems)
3445}else if(status < 100){
3446QString strproblems = _recv_Line();
3447qDebug() <<"RoboDK API ERROR: "<< strproblems;
3448}else{
3449 //throw new RDKException("Communication problems with the RoboDK API"); //raise Exception('Problems running function');
3450qDebug() <<"Communication problems with the RoboDK API";
3451}
3452 returnstatus;
3453}
3454
3455
3456
3457 voidRoboDK::_disconnect(){
3458 if(_COM !=nullptr){
3459_COM->deleteLater();
3460_COM =nullptr;
3461}
3462}
3463
3464 // attempt a simple connection to RoboDK and start RoboDK if it is not running
3465 boolRoboDK: _connect_smart (){
3466 //Establishes a connection with robodk. robodk must be running, otherwise, it will attempt to start it
3467 if(_connect()){
3468qDebug() <<"The RoboDK API is connected";
3469 return true;
3470}
3471
3472qDebug() <<"...Trying to start RoboDK: "<< _ROBODK_BIN <<" "<< _ARGUMENTS;
3473 // Start RoboDK
3474QProcess *p =newQProcess();
3475 //_ARGUMENTS = "/DEBUG";
3476p->start(_ROBODK_BIN, _ARGUMENTS.split(" ", QString::SkipEmptyParts));
3477p->setReadChannel(QProcess::StandardOutput);
3478 //p->waitForReadyRead(10000);
3479_PROCESS = p->processId();
3480 while(p->canReadLine() || p->waitForReadyRead(5000)){
3481QString line = QString::fromUtf8(p->readLine().trimmed());
3482 //qDebug() << "RoboDK process: " << line;
3483 if(line.contains("Running", Qt::CaseInsensitive)){
3484qDebug() <<"RoboDK is Running... Connecting API";
3485 boolis_connected = _connect();
3486 if(is_connected) {
3487qDebug() <<"The RoboDK API is connected";
3488}else{
3489qDebug() <<"The RoboDK API is NOT connected!";
3490}
3491 returnis_connected;
3492}
3493}
3494qDebug() <<"Could not start RoboDK!";
3495 return false;
3496}
3497
3498 // attempt a simple connection to RoboDK
3499 boolRoboDK::_connect(){
3500_disconnect();
3501_COM =newQTcpSocket();
3502 if(_IP.isEmpty()){
3503_COM->connectToHost("127.0.0.1", _PORT);//QHostAddress::LocalHost, _PORT);
3504}else{
3505_COM->connectToHost(_IP, _PORT);
3506}
3507 // usually, 5 msec should be enough for localhost
3508 if(!_COM->waitForConnected(_TIMEOUT)){
3509_COM->deleteLater();
3510_COM =nullptr;
3511 return false;
3512}
3513
3514 // RoboDK protocol to check that we are connected to the right port
3515_COM->write(ROBODK_API_START_STRING); _COM->write(ROBODK_API_LF, 1);
3516_COM->write("1 0"); _COM->write(ROBODK_API_LF, 1);
3517
3518 // 5 msec should be enough for localhost
3519 /*if (!_COM->waitForBytesWritten(_TIMEOUT)){
3520 _COM->deleteLater();
3521 _COM = nullptr;
3522 return false;
3523 }*/
3524 // 10 msec should be enough for localhost
3525 if(!_COM->canReadLine() && !_COM->waitForReadyRead(_TIMEOUT)){
3526_COM->deleteLater();
3527_COM =nullptr;
3528 return false;
3529}
3530QString读(_COM - > readAll ());
3531 // make sure we receive the OK from RoboDK
3532 if(!read.startsWith(ROBODK_API_READY_STRING)){
3533_COM->deleteLater();
3534_COM =nullptr;
3535 return false;
3536}
3537 return true;
3538}
3539
3540
3542 boolRoboDK::_waitline(QTcpSocket *com){
3543 if(com ==nullptr) {
3544com = _COM;
3545}
3546 if(com ==nullptr){return false; }
3547 while(!com->canReadLine()){
3548 if(!com->waitForReadyRead(_TIMEOUT)){
3549 return false;
3550}
3551}
3552 return true;
3553}
3554QString RoboDK::_recv_Line(QTcpSocket *com){//QString &string){
3555 if(com ==nullptr) {
3556com = _COM;
3557}
3558QString string;
3559 if(!_waitline(com)){
3560 if(com !=nullptr){
3561 //if this happens it means that there are problems: delete buffer
3562com->readAll();
3563}
3564 returnstring;
3565}
3566QByteArray line = _COM->readLine().trimmed();//remove last character \n //.trimmed();
3567 string.append(QString::fromUtf8(line));
3568 returnstring;
3569}
3570 boolRoboDK::_send_Line(constQString&string,QTcpSocket *com){
3571 if(com ==nullptr) {
3572com = _COM;
3573}
3574 if(com ==nullptr|| !com->isOpen()){return false; }
3575com->write(string.toUtf8());
3576com->write(ROBODK_API_LF, 1);
3577 return true;
3578}
3579
3580 intRoboDK::_recv_Int(QTcpSocket *com){//qint32 &value){
3581 if(com ==nullptr){
3582com = _COM;
3583}
3584qint32 value;// do not change type
3585 if(com ==nullptr){return false; }
3586 if(com->bytesAvailable() <sizeof(qint32)){
3587com->waitForReadyRead(_TIMEOUT);
3588 if(com->bytesAvailable() <sizeof(qint32)){
3589 return-1;
3590}
3591}
3592QDataStream ds(com);
3593ds >> value;
3594 returnvalue;
3595}
3596 boolRoboDK::_send_Int(qint32 value,QTcpSocket *com){
3597 if(com ==nullptr) {
3598com = _COM;
3599}
3600 if(com ==nullptr|| !com->isOpen()){return false; }
3601QDataStream ds(com);
3602ds << value;
3603 return true;
3604}
3605
3606Item RoboDK::_recv_Item(QTcpSocket *com){//Item *item){
3607 if(com ==nullptr) {
3608com = _COM;
3609}
3610Item item(this);
3611 if(com ==nullptr){returnitem; }
3612item._PTR = 0;
3613item._TYPE = -1;
3614 if(com->bytesAvailable() <sizeof(quint64)){
3615com->waitForReadyRead(_TIMEOUT);
3616 if(com->bytesAvailable() <sizeof(quint64)){
3617 returnitem;
3618}
3619}
3620QDataStream ds(com);
3621ds >> item._PTR;
3622ds >> item._TYPE;
3623 returnitem;
3624}
3625 boolRoboDK::_send_Item(constItem *item){
3626 if(_COM ==nullptr|| !_COM->isOpen()){return false; }
3627QDataStream ds(_COM);
3628quint64 ptr = 0;
3629 if(item !=nullptr){
3630ptr = item->_PTR;
3631}
3632ds << ptr;
3633 return true;
3634}
3635 boolRoboDK::_send_Item(const项目项目){
3636 return_send_Item(&item);
3637}
3638
3639Mat RoboDK::_recv_Pose(QTcpSocket *com){//Mat &pose){
3640 if(com ==nullptr) {
3641com = _COM;
3642}
3643
3644Mat pose;
3645 if(com ==nullptr){returnpose; }
3646 intsize = 16*sizeof(double);
3647 if(com->bytesAvailable() < size){
3648com->waitForReadyRead(_TIMEOUT);
3649 if(com->bytesAvailable() < size){
3650 returnpose;
3651}
3652}
3653QDataStream ds(com);
3654ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3655 //ds.setByteOrder(QDataStream::LittleEndian);
3656 doublevaluei;
3657 for(intj=0; j<4; j++){
3658 for(inti=0; i<4; i++){
3659ds >> valuei;
3660pose.Set(i,j,valuei);
3661 //pose.data()[i][j] = valuei;
3662}
3663}
3664 returnpose;
3665}
3666 boolRoboDK::_send_Pose(constMat &pose){
3667 if(_COM ==nullptr|| !_COM->isOpen()){return false; }
3668QDataStream ds(_COM);
3669ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3670 //ds.setByteOrder(QDataStream::LittleEndian);
3671 doublevaluei;
3672 for(intj=0; j<4; j++){
3673 for(inti=0; i<4; i++){
3674valuei = pose.Get(i,j);
3675ds << valuei;
3676}
3677}
3678 return true;
3679}
3680 boolRoboDK::_recv_XYZ(tXYZpos){
3681 if(_COM ==nullptr){return false; }
3682 intsize = 3*sizeof(double);
3683 if(_COM->bytesAvailable() < size){
3684_COM->waitForReadyRead(_TIMEOUT);
3685 if(_COM->bytesAvailable() < size){
3686 return false;
3687}
3688}
3689QDataStream ds(_COM);
3690ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3691 //ds.setByteOrder(QDataStream::LittleEndian);
3692 doublevaluei;
3693 for(inti=0; i<3; i++){
3694ds >> valuei;
3695pos (i] = valuei;
3696}
3697 return true;
3698}
3699 boolRoboDK::_send_XYZ(const tXYZpos){
3700 if(_COM ==nullptr|| !_COM->isOpen()){return false; }
3701QDataStream ds(_COM);
3702ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3703 //ds.setByteOrder(QDataStream::LittleEndian);
3704 doublevaluei;
3705 for(inti=0; i<3; i++){
3706valuei = pos[i];
3707ds << valuei;
3708}
3709 return true;
3710}
3711 boolRoboDK::_recv_Array(tJoints *jnts){
3712 return_recv_Array(jnts->_Values, &(jnts->_nDOFs));
3713}
3714 boolRoboDK::_send_Array(consttJoints *jnts){
3715 if(jnts ==nullptr){
3716 return_send_Int(0);
3717}
3718 return_send_Array(jnts->_Values, jnts->_nDOFs);
3719}
3720 boolRoboDK::_send_Array(constMat *mat){
3721 if(mat ==nullptr){
3722 return_send_Int(0);
3723}
3724 doublem44[16];
3725 for(intc=0; c<4; c++){
3726 for(intr=0; r<4; r++){
3727m44[c*4+r] = mat->Get(r,c);
3728}
3729}
3730 return_send_Array(m44, 16);
3731}
3732 boolRoboDK::_recv_Array(double*values,int*psize, QTcpSocket *com){
3733 if(com ==nullptr) {
3734com = _COM;
3735}
3736 intnvalues = _recv_Int();
3737 if(com ==nullptr|| nvalues < 0) {return false;}
3738 if(psize !=nullptr){
3739*psize = nvalues;
3740}
3741 if(nvalues < 0 || nvalues > 50){return false;}//check if the value is not too big
3742 intsize = nvalues*sizeof(double);
3743 if(com->bytesAvailable() < size){
3744com->waitForReadyRead(_TIMEOUT);
3745 if(com->bytesAvailable() < size){
3746 return false;
3747}
3748}
3749QDataStream ds(com);
3750ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3751 //ds.setByteOrder(QDataStream::LittleEndian);
3752 doublevaluei;
3753 for(inti=0; i
3754ds >> valuei;
3755values[i] = valuei;
3756}
3757 return true;
3758}
3759
3760 boolRoboDK::_send_Array(const double*values,intnvalues){
3761 if(_COM ==nullptr|| !_COM->isOpen()){return false; }
3762 if(!_send_Int((qint32)nvalues)){return false; }
3763QDataStream ds(_COM);
3764ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3765 //ds.setByteOrder(QDataStream::LittleEndian);
3766 doublevaluei;
3767 for(inti=0; i
3768valuei = values[i];
3769ds << valuei;
3770}
3771 return true;
3772}
3773
3774 boolRoboDK::_recv_Matrix2D(tMatrix2D **mat){// needs to delete after!
3775qint32 dim1 = _recv_Int();
3776qint32 dim2 = _recv_Int();
3777*mat =Matrix2D_Create();
3778 //emxInit_real_T(mat, 2);
3779 if(dim1 < 0 || dim2 < 0){return false; }
3780 Matrix2D_Set_Size(*mat, dim1, dim2);
3781 if(dim1*dim2 <= 0){
3782 return true;
3783}
3784QByteArray buffer;
3785 intcount = 0;
3786 doublevalue;
3787 while(true){
3788 intremaining = dim1*dim2 - count;
3789 if(remaining <= 0){return true; }
3790 if(_COM->bytesAvailable() <= 0 && !_COM->waitForReadyRead(_TIMEOUT)){
3791 Matrix2D_Delete(mat);
3792 return false;
3793}
3794buffer.append(_COM->read(remaining *sizeof(double) - buffer.size()));
3795 intnp = buffer.size() /sizeof(double);
3796QDataStream indata(buffer);
3797indata.setFloatingPointPrecision(QDataStream::DoublePrecision);
3798 for(inti=0; i
3799indata >> value;
3800(*mat)->data[count] = value;
3801count = count + 1;
3802}
3803buffer = buffer.mid(np *sizeof(double));
3804}
3805 return false;// we should never arrive here...
3806}
3807 boolRoboDK::_send_Matrix2D(tMatrix2D *mat){
3808 if(_COM ==nullptr|| !_COM->isOpen()){return false; }
3809QDataStream ds(_COM);
3810ds.setFloatingPointPrecision(QDataStream::DoublePrecision);
3811 //ds.setByteOrder(QDataStream::LittleEndian);
3812qint32 dim1 =Matrix2D_Size(mat, 1);
3813qint32 dim2 =Matrix2D_Size(mat, 2);
3814 boolok1 = _send_Int(dim1);
3815 boolok2 = _send_Int(dim2);
3816 if(!ok1 || !ok2) {return false; }
3817 doublevaluei;
3818 for(intj=0; j
3819 for(inti=0; i
3820valuei =Matrix2D_Get_ij(mat, i, j);
3821ds << valuei;
3822}
3823}
3824 return true;
3825}
3826 // private move type, to be used by public methods (MoveJ and MoveL)
3827 voidRoboDK::_moveX(constItem *target,consttJoints *joints,constMat *mat_target,constItem *itemrobot,intmovetype,boolblocking){
3828itemrobot->WaitMove();
3829_send_Line("MoveX");
3830_send_Int(movetype);
3831 if(target !=nullptr){
3832_send_Int(3);
3833_send_Array((tJoints*)nullptr);
3834_send_Item(target);
3835}else if(joints !=nullptr){
3836_send_Int(1);
3837_send_Array(joints);
3838_send_Item(nullptr);
3839}else if(mat_target !=nullptr){// && mat_target.IsHomogeneous()) {
3840_send_Int(2);
3841_send_Array(mat_target);// keep it as array!
3842_send_Item(nullptr);
3843}else{
3844 //throw new RDKException("Invalid target type"); //raise Exception('Problems running function');
3845 throw0;
3846}
3847_send_Item(itemrobot);
3848_check_status();
3849 if(blocking){
3850itemrobot->WaitMove();
3851}
3852}
3853 // private move type, to be used by public methods (MoveJ and MoveL)
3854 voidRoboDK::_moveC(constItem *target1,consttJoints *joints1,constMat *mat_target1,constItem *target2,consttJoints *joints2,constMat *mat_target2,constItem *itemrobot,boolblocking){
3855itemrobot->WaitMove();
3856_send_Line("MoveC");
3857_send_Int(3);
3858 if(target1 !=nullptr){
3859_send_Int(3);
3860_send_Array((tJoints*)nullptr);
3861_send_Item(target1);
3862}else if(joints1 !=nullptr) {
3863_send_Int(1);
3864_send_Array(joints1);
3865_send_Item(nullptr);
3866}else if(mat_target1 !=nullptr){// && mat_target1.IsHomogeneous()) {
3867_send_Int(2);
3868_send_Array(mat_target1);
3869_send_Item(nullptr);
3870}else{
3871 throw0;
3872 //throw new RDKException("Invalid type of target 1");
3873}
3875 if(target2 !=nullptr) {
3876_send_Int(3);
3877_send_Array((tJoints*)nullptr);
3878_send_Item(target2);
3879}else if(joints2 !=nullptr) {
3880_send_Int(1);
3881_send_Array(joints2);
3882_send_Item(nullptr);
3883}else if(mat_target2 !=nullptr){// && mat_target2.IsHomogeneous()) {
3884_send_Int(2);
3885_send_Array(mat_target2);
3886_send_Item(nullptr);
3887}else{
3888 throw0;
3889 //throw new RDKException("Invalid type of target 2");
3890}
3892_send_Item(itemrobot);
3893_check_status();
3894 if(blocking){
3895itemrobot->WaitMove();
3896}
3897}
3898
3899
3900
3901
3902
3903
3904
3905
3906
3907
3908
3909
3910
3911 //---------------------------------------------------------------------------------------------------
3912 //---------------------------------------------------------------------------------------------------
3913 //---------------------------------------------------------------------------------------------------
3914 //---------------------------------------------------------------------------------------------------
3915 //---------------------------------------------------------------------------------------------------
3916 //---------------------------------------------------------------------------------------------------
3918 // 2D matrix functions
3920 voidemxInit_real_T(tMatrix2D **pEmxArray,intnumDimensions)
3921{
3922tMatrix2D *emxArray;
3923 inti;
3924*pEmxArray = (tMatrix2D *)malloc(sizeof(tMatrix2D));
3925emxArray = * pEmxArray;
3926emxArray->data = (double*)NULL;
3927emxArray->numDimensions = numDimensions;
3928emxArray->size = (int*)malloc((unsigned int)(sizeof(int) * numDimensions));
3929emxArray->allocatedSize = 0;
3930emxArray->canFreeData =true;
3931 for(i = 0; i < numDimensions; i++) {
3932emxArray->size[i] = 0;
3933}
3934}
3937 tMatrix2D*matrix;
3938emxInit_real_T((tMatrix2D**)(&matrix), 2);
3939 returnmatrix;
3940}
3941
3942
3943 voidemxFree_real_T(tMatrix2D **pEmxArray){
3944 if(*pEmxArray != (tMatrix2D *)NULL) {
3945 if(((*pEmxArray)->data != (double*)NULL) && (*pEmxArray)->canFreeData) {
3946free((void*)(*pEmxArray)->data);
3947}
3948free((void*)(*pEmxArray)->size);
3949free((void*)*pEmxArray);
3950*pEmxArray = (tMatrix2D *)NULL;
3951}
3952}
3953
3955emxFree_real_T((tMatrix2D**)(mat));
3956}
3957
3958
3959
3960 voidemxEnsureCapacity(tMatrix2D *emxArray,intoldNumel,unsigned intelementSize){
3961 intnewNumel;
3962 inti;
3963 double*newData;
3964 if(oldNumel < 0) {
3965oldNumel = 0;
3966}
3967newNumel = 1;
3968 for(i = 0; i < emxArray->numDimensions; i++) {
3969newNumel *= emxArray->size[i];
3970}
3971 if(newNumel > emxArray->allocatedSize) {
3972i = emxArray->allocatedSize;
3973 if(我<16) {
3974i = 16;
3975}
3976 while(我
3977 if(i > 1073741823) {
3978i =(2147483647);//MAX_int32_T;
3979}else{
3980i <<= 1;
3981}
3982}
3983newData = (double*) calloc((unsigned int)i, elementSize);
3984 if(emxArray->data != NULL) {
3985memcpy(newData, emxArray->data, elementSize * oldNumel);
3986 if(emxArray->canFreeData) {
3987free(emxArray->data);
3988}
3989}
3990emxArray->data = newData;
3991emxArray->allocatedSize = i;
3992emxArray->canFreeData =true;
3993}
3994}
3995
3996 void Matrix2D_Set_Size(tMatrix2D*mat,introws,intcols) {
3997 intold_numel;
3998 intnumbel;
3999old_numel = mat->size[0] * mat->size[1];
4000mat->size[0] = rows;
4001mat->size[1] = cols;
4002numbel = rows*cols;
4003emxEnsureCapacity(mat, old_numel,sizeof(double));
4004 /*for (i=0; i
4005 mat->data[i] = 0.0;
4006 }*/
4007}
4008
4009 int Matrix2D_Size(const tMatrix2D*var,intdim) {// ONE BASED!!
4010 if(var->numDimensions>= dim) {
4011 returnvar->size[dim - 1];
4012}
4013 else{
4014 return0;
4015}
4016}
4018 return Matrix2D_Size(var, 2);
4019}
4021 return Matrix2D_Size(var, 1);
4022}
4023 double Matrix2D_Get_ij(const tMatrix2D*var,inti,intj) {// ZERO BASED!!
4024 returnvar->data[var->size[0] * j + i];
4025}
4026 voidMatrix2D_SET_ij(consttMatrix2D *var,inti,intj,doublevalue) {// ZERO BASED!!
4027var->data[var->size[0] * j + i] = value;
4028}
4029
4030 double*Matrix2D_Get_col(const tMatrix2D*var,intcol) {// ZERO BASED!!
4031 return(var->data+ var->size[0] * col);
4032}
4033
4034
4035 voidMatrix2D_Add(tMatrix2D *var,const double*array,intnumel){
4036 intoldnumel;
4037 intsize1 = var->size[0];
4038 intsize2 = var->size[1];
4039oldnumel = size1*size2;
4040var->size[1] = size2 + 1;
4041emxEnsureCapacity(var, oldnumel, (int)sizeof(double));
4042numel = qMin(numel, size1);
4043 for(inti=0; i
4044var->data[size1*size2 + i] = array[i];
4045}
4046}
4047
4048 voidMatrix2D_Add(tMatrix2D *var,consttMatrix2D *varadd){
4049 intoldnumel;
4050 intsize1 = var->size[0];
4051 intsize2 = var->size[1];
4052 intsize1_ap = varadd->size[0];
4053 intsize2_ap = varadd->size[1];
4054 intnumel = size1_ap*size2_ap;
4055 if(size1 != size1_ap){
4056 return;
4057}
4058oldnumel = size1*size2;
4059var->size[1] = size2 + size2_ap;
4060emxEnsureCapacity(var, oldnumel, (int)sizeof(double));
4061 for(inti=0; i
4062var->data[size1*size2 + i] = varadd->data[i];
4063}
4064}
4065
4066 void Debug_Array(const double*array,intarraysize) {
4067 inti;
4068 for(i = 0; i < arraysize; i++) {
4069 //char chararray[500]; // You had better have room for what you are sprintf()ing!
4070 //sprintf(chararray, "%.3f", array[i]);
4071 //std::cout << chararray;
4072printf("%.3f", array[i]);
4073 if(我
4074 //std::cout << " , ";
4075printf(" , ");
4076}
4077}
4078}
4079
4080 void Debug_Matrix2D(const tMatrix2D*emx) {
4081 intsize1;
4082 intsize2;
4083 intj;
4084 double*column;
4085size1 =Matrix2D_Get_nrows(emx);
4086size2 =Matrix2D_Get_ncols(emx);
4087printf("Matrix size = [%i, %i]\n", size1, size2);
4088 //std::out << "Matrix size = [%i, %i]" << size1 << " " << size2 << "]\n";
4089 for(j = 0; j
4090column =Matrix2D_Get_col(emx, j);
4091 Debug_Array(column, size1);
4092printf("\n");
4093 //std::cout << "\n";
4094}
4095}
4096 /*
4097 void Debug_Mat(Mat pose, char show_full_pose) {
4098 tMatrix4x4 pose_tr;
4099 double xyzwpr[6];
4100 int j;
4101 if (show_full_pose > 0) {
4102 POSE_TR(pose_tr, pose);
4103 printf("Pose size = [4x4]\n");
4104 //std::cout << "Pose size = [4x4]\n";
4105 for (j = 0; j < 4; j++) {
4106 Debug_Array(pose_tr + j * 4, 4);
4107 printf("\n");
4108 //std::cout << "\n";
4109 }
4110 }
4111 else {
4112 POSE_2_XYZWPR(xyzwpr, pose);
4113 //std::cout << "XYZWPR = [ ";
4114 printf("XYZWPR = [ ");
4115 Debug_Array(xyzwpr, 6);
4116 printf(" ]\n");
4117 //std::cout << " ]\n";
4118 }
4119 }
4120 */
4121
4122
4123
4124
4125 #ifndef RDK_SKIP_NAMESPACE
4126}
4127 #endif
The Item class represents an item in RoboDK station. An item can be a robot, a frame,...
Definition: robodk_api.h:1749
Item ObjectLink(int link_id=0)
受潮湿腐烂urns an item pointer to the geometry of a robot link. This is useful to show/hide certain robot li...
Definition: robodk_api.cpp:1134
int RunInstruction (const QString代码,int run_type=RoboDK::INSTRUCTION_CALL_PROGRAM)
Adds a program call, code, message or comment inside a program.
Definition: robodk_api.cpp:1721
RoboDK* RDK()
受潮湿腐烂urns the RoboDK link Robolink().
Definition: robodk_api.cpp:586
Mat GeometryPose()
受潮湿腐烂urns the position (pose) the object geometry with respect to its own reference frame....
Definition: robodk_api.cpp:840
Item getLink(int type_linked=RoboDK::ITEM_TYPE_ROBOT)
受潮湿腐烂urns an item linked to a robot, object, tool, program or robot machining project....
Definition: robodk_api.cpp:1149
Item setMachiningParameters(QString ncfile="", Item part_obj=nullptr, QString options="")
Update the robot milling path input and parameters. Parameter input can be an NC file (G-code or APT ...
Definition: robodk_api.cpp:1036
void Stop()
Stops a program or a robot
Definition: robodk_api.cpp:1596
void MoveC(const Item &itemtarget1, const Item &itemtarget2, bool blocking=true)
Moves a robot to a specific target ("Move Circular" mode). By default, this function blocks until the...
Definition: robodk_api.cpp:1467
tMatrix2D * SolveIK_All_Mat2D(const Mat &pose, const Mat *tool=nullptr, const Mat *ref=nullptr)
Computes the inverse kinematics for the specified robot and pose. The function returns all available ...
Definition: robodk_api.cpp:1331
void setRunType(int program_run_type)
Sets if the program will be run in simulation mode or on the real robot. Use: "PROGRAM_RUN_ON_SIMULAT...
Definition: robodk_api.cpp:1665
void setSpeed(double speed_linear, double accel_linear=-1, double speed_joints=-1, double accel_joints=-1)
Sets the speed and/or the acceleration of a robot.
Definition: robodk_api.cpp:1541
int InstructionList(tMatrix2D *instructions)
受潮湿腐烂urns the list of program instructions as an MxN matrix, where N is the number of instructions and ...
Definition: robodk_api.cpp:1969
void NewLink()
Create a new communication link for RoboDK. Use this for robots if you use a multithread application ...
Definition: robodk_api.cpp:593
Mat SolveFK(const tJoints &joints, const Mat *tool=nullptr, const Mat *ref=nullptr)
Computes the forward kinematics of the robot for the provided joints. The tool and the reference fram...
Definition: robodk_api.cpp:1238
bool Connect(const QString &robot_ip="")
Connect to a real robot using the corresponding robot driver.
Definition: robodk_api.cpp:1366
void setPoseTool(const Mat tool_pose)
设置一个机器人的工具或工具对象(Ce的工具nter Point, or TCP). The tool pose can be either an...
Definition: robodk_api.cpp:932
void setJoints(const tJoints &jnts)
Set robot joints or the joints of a target
Definition: robodk_api.cpp:1164
bool Valid(bool check_pointer=false) const
Check if an item is valid (not null and available in the open station)
Definition: robodk_api.cpp:637
void Instruction(int ins_id, QString &name, int &instype, int &movetype, bool &isjointtarget, Mat &target, tJoints &joints)
受潮湿腐烂urns the program instruction at position id
Definition: robodk_api.cpp:1916
qint32 _TYPE
Item type.
Definition: robodk_api.h:2488
void setAccuracyActive(int accurate=1)
Sets the accuracy of the robot active or inactive. A robot must have been calibrated to properly use ...
Definition: robodk_api.cpp:1627
Mat Pose() const
受潮湿腐烂urns the local position (pose) of an object, target or reference frame. For example,...
Definition: robodk_api.cpp:815
void setColor(double colorRGBA[4])
Changes the color of a robot/object/tool. A color must must in the format COLOR=[R,...
Definition: robodk_api.cpp:983
int Type() const
Item type (object, robot, tool, reference, robot machining project, ...)
Definition: robodk_api.cpp:602
void customInstruction(const QString &name, const QString &path_run, const QString &path_icon="", bool blocking=true, const QString &cmd_run_on_robot="")
Add a custom instruction. This instruction will execute a Python file or an executable file.
Definition: robodk_api.cpp:1826
void setDO(const QString &io_var, const QString &io_value)
Sets a variable (output) to a given value. This can also be used to set any variables to a desired va...
Definition: robodk_api.cpp:1750
void setPose(const Mat pose)
Sets the local position (pose) of an object, target or reference frame. For example,...
Definition: robodk_api.cpp:802
void setGeometryPose(const Mat pose)
Sets the position (pose) the object geometry with respect to its own reference frame....
Definition: robodk_api.cpp:828
bool Busy()
Checks if a robot or program is currently running (busy or moving)
Definition: robodk_api.cpp:1583
void setAsJointTarget()
Sets a target as a joint target. A joint target moves to a joints position without regarding the cart...
Definition: robodk_api.cpp:1065
int RunCode(const QString ¶meters)
Runs a program. It returns the number of instructions that can be executed successfully (a quick prog...
Definition: robodk_api.cpp:1701
Mat PoseTool()
受潮湿腐烂urns the tool pose of an item. If a robot is provided it will get the tool pose of the active tool...
Definition: robodk_api.cpp:879
bool Disconnect()
Disconnect from a real robot (when the robot driver is used)
Definition: robodk_api.cpp:1380
void setRobot(const Item &robot)
Sets the robot of a program or a target. You must set the robot linked to a program or a target every...
Definition: robodk_api.cpp:1207
void setPoseAbs(const Mat pose)
Sets the global position (pose) of an item. For example, the position of an object/frame/target with ...
Definition: robodk_api.cpp:957
void setJointsHome(const tJoints &jnts)
Set robot joints for the home position
Definition: robodk_api.cpp:1121
void setParent(Item parent)
Attaches the item to a new parent while maintaining the relative position with its parent....
Definition: robodk_api.cpp:647
void setPoseFrame(const Mat frame_pose)
Sets the reference frame of a robot(user frame). The frame can be either an item or a pose....
Definition: robodk_api.cpp:906
void setRounding(double zonedata)
Sets the robot movement smoothing accuracy (also known as zone data value).
Definition: robodk_api.cpp:1558
void JointsConfig(const tJoints &joints, tConfig config)
受潮湿腐烂urns the robot configuration state for a set of robot joints.
Definition: robodk_api.cpp:1260
void setParentStatic(Item parent)
Attaches the item to another parent while maintaining the current absolute position in the station....
Definition: robodk_api.cpp:660
int InstructionListJoints(QString &error_msg, tMatrix2D **joint_list, double mm_step=10.0, double deg_step=5.0, const QString &save_to_file="", bool collision_check=false, int flags=0, double time_step_s=0.1)
受潮湿腐烂urns a list of joints an MxN matrix, where M is the number of robot axes plus 4 columns....
Definition: robodk_api.cpp: 2030
void ShowTargets(bool visible=true)
Show or hide targets of a program in the RoboDK tree
Definition: robodk_api.cpp:1883
tJoints Joints() const
受潮湿腐烂urns the current joints of a robot or the joints of a target. If the item is a cartesian target,...
Definition: robodk_api.cpp:1090
void WaitMove(double timeout_sec=300) const
Waits (blocks) until the robot finishes its movement.
Definition: robodk_api.cpp:1607
Mat PoseFrame()
受潮湿腐烂urns the reference frame pose of an item. If a robot is provided it will get the tool pose of the ...
Definition: robodk_api.cpp:892
Mat PoseAbs()
受潮湿腐烂urns the global position (pose) of an item. For example, the position of an object/frame/target wi...
Definition: robodk_api.cpp:970
RoboDK* _RDK
Pointer to RoboDK link object.
Definition: robodk_api.h:2482
QList< tJoints > SolveIK_All(const Mat &pose, const Mat *tool=nullptr, const Mat *ref=nullptr)
Computes the inverse kinematics for the specified robot and pose. The function returns all available ...
Definition: robodk_api.cpp:1348
bool isJointTarget() const
受潮湿腐烂urns True if a target is a joint target (green icon). Otherwise, the target is a Cartesian target ...
Definition: robodk_api.cpp:1075
quint64 _PTR
Pointer to the item inside RoboDK.
Definition: robodk_api.h:2485
void MoveJ(const Item &itemtarget, bool blocking=true)
Moves a robot to a specific target ("Move Joint" mode). By default, this function blocks until the ro...
Definition: robodk_api.cpp:1394
QList< Item > Childs() const
受潮湿腐烂urns a list of the item childs that are attached to the provided item.
Definition: robodk_api.cpp:726
int InstructionCount()
受潮湿腐烂urns the number of instructions of a program.
Definition: robodk_api.cpp:1897
void setName(const QString &name)
Set the name of a RoboDK item.
Definition: robodk_api.cpp:787
double Update(int collision_check=RoboDK::COLLISION_OFF, int timeout_sec=3600, double *out_nins_time_dist=nullptr, double mm_step=-1, double deg_step=-1)
Updates a program and returns the estimated time and the number of valid instructions....
Definition: robodk_api.cpp:1991
void Save(const QString &filename)
Save a station, a robot, a tool or an object to a file
Definition: robodk_api.cpp:617
void Pause(double time_ms=-1)
Generates a pause instruction for a robot or a program when generating code. Set it to -1 (default) i...
Definition: robodk_api.cpp:1736
tJoints JointsHome() const
受潮湿腐烂urns the home joints of a robot. These joints can be manually set in the robot "Parameters" menu,...
Definition: robodk_api.cpp:1106
Item AttachClosest()
Attach the closest object to the tool. Returns the item that was attached.
Definition: robodk_api.cpp:672
void DetachAll(Item parent)
Detach any object attached to a tool.
Definition: robodk_api.cpp:698
void ShowInstructions(bool visible=true)
Show or hide instruction items of a program in the RoboDK tree
Definition: robodk_api.cpp:1871
void setAO(const QString &io_var, const QString &io_value)
Set an analog Output
Definition: robodk_api.cpp:1763
void JointLimits(tJoints *lower_limits, tJoints *upper_limits)
受潮湿腐烂rieve the joint limits of a robot
Definition: robodk_api.cpp:1177
bool Visible() const
受潮湿腐烂urns 1 if the item is visible, otherwise, returns 0.
Definition: robodk_api.cpp:744
bool MakeProgram(const QString &filename)
Saves a program to a file.
Definition: robodk_api.cpp:1645
void Delete()
Deletes an item and its childs from the station.
Definition: robodk_api.cpp:624
void waitDI(const QString &io_var, const QString &io_value, double timeout_ms=-1)
Waits for an input io_id to attain a given value io_value. Optionally, a timeout can be provided.
Definition: robodk_api.cpp:1806
QString getDI(const QString &io_var)
Get a Digital Input (DI). This function is only useful when connected to a real robot using the robot...
Definition: robodk_api.cpp:1776
void setVisible(bool visible, int visible_frame=-1)
Sets the item visiblity status
Definition: robodk_api.cpp:757
void MoveL(const Item &itemtarget, bool blocking=true)
Moves a robot to a specific target ("Move Linear" mode). By default, this function blocks until the r...
Definition: robodk_api.cpp:1430
QString setParam(const QString ¶m, const QString &value)
Set a specific item parameter. Select Tools-Run Script-Show Commands to see all available commands fo...
Definition: robodk_api.cpp: 2060
int RunProgram()
Runs a program. It returns the number of instructions that can be executed successfully (a quick prog...
Definition: robodk_api.cpp:1682
void setAsCartesianTarget()
Sets a target as a cartesian target. A cartesian target moves to cartesian coordinates.
Definition: robodk_api.cpp:1055
void ShowSequence(tMatrix2D *sequence)
Displays a sequence of joints
Definition: robodk_api.cpp:1570
int MoveL_Test(const tJoints &joints1, const Mat &pose2, double minstep_mm=-1)
Checks if a linear movement is free of issues and, optionally, collisions.
Definition: robodk_api.cpp:1519
QString Name() const
受潮湿腐烂urns the name of an item. The name of the item is always displayed in the RoboDK station tree
Definition: robodk_api.cpp:774
bool Finish()
Disconnect from the RoboDK API. This flushes any pending program generation.
Definition: robodk_api.cpp: 2075
Item AddTool(const Mat &tool_pose, const QString &tool_name="New TCP")
Adds an empty tool to the robot provided the tool pose (4x4 Matrix) and the tool name.
Definition: robodk_api.cpp:1222
tJoints SolveIK(const Mat &pose, const Mat *tool=nullptr, const Mat *ref=nullptr)
Computes the inverse kinematics for the specified robot and pose. The joints returned are the closest...
Definition: robodk_api.cpp:1278
quint64 GetID()
Get the item pointer.
Definition: robodk_api.cpp: 2080
void Scale(double scale)
Apply a scale to an object to make it bigger or smaller. The scale can be uniform (if scale is a floa...
Definition: robodk_api.cpp:1003
int MoveJ_Test(const tJoints &j1, const tJoints &j2, double minstep_deg=-1)
Checks if a joint movement is possible and, optionally, free of collisions.
Definition: robodk_api.cpp:1498
void setInstruction(int ins_id, const QString &name, int instype, int movetype, bool isjointtarget, const Mat &target, const tJoints &joints)
Sets the program instruction at position id
Definition: robodk_api.cpp:1946
QString getAI(const QString &io_var)
Get an Analog Input (AI). This function is only useful when connected to a real robot using the robot...
Definition: robodk_api.cpp:1790
Item DetachClosest(Item parent)
Detach the closest object attached to the tool (see also setParentStatic).
Definition: robodk_api.cpp:685
void setJointLimits(const tJoints &lower_limits, const tJoints &upper_limits)
Set the joint limits of a robot
Definition: robodk_api.cpp:1192
Item Parent() const
受潮湿腐烂urn the parent item of this item
Definition: robodk_api.cpp:711
The Mat class represents a 4x4 pose matrix. The main purpose of this object is to represent a pose in...
Definition: robodk_api.h:506
Mat()
Create the identity matrix.
Definition: robodk_api.cpp:189
void VX(tXYZ xyz) const
Get the X vector (N vector)
Definition: robodk_api.cpp:230
void setVX(double x, double y, double z)
Set the X vector values (N vector)
Definition: robodk_api.cpp:250
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: robodk_api.cpp:480
void VY(tXYZ xyz) const
Get the Y vector (O vector)
Definition: robodk_api.cpp:235
static Mat transl(double x, double y, double z)
受潮湿腐烂urn a translation matrix.
Definition: robodk_api.cpp:527
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: robodk_api.cpp:382
void setVZ(double x, double y, double z)
Set the Z vector values (A vector)
Definition: robodk_api.cpp:261
double Get(int r, int c) const
Get a matrix value.
Definition: robodk_api.cpp:302
bool有效()常量
Check if the matrix is valid.
Definition: robodk_api.cpp:523
void setVY(double x, double y, double z)
Set the Y vector values (O vector)
Definition: robodk_api.cpp:255
static Mat roty(double ry)
受潮湿腐烂urn a Y-axis rotation matrix
Definition: robodk_api.cpp:540
const double * Values() const
Get a pointer to the 16-digit array (doubles or floats if ROBODK_API_FLOATS is defined).
Definition: robodk_api.cpp:505
void Pos(tXYZ xyz) const
Get the position (T position), in mm.
Definition: robodk_api.cpp:245
bool MakeHomogeneous()
Forces 4x4 matrix to be homogeneous (vx,vy,vz must be unitary vectors and respect: vx x vy = vz)....
Definition: robodk_api.cpp:354
static Mat rotz(double rz)
受潮湿腐烂urn a Z-axis rotation matrix.
Definition: robodk_api.cpp:546
const double * ValuesD() const
Get a pointer to the 16-digit double array.
Definition: robodk_api.cpp:489
double _ddata16[16]
Copy of the data as a double array.
Definition: robodk_api.h:753
QString ToString(const QString &separator=", ", int precision=3, bool xyzwpr_only=false) const
受潮湿腐烂rieve a string representation of the pose.
Definition: robodk_api.cpp:408
bool FromString(const QString &str)
Set the matrix given a XYZRPW string array (6-values)
Definition: robodk_api.cpp:444
Mat inv() const
Invert the pose (homogeneous matrix assumed)
Definition: robodk_api.cpp:309
void setPos(double x, double y, double z)
Set the position (T position) in mm.
Definition: robodk_api.cpp:267
void VZ(tXYZ xyz) const
Get the Z vector (A vector)
Definition: robodk_api.cpp:240
bool isHomogeneous() const
受潮湿腐烂urns true if the matrix is homogeneous, otherwise it returns false.
Definition: robodk_api.cpp:313
bool _valid
Flags if a matrix is not valid.
Definition: robodk_api.h:748
const float * ValuesF() const
Get a pointer to the 16-digit array as an array of floats.
Definition: robodk_api.cpp:496
void Set(int r, int c, double value)
Set a matrix value.
Definition: robodk_api.cpp:293
static Mat rotx(double rx)
受潮湿腐烂urn the X-axis rotation matrix.
Definition: robodk_api.cpp:534
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: robodk_api.cpp:464
This class is the iterface to the RoboDK API. With the RoboDK API you can automate certain tasks and ...
Definition: robodk_api.h:762
void setViewPose(const Mat &pose)
Set the pose of the wold reference frame with respect to the user view (camera/screen).
Definition: robodk_api.cpp:3057
void setSimulationSpeed(double speed)
Sets the current simulation speed. Set the speed to 1 for a real-time simulation. The slowest speed a...
Definition: robodk_api.cpp:2824
@ ITEM_TYPE_PROGRAM
Program item.
Definition: robodk_api.h:1348
Item getItem(QString name, int itemtype=-1)
受潮湿腐烂urns an item by its name. If there is no exact match it will return the last closest match.
Definition: robodk_api.cpp:2171
Item Paste(const Item *paste_to=nullptr)
Paste the copied item as a dependency of another item (same as Ctrl+V). Paste should be used after Co...
Definition: robodk_api.cpp:2386
QList< Item > getCollisionItems(QList< int > link_id_list)
受潮湿腐烂urn the list of items that are in a collision state. This function can be used after calling Colli...
Definition: robodk_api.cpp:2799
void CloseRoboDK()
Closes RoboDK window and finishes RoboDK execution
Definition: robodk_api.cpp:2274
QString Command(const QString &cmd, const QString &value="")
Send a special command. These commands are meant to have a specific effect in RoboDK,...
Definition: robodk_api.cpp:2938
void ShowMessage(const QString &message, bool popup=true)
Show a message in RoboDK (it can be blocking or non blocking in the status bar)
Definition: robodk_api.cpp:2351
Item AddPoints(tMatrix2D *points, Item *referenceObject=nullptr, bool addToRef=false, int ProjectionType=PROJECTION_ALONG_NORMAL_RECALC)
Adds a list of points to an object. The provided points must be a list of vertices....
Definition: robodk_api.cpp:2495
int RunCode(const QString &code, bool code_is_fcn_call=false)
Adds code to run in the program output. If the program exists it will also run the program in simulat...
Definition: robodk_api.cpp:2671
bool LaserTrackerMeasure(tXYZ xyz, tXYZ estimate, bool search=false)
Takes a laser tracker measurement with respect to its own reference frame. If an estimate point is pr...
Definition: robodk_api.cpp:2948
int Collision(Item item1, Item item2)
受潮湿腐烂urns 1 if item1 and item2 collided. Otherwise returns 0.
Definition: robodk_api.cpp:2789
void Save(const QString &filename, const Item *itemsave=nullptr)
Save an item to a file. If no item is provided, the open station is saved.
Definition: robodk_api.cpp:2418
bool IsInside(Item object_inside, Item object_parent)
Check if an object is fully inside another one.
Definition: robodk_api.cpp: 2724
QString License()
受潮湿腐烂urns the license as a readable string (same name shown in the RoboDK's title bar,...
Definition: robodk_api.cpp:3166
void setRunMode(int run_mode=1)
Sets the behavior of the RoboDK API. By default, RoboDK shows the path simulation for movement instru...
Definition: robodk_api.cpp:2852
void PluginLoad(const QString &pluginName, int load=1)
Load or unload the specified plugin (path to DLL, dylib or SO file). If the plugin is already loaded ...
Definition: robodk_api.cpp:3208
QString Version()
受潮湿腐烂urn the vesion of RoboDK as a 4 digit string: Major.Minor.Revision.Build
Definition: robodk_api.cpp:2282
Item AddFile(const QString &filename, const Item *parent=nullptr)
Loads a file and attaches it to parent. It can be any file supported by RoboDK.
Definition: robodk_api.cpp:2401
Item AddStation(const QString &name)
Add a new empty station. It returns the station item added.
Definition: robodk_api.cpp:2523
void ShowAsCollided(QList< Item > itemList, QList< bool > collidedList, QList< int > *robot_link_id=nullptr)
Show a list of items as collided.
Definition: robodk_api.cpp:2963
bool setCollisionActivePair(int check_state, Item item1, Item item2, int id1=0, int id2=0)
Set collision checking ON or OFF (COLLISION_ON/COLLISION_OFF) for a specific pair of objects....
Definition: robodk_api.cpp:2758
bool FileGet(const QString &path_file_local, Item *station=nullptr, const QString path_file_remote="")
受潮湿腐烂rieve a file from the RoboDK running instance.
Definition: robodk_api.cpp:3296
int RunMode()
受潮湿腐烂urns the behavior of the RoboDK API. By default, RoboDK shows the path simulation for movement ins...
Definition: robodk_api.cpp:2867
Item getActiveStation()
受潮湿腐烂urns the active station item (station currently visible).
Definition: robodk_api.cpp:2635
QList< QPair< QString, QString > > getParams()
Gets all the user parameters from the open RoboDK station. The parameters can also be modified by rig...
Definition: robodk_api.cpp:2880
void ShowRoboDK()
Shows or raises the RoboDK window.
Definition: robodk_api.cpp:2256
void CalibrateTool(tMatrix2D *poses_joints, tXYZ tcp_xyz, int format=EULER_RX_RY_RZ, int algorithm=CALIBRATE_TCP_BY_POINT, Item *robot=nullptr, double *error_stats=nullptr)
Calibrate a tool (TCP) given a number of points or calibration joints. Important: If the robot is cal...
Definition: robodk_api.cpp:2997
void Disconnect()
Disconnect from the RoboDK API. This flushes any pending program generation.
Definition: robodk_api.cpp:2152
Item Popup_ISO9283_CubeProgram(Item *robot=nullptr, tXYZ center=nullptr, double side=-1, bool blocking=true)
Show the popup menu to create the ISO9283 path for position accuracy, repeatability and path accuracy...
Definition: robodk_api.cpp:3235
int setCollisionActive(int check_state=COLLISION_ON)
Turn collision checking ON or OFF (COLLISION_OFF/COLLISION_OFF) according to the collision map....
Definition: robodk_api.cpp:2739
Item getCursorXYZ(int x=-1, int y=-1, tXYZ xyzStation=nullptr)
受潮湿腐烂urns the position of the cursor as XYZ coordinates (by default), or the 3D position of a given set...
Definition: robodk_api.cpp:3139
Mat ViewPose()
Get the pose of the wold reference frame with respect to the user view (camera/screen).
Definition: robodk_api.cpp:3068
Item ItemUserPick(const QString &message="Pick one item", int itemtype=-1)
Shows a RoboDK popup to select one object from the open RoboDK station. An item type can be specified...
Definition: robodk_api.cpp:2241
Item AddTarget(const QString &name, Item *itemparent=nullptr, Item *itemrobot=nullptr)
Adds a new target that can be reached with a robot.
Definition: robodk_api.cpp:2552
void setSelection(QList< Item > list_items)
Sets the selection in the tree (it can be one or more items).
Definition: robodk_api.cpp:3194
QList< Item > Selection()
受潮湿腐烂urns the list of items selected (it can be one or more items).
Definition: robodk_api.cpp:3178
Item AddMachiningProject(const QString &name="Curve follow settings", Item *itemrobot=nullptr)
Add a new robot machining project. Machining projects can also be used for 3D printing,...
Definition: robodk_api.cpp:2603
Item AddCurve(tMatrix2D *curvePoints, Item *referenceObject=nullptr, bool addToRef=false, int ProjectionType=PROJECTION_ALONG_NORMAL_RECALC)
Adds a curve provided point coordinates. The provided points must be a list of vertices....
Definition: robodk_api.cpp:2474
Item AddProgram(const QString &name, Item *itemrobot=nullptr)
Adds a new Frame that can be referenced by a robot.
Definition: robodk_api.cpp:2585
int Cam2D_Snapshot(const QString &file_save_img, const Item &cam_item, const QString ¶ms="")
Take a snapshot of a simulated camera and save it as an image.
Definition: robodk_api.cpp:3116
QList< Item > getOpenStation()
受潮湿腐烂urns the list of open stations in RoboDK.
Definition: robodk_api.cpp: 2616
int RunProgram(const QString &function_w_params)
Adds a function call in the program output. RoboDK will handle the syntax when the code is generated ...
Definition: robodk_api.cpp:2661
int ProgramStart(const QString &progname, const QString &defaultfolder="", const QString &postprocessor="", Item *robot=nullptr)
Defines the name of the program when the program is generated. It is also possible to specify the nam...
Definition: robodk_api.cpp:3041
QList< Item > getItemList(int filter=-1)
受潮湿腐烂urns a list of items (list of names or pointers) of all available items in the currently open stat...
Definition: robodk_api.cpp:2215
int Cam2D_SetParams(const QString &cam_params, const Item &cam_item)
Set the camera parameters.
Definition: robodk_api.cpp:3129
Mat CalibrateReference(tMatrix2D *poses_joints, int method=CALIBRATE_FRAME_3P_P1_ON_X, bool use_joints=false, Item *robot=nullptr)
Calibrate a Reference Frame given a list of points or joint values. Important: If the robot is calibr...
Definition: robodk_api.cpp:3026
void RunMessage(const QString &message, bool message_is_comment=false)
Shows a message or a comment in the output robot program.
Definition: robodk_api.cpp:2686
Item AddFrame(const QString &name, Item *itemparent=nullptr)
Adds a new Frame that can be referenced by a robot.
Definition: robodk_api.cpp:2569
int getFlagsItem(Item item)
受潮湿腐烂rieve current item flags. Item flags allow defining how much access the user has to item-specific ...
Definition: robodk_api.cpp:2337
void setActiveStation(Item stn)
Set the active station (project currently visible).
Definition: robodk_api.cpp:2647
Item AddShape(tMatrix2D *trianglePoints, Item *addTo=nullptr, bool shapeOverride=false, Color *color=nullptr)
Adds a shape provided triangle coordinates. Triangles must be provided as a list of vertices....
Definition: robodk_api.cpp:2438
Item Cam2D_Add(const Item &item_object, const QString &cam_params, const Item *cam_item=nullptr)
Add a simulated 2D or depth camera as an item. Use Delete to delete it.
Definition: robodk_api.cpp:3106
QStringList getItemListNames(int filter=-1)
受潮湿腐烂urns a list of items (list of names or Items) of all available items in the currently open station...
Definition: robodk_api.cpp:2192
void setWindowState(int windowstate=WINDOWSTATE_NORMAL)
Set the state of the RoboDK window
Definition: robodk_api.cpp:2300
QString PluginCommand(const QString &pluginName, const QString &pluginCommand, const QString &pluginValue="")
Send a specific command to a RoboDK plugin. The command and value (optional) must be handled by your ...
Definition: robodk_api.cpp:3219
void HideRoboDK()
Hides the RoboDK window. RoboDK will continue running in the background.
Definition: robodk_api.cpp: 2265
@ INS_TYPE_MOVE
Linear or joint movement instruction.
Definition: robodk_api.h:1375
int Collisions()
受潮湿腐烂urns the number of pairs of objects that are currently in a collision state.
Definition: robodk_api.cpp:2775
void Copy(const Item &tocopy)
Makes a copy of an item (same as Ctrl+C), which can be pasted (Ctrl+V) using Paste().
Definition: robodk_api.cpp:2374
void CloseStation()
Close the current station without asking to save.
Definition: robodk_api.cpp:2539
void setParam(const QString ¶m, const QString &value)
Sets a global parameter from the RoboDK station. If the parameters exists, it will be modified....
Definition: robodk_api.cpp:2924
void Update()
Update the screen. This updates the position of all robots and internal links according to previously...
Definition: robodk_api.cpp:2710
QString getParam(const QString ¶m)
Gets a global or a user parameter from the open RoboDK station. The parameters can also be modified b...
Definition: robodk_api.cpp:2905
空白ProjectPoints (tMatrix2D *点,tMatrix2D * *projected, Item objectProject, int ProjectionType=PROJECTION_ALONG_NORMAL_RECALC)
Projects a point given its coordinates. The provided points must be a list of [XYZ] coordinates....
Definition: robodk_api.cpp:2507
void setFlagsRoboDK(int flags=FLAG_ROBODK_ALL)
Update the RoboDK flags. RoboDK flags allow defining how much access the user has to certain RoboDK f...
Definition: robodk_api.cpp:2312
void Finish()
Disconnect from the RoboDK API. This flushes any pending program generation.
Definition: robodk_api.cpp:2159
void setFlagsItem(Item item, int flags=FLAG_ITEM_ALL)
Update item flags. Item flags allow defining how much access the user has to item-specific features....
Definition: robodk_api.cpp:2324
void Render(bool always_render=false)
Update the scene.
Definition: robodk_api.cpp:2698
double SimulationSpeed()
Gets the current simulation speed. Set the speed to 1 for a real-time simulation.
Definition: robodk_api.cpp:2835
bool FileSet(const QString &file_local, const QString &file_remote="", bool load_file=true, Item *attach_to=nullptr)
Send file from the current location to the RoboDK instance.
Definition: robodk_api.cpp:3269
The tJoints class represents a joint position of a robot (robot axes).
Definition: robodk_api.h:384
int GetValues(double *joints)
GetValues.
Definition: robodk_api.cpp:121
tJoints(int ndofs=0)
tJoints
Definition: robodk_api.cpp:42
QString ToString(const QString &separator=", ", int precision=3) const
受潮湿腐烂rieve a string representation of the joint values.
Definition: robodk_api.cpp:127
bool有效()常量
Check if the joints are valid. For example, when we request the Inverse kinematics and there is no so...
Definition: robodk_api.cpp:163
int Length() const
Number of joint axes of the robot (or degrees of freedom)
Definition: robodk_api.cpp:153
const double * Values() const
Joint values.
Definition: robodk_api.cpp:94
int _nDOFs
number of degrees of freedom
Definition: robodk_api.h:485
double * Data()
Definition: robodk_api.cpp:99
const double * ValuesD() const
Joint values.
Definition: robodk_api.cpp:80
double _Values[RDK_SIZE_JOINTS_MAX]
joint values (doubles, used to store the joint values)
Definition: robodk_api.h:488
bool FromString(const QString &str)
Set the joint values given a comma-separated string. Tabs and spaces are also allowed.
Definition: robodk_api.cpp:143
void setLength(int new_length)
Set the length of the array (only shrinking the array is allowed)
Definition: robodk_api.cpp:157
float _ValuesF[RDK_SIZE_JOINTS_MAX]
joint values (floats, used to return a copy as a float pointer)
Definition: robodk_api.h:491
const float * ValuesF() const
Joint values.
Definition: robodk_api.cpp:83
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...
Definition: robodk_api.cpp:104
All RoboDK API functions are wrapped in the RoboDK_API namespace. If you prefer to forget about the R...
Definition: robodk_api.cpp:37
double * Matrix2D_Get_col(const tMatrix2D *var, int col)
受潮湿腐烂urns the pointer of a column of a tMatrix2D. A column has Matrix2D_Get_nrows values that can be ac...
Definition: robodk_api.cpp:4030
tMatrix2D * Matrix2D_Create()
Creates a new 2D matrix tMatrix2D.. Use Matrix2D_Delete to delete the matrix (to free the memory)....
Definition: robodk_api.cpp:3936
Mat roty(double ry)
Translation matrix class: Mat::roty.
Definition: robodk_api.cpp:181
int Matrix2D_Get_ncols(const tMatrix2D *var)
受潮湿腐烂urns the number of columns of a tMatrix2D.
Definition: robodk_api.cpp:4017
double Matrix2D_Get_ij(const tMatrix2D *var, int i, int j)
受潮湿腐烂urns the value at location [i,j] of a tMatrix2D.
Definition: robodk_api.cpp:4023
Mat rotz(double rz)
Translation matrix class: Mat::rotz.
Definition: robodk_api.cpp:185
double tConfig[RDK_SIZE_MAX_CONFIG]
The robot configuration defines a specific state of the robot without crossing any singularities....
Definition: robodk_api.h:310
Mat transl(double x, double y, double z)
Translation matrix class: Mat::transl.
Definition: robodk_api.cpp:173
void Matrix2D_Set_Size(tMatrix2D *mat, int rows, int cols)
Sets the size of a tMatrix2D.
Definition: robodk_api.cpp:3996
int Matrix2D_Size(const tMatrix2D *var, int dim)
Sets the size of a tMatrix2D.
Definition: robodk_api.cpp:4009
void Debug_Matrix2D(const tMatrix2D *emx)
Display the content of a tMatrix2D through STDOUT. This is only intended for debug purposes.
Definition: robodk_api.cpp:4080
Mat rotx(double rx)
Translation matrix class: Mat::rotx.
Definition: robodk_api.cpp:177
double tXYZ[3]
tXYZ (mm) represents a position or a vector in mm
Definition: robodk_api.h:299
int Matrix2D_Get_nrows(const tMatrix2D *var)
受潮湿腐烂urns the number of rows of a tMatrix2D.
Definition: robodk_api.cpp:4020
void Debug_Array(const double *array, int arraysize)
Show an array through STDOUT Given an array of doubles, it generates a string.
Definition: robodk_api.cpp:4066
void Matrix2D_Delete(tMatrix2D **mat)
Deletes a tMatrix2D.
Definition: robodk_api.cpp:3954
double tXYZWPR[6]
Six doubles that represent robot joints (usually in degrees)
Definition: robodk_api.h:296
The Color struct represents an RGBA color (each color component should be in the range [0-1])
Definition: robodk_api.h:336
float r
Red color.
Definition: robodk_api.h:338
float a
Alpha value (0 = transparent; 1 = opaque)
Definition: robodk_api.h:347
float b
Blue color.
Definition: robodk_api.h:344
float g
Green color.
Definition: robodk_api.h:341
The tMatrix2D struct represents a variable size 2d Matrix. Use the Matrix2D_... functions to oeprate ...
Definition: robodk_api.h:361
double * data
Pointer to the data.
Definition: robodk_api.h:363
int * size
Pointer to the size array.
Definition: robodk_api.h:366
int numDimensions
Number of dimensions (usually 2)
Definition: robodk_api.h:372