icub-test
Loading...
Searching...
No Matches
Public Member Functions
OpticalEncodersConsistency Class Reference
A list of available tests

This tests checks if the motor encoder reading are consistent with the joint encoder readings. More...

#include <motorEncodersConsistency.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions

virtual bool setup (yarp::os::Property &property)
 
virtual void tearDown ()
 
virtual void run ()
 
void goHome ()
 
void setMode (int desired_mode)
 
void saveToFile (std::string filename, yarp::os::Bottle &b)
 

Detailed Description

This tests checks if the motor encoder reading are consistent with the joint encoder readings.

Since the two sensors may be placed in different places, with gearboxes or tendon transmissions in between, a (signed) factor is needed to convert the two measurements. The test performes a cyclic movement between two reference positions (min and max) and collects data from both the encoders during the movement. The test generates four text data files, which are subsequently opened to generate plots. In all figures the joint and motor plots need to be reasonably aligned. The four plots are:

Example: testRunner v -t motorEncodersConsistency.dll -p "--robot icub --part left_arm --joints ""(0 1 2)"" --home ""(-30 30 10)"" --speed ""(20 20 20)"" --max ""(-20 40 20)"" --min ""(-40 20 0)"" --cycles 10 --tolerance 1.0 " Example: testRunner v -s "..\icub-tests\suites\encoders-icubSim.xml"

Check the following functions:

Accepts the following parameters:

Parameter name Type Units Default Value Required Description Notes
robot string - - Yes The name of the robot. e.g. icub
part string - - Yes The name of trhe robot part. e.g. left_arm
joints vector of ints - - Yes List of joints to be tested
home vector of doubles of size joints deg - Yes The home position for each joint
cycles int - 10 No The number of test cycles (going from max to min position and viceversa
max vector of doubles of size joints deg - Yes The max position using during the joint movement
min vector of doubles of size joints deg - Yes The min position using during the joint movement
tolerance vector of doubles of size joints deg - Yes The tolerance used when moving from min to max reference position and viceversa
speed vector of doubles of size joints deg/s - Yes The reference speed used during the movement
matrix_size int - - Yes The number of rows of the coupling matrix Typical value = 4.
matrix vector of doubles of size matrix_size - - Yes The kinematic_mj coupling matrix matrix is identity if joints are not coupled
plotstring1 string - Yes The string which generates plot 1
plotstring2 string - Yes The string which generates plot 2
plotstring3 string - Yes The string which generates plot 3
plotstring4 string - Yes The string which generates plot 4

Definition at line 80 of file motorEncodersConsistency.h.

Constructor & Destructor Documentation

◆ OpticalEncodersConsistency()

OpticalEncodersConsistency::OpticalEncodersConsistency ( )

Definition at line 49 of file motorEncodersConsistency.cpp.

49 : yarp::robottestingframework::TestCase("OpticalEncodersConsistency") {
50 jointsList=0;
51 dd=0;
52 ipos=0;
53 icmd=0;
54 iimd=0;
55 ienc=0;
56 imot=0;
57 imotenc=0;
58
59 enc_jnt=0;
60 enc_jnt2mot=0;
61 enc_mot=0;
62 vel_jnt=0;
63 vel_jnt2mot=0;
64 vel_mot=0;
65 acc_jnt=0;
66 acc_jnt2mot=0;
67 acc_mot=0;
68 cycles =10;
69 tolerance = 1.0;
70 plot_enabled = false;
71}

◆ ~OpticalEncodersConsistency()

OpticalEncodersConsistency::~OpticalEncodersConsistency ( )
virtual

Definition at line 73 of file motorEncodersConsistency.cpp.

73{ }

Member Function Documentation

◆ goHome()

void OpticalEncodersConsistency::goHome ( )

Definition at line 260 of file motorEncodersConsistency.cpp.

261{
262 if (ipos == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid position control interface");
263 if (ienc == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid encoders interface");
264
265 bool ret = true;
266 char buff [500];
267 sprintf(buff,"Homing the whole part");ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
268
269 for (unsigned int i=0; i<jointsList.size(); i++)
270 {
271 ret = ipos->setRefSpeed((int)jointsList[i],speed[i]);
272 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "ipos->setRefSpeed returned false");
273 ret = ipos->positionMove((int)jointsList[i],home[i]);
274 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "ipos->positionMove returned false");
275 }
276
277 int timeout = 0;
278 while (1)
279 {
280 int in_position=0;
281 for (unsigned int i=0; i<jointsList.size(); i++)
282 {
283 double tmp=0;
284 ienc->getEncoder((int)jointsList[i],&tmp);
285 if (fabs(tmp-home[i])<tolerance) in_position++;
286 }
287 if (in_position==jointsList.size()) break;
288 if (timeout>100)
289 {
290 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching home position");
291 }
292 yarp::os::Time::delay(0.2);
293 timeout++;
294 }
295}

◆ run()

void OpticalEncodersConsistency::run ( )
virtual

Definition at line 313 of file motorEncodersConsistency.cpp.

314{
315 char buff [500];
316 setMode(VOCAB_CM_POSITION);
317 goHome();
318
319 bool go_to_max=false;
320 for (unsigned int i=0; i<jointsList.size(); i++)
321 {
322 ipos->positionMove((int)jointsList[i], min[i]);
323 }
324
325 int cycle=0;
326 double start_time = yarp::os::Time::now();
327
328 //****************************************************************************************
329 //Retrieving coupling matrix using IRemoteVariable
330 //****************************************************************************************
331
332 yarp::os::Bottle b;
333
334 ivar->getRemoteVariable("kinematic_mj", b);
335
336 int matrix_size = matrix.cols();
337
338 matrix.eye();
339
340 int njoints [4];
341
342 for(int i=0 ; i< b.size() ; i++)
343 {
344 Bottle bv;
345 bv.fromString(b.get(i).toString());
346 njoints[i] = sqrt(bv.size());
347
348 int ele = 0;
349 if(i==0) {
350 for (int r=0; r < njoints[i]; r++)
351 {
352 for (int c=0; c < njoints[i]; c++)
353 {
354 matrix(r,c) = bv.get(ele).asFloat64();
355 ele++;
356 }
357 }
358
359 }
360 else{
361 for (int r=0; r < njoints[i]; r++)
362 {
363 for (int c=0; c < njoints[i]; c++)
364 {
365 int jntprev = 0;
366 for (int j=0; j < i; j++) jntprev += njoints[j];
367 if(!jntprev > matrix_size) matrix(r+jntprev,c+jntprev) = bv.get(ele).asFloat64();
368 ele++;
369 }
370 }
371 }
372
373 }
374
375 // yDebug() << "MATRIX J2M : \n" << matrix.toString();
376
377// **************************************************************************************
378
379 trasp_matrix = matrix.transposed();
380 inv_matrix = yarp::math::luinv(matrix);
381 inv_trasp_matrix = inv_matrix.transposed();
382
383 sprintf(buff,"Matrix:\n %s \n", matrix.toString().c_str());
384 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
385 sprintf(buff,"Inv matrix:\n %s \n", inv_matrix.toString().c_str());
386 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
387
388 Bottle dataToPlot_test1;
389 Bottle dataToPlot_test2;
390 Bottle dataToPlot_test3;
391 Bottle dataToPlot_test4;
392 Bottle dataToPlot_test1rev;
393
394 bool test_data_is_valid = false;
395 bool first_time = true;
396 yarp::sig::Vector off_enc_mot; off_enc_mot.resize(jointsList.size());
397 yarp::sig::Vector off_enc_jnt; off_enc_jnt.resize(jointsList.size());
398 yarp::sig::Vector off_enc_jnt2mot; off_enc_jnt2mot.resize(jointsList.size());
399 yarp::sig::Vector off_enc_mot2jnt; off_enc_mot2jnt.resize(jointsList.size());
400 yarp::sig::Vector tmp_vector;
401 tmp_vector.resize(n_part_joints);
402
403
404
405 while (1)
406 {
407 double curr_time = yarp::os::Time::now();
408 double elapsed = curr_time - start_time;
409
410 bool ret = true;
411 ret = ienc->getEncoders(tmp_vector.data());
412 for (unsigned int i = 0; i < jointsList.size(); i++)
413 enc_jnt[i] = tmp_vector[jointsList[i]];
414
415
416 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "ienc->getEncoders returned false");
417 ret = imotenc->getMotorEncoders(tmp_vector.data()); for (unsigned int i = 0; i < jointsList.size(); i++) enc_mot[i] = tmp_vector[jointsList(i)];
418 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "imotenc->getMotorEncoder returned false");
419 ret = ienc->getEncoderSpeeds(tmp_vector.data()); for (unsigned int i = 0; i < jointsList.size(); i++) vel_jnt[i] = tmp_vector[jointsList(i)];
420 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "ienc->getEncoderSpeeds returned false");
421 ret = imotenc->getMotorEncoderSpeeds(tmp_vector.data()); for (unsigned int i = 0; i < jointsList.size(); i++) vel_mot[i] = tmp_vector[jointsList(i)];
422 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "imotenc->getMotorEncoderSpeeds returned false");
423 ret = ienc->getEncoderAccelerations(tmp_vector.data()); for (unsigned int i = 0; i < jointsList.size(); i++) acc_jnt[i] = tmp_vector[jointsList(i)];
424 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "ienc->getEncoderAccelerations returned false");
425 ret = imotenc->getMotorEncoderAccelerations(tmp_vector.data()); for (unsigned int i = 0; i < jointsList.size(); i++) acc_mot[i] = tmp_vector[jointsList(i)];
426 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ret, "imotenc->getMotorEncoderAccelerations returned false");
427
428 //if (enc_jnt == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getEncoders data"); test_data_is_valid = true; }
429 //if (enc_mot == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getMotorEncoders data"); test_data_is_valid = true; }
430 //if (vel_jnt == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getEncoderSpeeds data"); test_data_is_valid = true; }
431 //if (vel_mot == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getMotorEncoderSpeeds data"); test_data_is_valid = true; }
432 //if (acc_jnt == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getEncoderAccelerations data"); test_data_is_valid = true; }
433 //if (acc_mot == zero_vector) { ROBOTTESTINGFRAMEWORK_TEST_REPORT("Invalid getMotorEncoderAccelerations data"); test_data_is_valid = true; }
434
435 if (first_time)
436 {
437 off_enc_jnt = enc_jnt;
438 off_enc_mot2jnt = enc_mot2jnt;
439
440 }
441
442 enc_jnt2mot = matrix * enc_jnt;
443 enc_mot2jnt = inv_matrix * (enc_mot - off_enc_mot);
444 vel_jnt2mot = matrix * vel_jnt;
445 //acc_jnt2mot = matrix * acc_jnt;
446
447
448 for (unsigned int i = 0; i < jointsList.size(); i++) enc_jnt2mot[i] = enc_jnt2mot[i] * gearbox[i];;
449 for (unsigned int i = 0; i < jointsList.size(); i++) vel_jnt2mot[i] = vel_jnt2mot[i] * gearbox[i];
450 //for (unsigned int i = 0; i < jointsList.size(); i++) acc_jnt2mot[i] = acc_jnt2mot[i] * gearbox[i];
451 for (unsigned int i = 0; i < jointsList.size(); i++) enc_mot2jnt[i] = enc_mot2jnt[i] / gearbox[i];
452
453 bool reached = false;
454 int in_position = 0;
455 for (unsigned int i = 0; i < jointsList.size(); i++)
456 {
457 double curr_val = 0;
458 if (go_to_max == false) curr_val = min[i];
459 else curr_val = max[i];
460 if (fabs(enc_jnt[i] - curr_val) < tolerance) in_position++;
461 }
462 if (in_position == jointsList.size()) reached = true;
463
464 if (elapsed >= 20.0)
465 {
466 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while moving joint");
467 }
468
469 if (reached)
470 {
471 sprintf(buff, "Test cycle %d/%d", cycle, cycles); ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
472 if (go_to_max == false)
473 {
474 for (unsigned int i = 0; i < jointsList.size(); i++)
475 ipos->positionMove(jointsList[i], max[i]);
476 go_to_max = true;
477 cycle++;
478 start_time = yarp::os::Time::now();
479 }
480 else
481 {
482 for (unsigned int i = 0; i < jointsList.size(); i++)
483 ipos->positionMove(jointsList[i], min[i]);
484 go_to_max = false;
485 cycle++;
486 start_time = yarp::os::Time::now();
487 }
488 }
489
490 //update previous and computes diff
491 diff_enc_jnt = (enc_jnt - prev_enc_jnt) / 0.010;
492 diff_enc_mot = (enc_mot - prev_enc_mot) / 0.010;
493 diff_enc_jnt2mot = (enc_jnt2mot - prev_enc_jnt2mot) / 0.010;
494 diff_vel_jnt = (vel_jnt - prev_vel_jnt) / 0.010;
495 diff_vel_mot = (vel_mot - prev_vel_mot) / 0.010;
496 diff_vel_jnt2mot = (vel_jnt2mot - prev_vel_jnt2mot) / 0.010;
497 diff_acc_jnt = (acc_jnt - prev_acc_jnt) / 0.010;
498 diff_acc_mot = (acc_mot - prev_acc_mot) / 0.010;
499 //diff_acc_jnt2mot = (acc_jnt2mot - prev_acc_jnt2mot) / 0.010;
500 prev_enc_jnt = enc_jnt;
501 prev_enc_mot = enc_mot;
502 prev_enc_jnt2mot = enc_jnt2mot;
503 prev_vel_jnt = vel_jnt;
504 prev_vel_mot = vel_mot;
505 prev_vel_jnt2mot = vel_jnt2mot;
506 // prev_acc_jnt = acc_jnt;
507 //prev_acc_mot = acc_mot;
508 // prev_acc_jnt2mot = acc_jnt2mot;
509
510 if (first_time)
511 {
512 off_enc_mot = enc_mot;
513 off_enc_jnt2mot = enc_jnt2mot;
514 }
515
516 {
517 //prepare data to plot
518 //JOINT POSITIONS vs MOTOR POSITIONS
519 Bottle& row_test1 = dataToPlot_test1.addList();
520 Bottle& v1_test1 = row_test1.addList();
521 Bottle& v2_test1 = row_test1.addList();
522 yarp::sig::Vector v1 = enc_mot - off_enc_mot;
523 yarp::sig::Vector v2 = enc_jnt2mot - off_enc_jnt2mot;
524 v1_test1.read(v1);
525 v2_test1.read(v2);
526 }
527
528 {
529 //JOINT VELOCITES vs MOTOR VELOCITIES
530 Bottle& row_test2 = dataToPlot_test2.addList();
531 Bottle& v1_test2 = row_test2.addList();
532 Bottle& v2_test2 = row_test2.addList();
533 v1_test2.read(vel_mot);
534 v2_test2.read(vel_jnt2mot);
535 }
536
537 {
538 //JOINT POSITIONS(DERIVED) vs JOINT SPEED
539 if (first_time == false)
540 {
541 Bottle& row_test3 = dataToPlot_test3.addList();
542 Bottle& v1_test3 = row_test3.addList();
543 Bottle& v2_test3 = row_test3.addList();
544 v1_test3.read(vel_jnt);
545 v2_test3.read(diff_enc_jnt);
546 }
547 }
548
549 {
550 //MOTOR POSITIONS(DERIVED) vs MOTOR SPEED
551 if (first_time == false)
552 {
553 Bottle& row_test4 = dataToPlot_test4.addList();
554 Bottle& v1_test4 = row_test4.addList();
555 Bottle& v2_test4 = row_test4.addList();
556 v1_test4.read(vel_mot);
557 v2_test4.read(diff_enc_mot);
558 }
559 }
560
561 {
562 //JOINT POSITIONS vs MOTOR POSITIONS REVERSED
563 Bottle& row_test1 = dataToPlot_test1rev.addList();
564 Bottle& v1_test1 = row_test1.addList();
565 Bottle& v2_test1 = row_test1.addList();
566 yarp::sig::Vector v1 = enc_jnt;
567 yarp::sig::Vector v2 = enc_mot2jnt + off_enc_jnt;
568 v1_test1.read(v1);
569 v2_test1.read(v2);
570 }
571
572 //flag set
573 first_time = false;
574
575 //exit condition
576 if (cycle>=cycles) break;
577 }
578
579 goHome();
580
581 yarp::os::ResourceFinder rf;
582 rf.setDefaultContext("scripts");
583
584 string partfilename = partName+".txt";
585 string testfilename = "encConsis_";
586 string filename1 = testfilename + "jointPos_MotorPos_" + partfilename;
587 saveToFile(filename1,dataToPlot_test1);
588 string filename2 = testfilename + "jointVel_motorVel_" + partfilename;
589 saveToFile(filename2,dataToPlot_test2);
590 string filename3 = testfilename + "joint_derivedVel_vel_" + partfilename;
591 saveToFile(filename3,dataToPlot_test3);
592 string filename4 = testfilename + "motor_derivedVel_vel_" + partfilename;
593 saveToFile(filename4,dataToPlot_test4);
594
595 string filename1rev = testfilename + "jointPos_MotorPos_reversed_" + partfilename;
596 saveToFile(filename1rev,dataToPlot_test1rev);
597
598 //find octave scripts
599 std::string octaveFile = rf.findFile("encoderConsistencyPlotAll.m");
600 if(octaveFile.size() == 0)
601 {
602 yError()<<"Cannot find file encoderConsistencyPlotAll.m";
603 return;
604 }
605
606 //prepare octave command
607 std::string octaveCommand= "octave --path "+ getPath(octaveFile);
608 stringstream ss;
609 ss << jointsList.size();
610 string str = ss.str();
611 octaveCommand+= " -q --eval \"encoderConsistencyPlotAll('" +partName +"'," + str +")\" --persist";
612
613 if(plot_enabled)
614 {
615 int ret = system (octaveCommand.c_str());
616 }
617 else
618 {
619 yInfo() << "Test has collected all data. You need to plot data to check is test is passed";
620 yInfo() << "Please run following command to plot data.";
621 yInfo() << octaveCommand;
622 yInfo() << "To exit from Octave application please type 'exit' command.";
623 }
624 // ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(test_data_is_valid,"Invalid data obtained from encoders interface");
625}

◆ saveToFile()

void OpticalEncodersConsistency::saveToFile ( std::string  filename,
yarp::os::Bottle &  b 
)

Definition at line 297 of file motorEncodersConsistency.cpp.

298{
299 std::fstream fs;
300 fs.open (filename.c_str(), std::fstream::out);
301
302 for (int i=0; i<b.size(); i++)
303 {
304 std::string s = b.get(i).toString();
305 std::replace(s.begin(), s.end(), '(', ' ');
306 std::replace(s.begin(), s.end(), ')', ' ');
307 fs << s << endl;
308 }
309
310 fs.close();
311}

◆ setMode()

void OpticalEncodersConsistency::setMode ( int  desired_mode)

Definition at line 225 of file motorEncodersConsistency.cpp.

226{
227 if (icmd == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid control mode interface");
228 if (iimd == 0) ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid interaction mode interface");
229
230 for (unsigned int i=0; i<jointsList.size(); i++)
231 {
232 icmd->setControlMode((int)jointsList[i],desired_mode);
233 iimd->setInteractionMode((int)jointsList[i],VOCAB_IM_STIFF);
234 yarp::os::Time::delay(0.010);
235 }
236
237 int cmode;
238 yarp::dev::InteractionModeEnum imode;
239 int timeout = 0;
240
241 while (1)
242 {
243 int ok=0;
244 for (unsigned int i=0; i<jointsList.size(); i++)
245 {
246 icmd->getControlMode ((int)jointsList[i],&cmode);
247 iimd->getInteractionMode((int)jointsList[i],&imode);
248 if (cmode==desired_mode && imode==VOCAB_IM_STIFF) ok++;
249 }
250 if (ok==jointsList.size()) break;
251 if (timeout>100)
252 {
253 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Unable to set control mode/interaction mode");
254 }
255 yarp::os::Time::delay(0.2);
256 timeout++;
257 }
258}

◆ setup()

bool OpticalEncodersConsistency::setup ( yarp::os::Property &  property)
virtual

Definition at line 75 of file motorEncodersConsistency.cpp.

75 {
76
77 if(property.check("name"))
78 setName(property.find("name").asString());
79
80 char b[5000];
81 strcpy (b,property.toString().c_str());
82 ROBOTTESTINGFRAMEWORK_TEST_REPORT("on setup()");
83 // updating parameters
84 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
85 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
86 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
87 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("home"), "The home position must be given as the test parameter!");
88 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("max"), "The max position must be given as the test parameter!");
89 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("min"), "The min position must be given as the test parameter!");
90 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("speed"), "The positionMove reference speed must be given as the test parameter!");
91 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("tolerance"), "The tolerance of the control signal must be given as the test parameter!");
92 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("matrix_size"), "The matrix size must be given!");
93 // ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("matrix"), "The coupling matrix must be given!");
94 robotName = property.find("robot").asString();
95 partName = property.find("part").asString();
96 if(property.check("plot_enabled"))
97 plot_enabled = property.find("plot_enabled").asBool();
98 /*if(plot_enabled)
99 {
100 plotString1 = property.find("plotString1").asString();
101 plotString2 = property.find("plotString2").asString();
102 plotString3 = property.find("plotString3").asString();
103 plotString4 = property.find("plotString4").asString();
104 }*/
105 Bottle* jointsBottle = property.find("joints").asList();
106 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
107
108 Bottle* homeBottle = property.find("home").asList();
109 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(homeBottle!=0,"unable to parse home parameter");
110
111 Bottle* maxBottle = property.find("max").asList();
112 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(maxBottle!=0,"unable to parse max parameter");
113
114 Bottle* minBottle = property.find("min").asList();
115 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(minBottle!=0,"unable to parse min parameter");
116
117 Bottle* speedBottle = property.find("speed").asList();
118 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(speedBottle!=0,"unable to parse speed parameter");
119
120 tolerance = property.find("tolerance").asFloat64();
121 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(tolerance>=0,"invalid tolerance");
122
123 int matrix_size=property.find("matrix_size").asInt32();
124 if (matrix_size>0)
125 {
126 matrix.resize(matrix_size,matrix_size);
127 matrix.eye();
128
129 // The couplig matrix is retrived run-time by the IRemoteVariable interface accessing the jinimatic_mj variable
130 //
131 // Bottle* matrixBottle = property.find("matrix").asList();
132
133 // if (matrixBottle!= NULL && matrixBottle->size() == (matrix_size*matrix_size) )
134 // {
135 // for (int i=0; i< (matrix_size*matrix_size); i++)
136 // {
137 // matrix.data()[i]=matrixBottle->get(i).asFloat64();
138 // }
139 // }
140 // else
141 // {
142 // char buff [500];
143 // sprintf (buff, "invalid number of elements of parameter matrix %d!=%d", matrixBottle->size() , (matrix_size*matrix_size));
144 // ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
145 // }
146 }
147 else
148 {
149 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("invalid matrix_size: must be >0");
150 }
151
152 //optional parameters
153 if (property.check("cycles"))
154 {cycles = property.find("cycles").asInt32();}
155
156 Property options;
157 options.put("device", "remote_controlboard");
158 options.put("remote", "/"+robotName+"/"+partName);
159 options.put("local", "/positionDirectTest/"+robotName+"/"+partName);
160
161 dd = new PolyDriver(options);
162 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
163 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
164 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
165 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
166 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
167 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(imotenc),"Unable to open motor encoders interface");
168 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(imot),"Unable to open motor interface");
169 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ivar),"Unable to open remote variables interface");
170
171
172 if (!ienc->getAxes(&n_part_joints))
173 {
174 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
175 }
176
177 int n_cmd_joints = jointsBottle->size();
178 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0 && n_cmd_joints<=n_part_joints,"invalid number of joints, it must be >0 & <= number of part joints");
179 jointsList.clear();
180 for (int i=0; i <n_cmd_joints; i++) jointsList.push_back(jointsBottle->get(i).asInt32());
181
182 enc_jnt.resize(n_cmd_joints); enc_jnt.zero();
183 enc_mot.resize(n_cmd_joints); enc_mot.zero();
184 vel_jnt.resize(n_cmd_joints); vel_jnt.zero();
185 vel_mot.resize(n_cmd_joints); vel_mot.zero();
186 acc_jnt.resize(n_cmd_joints); acc_jnt.zero();
187 acc_mot.resize(n_cmd_joints); acc_mot.zero();
188 prev_enc_jnt.resize(n_cmd_joints); prev_enc_jnt.zero();
189 prev_enc_mot.resize(n_cmd_joints); prev_enc_mot.zero();
190 prev_enc_jnt2mot.resize(n_cmd_joints); prev_enc_jnt2mot.zero();
191 prev_vel_jnt.resize(n_cmd_joints); prev_vel_jnt.zero();
192 prev_vel_mot.resize(n_cmd_joints); prev_vel_mot.zero();
193 prev_vel_jnt2mot.resize(n_cmd_joints); prev_vel_jnt2mot.zero();
194 prev_acc_jnt.resize(n_cmd_joints); prev_acc_jnt.zero();
195 prev_acc_mot.resize(n_cmd_joints); prev_acc_mot.zero();
196 prev_acc_jnt2mot.resize(n_cmd_joints); prev_acc_jnt2mot.zero();
197 zero_vector.resize(n_cmd_joints);
198 zero_vector.zero();
199
200 max.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) max[i]=maxBottle->get(i).asFloat64();
201 min.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) min[i]=minBottle->get(i).asFloat64();
202 home.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) home[i]=homeBottle->get(i).asFloat64();
203 speed.resize(n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) speed[i]=speedBottle->get(i).asFloat64();
204 gearbox.resize(n_cmd_joints);
205 for (int i=0; i< n_cmd_joints; i++)
206 {
207 double t;
208 int b=imot->getGearboxRatio(jointsList[i],&t);
209 gearbox[i]=t;
210 }
211
212
213 return true;
214}

◆ tearDown()

void OpticalEncodersConsistency::tearDown ( )
virtual

Definition at line 216 of file motorEncodersConsistency.cpp.

217{
218 char buff[500];
219 sprintf(buff,"Closing test module");ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
220 setMode(VOCAB_CM_POSITION);
221 goHome();
222 if (dd) {delete dd; dd =0;}
223}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8