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

The test checks if the joint is able to go in all the available control/interaction modes and if transition between the states is correct. More...

#include <ControlModes.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions

virtual bool setup (yarp::os::Property &property)
 
virtual void tearDown ()
 
virtual void run ()
 
void goHome ()
 
void executeCmd ()
 
void setMode (int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
 
void verifyMode (int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
 
void verifyAmplifier (int desired_amplifier_mode, std::string title)
 
void zeroCurrentLimits ()
 
void getOriginalCurrentLimits ()
 
void resetOriginalCurrentLimits ()
 
void verifyModeSingle (int joint, int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
 
void setModeSingle (int joint, int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
 
void checkJointWithTorqueMode ()
 
void checkControlModeWithImCompliant (int desired_control_mode, std::string title)
 

Detailed Description

The test checks if the joint is able to go in all the available control/interaction modes and if transition between the states is correct.

The following control modes are test: VOCAB_CM_POSITION, VOCAB_CM_POSITION_DIRECT, VOCAB_CM_VELOCITY, VOCAB_CM_MIXED, VOCAB_CM_PWM, VOCAB_CM_IDLE, VOCAB_CM_FORCE_IDLE, VOCAB_HW_FAULT. The following interaction modes are tested: VOCAB_IM_STIFF, VOCAB_IM_COMPLIANT. For some modes (VOCAB_CM_TORQUE, VOCAB_IM_COMPLIANT) the test asks to robotInterface if the joint capabilities, skipping the test if those modes are not implemented. The test intentionally generates an hardware fault to test the transition between VOCAB_CM_HW_FAULT to VOCAB_CM_IDLE. The fault is generated by zeroing the max current limit. Check of the amplifier internal status (iAmplifier->getAmpStatus) has to be implemented yet.

Example: testRunner -v -t ControlModes.dll -p "--robot icub --part head --joints ""(0 1 2 3 4 5)"" --zero 0"

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. | | | zero | double | deg | - | Yes | The home position for the tested joints. | |

Definition at line 53 of file ControlModes.h.

Constructor & Destructor Documentation

◆ ControlModes()

ControlModes::ControlModes ( )

Definition at line 37 of file ControlModes.cpp.

37 : yarp::robottestingframework::TestCase("ControlModes") {
38 jointsList=0;
39 pos_tot=0;
40 dd=0;
41 ipos=0;
42 iamp=0;
43 icmd=0;
44 iimd=0;
45 ienc=0;
46 idir=0;
47 itrq=0;
48 ivel=0;
49 cmd_some=0;
50 cmd_tot=0;
51 prevcurr_some=0;
52 prevcurr_tot=0;
53}

◆ ~ControlModes()

ControlModes::~ControlModes ( )
virtual

Definition at line 55 of file ControlModes.cpp.

55{ }

Member Function Documentation

◆ checkControlModeWithImCompliant()

void ControlModes::checkControlModeWithImCompliant ( int  desired_control_mode,
std::string  title 
)

Definition at line 373 of file ControlModes.cpp.

374{
375 char buff[500];
376 for (int i=0; i<n_cmd_joints; i++)
377 {
378 if(jointTorqueCtrlEnabled[jointsList[i]])
379 {
380 sprintf(buff, "joint %d has torque enabled. try to set %s and im_comp", jointsList[i], Vocab32::decode((NetInt32)desired_control_mode).c_str());
381 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
382 setModeSingle(jointsList[i],desired_control_mode,VOCAB_IM_COMPLIANT);
383 verifyModeSingle(jointsList[i], desired_control_mode,VOCAB_IM_COMPLIANT,title);
384 }
385 else
386 {
387 sprintf(buff, "joint %d doesn't support compliant interaction mode. test %s skipped", jointsList[i], title.c_str());
388 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
389 }
390 }
391
392}

◆ checkJointWithTorqueMode()

void ControlModes::checkJointWithTorqueMode ( )

Definition at line 264 of file ControlModes.cpp.

265{
266 yarp::os::Bottle b;
267 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(ivar->getRemoteVariable("torqueControlEnabled", b), "unable to get torqueControlEnabled");
268
269 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE((b.size() != n_part_joints), "torqueControlEnabled var returns joint num not corret.");
270
271 int j=0;
272 for(int i=0; i<b.size() && j<n_part_joints; i++)
273 {
274 yarp::os::Bottle *baux = b.get(i).asList();
275 for(int k=0; k<baux->size()&& j<n_part_joints; k++)
276 {
277 jointTorqueCtrlEnabled[j] = baux->get(k).asInt32();
278 j++;
279 }
280 }
281}

◆ executeCmd()

void ControlModes::executeCmd ( )

Definition at line 314 of file ControlModes.cpp.

315{
316 if (cmd_mode==single_joint)
317 {
318 for (int i=0; i<n_cmd_joints; i++)
319 {
320 idir->setPosition(jointsList[i],cmd_single);
321 }
322 }
323 else if (cmd_mode==some_joints)
324 {
325 for (int i=0; i<n_cmd_joints; i++)
326 {
327 cmd_some[i]=cmd_single;
328 }
329 idir->setPositions(n_cmd_joints,jointsList, cmd_some);
330 }
331 else if (cmd_mode==all_joints)
332 {
333 for (int i=0; i<n_part_joints; i++)
334 {
335 cmd_tot[i]=cmd_single;
336 }
337 idir->setPositions(cmd_tot);
338 }
339 else
340 {
341 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
342 }
343}

◆ getOriginalCurrentLimits()

void ControlModes::getOriginalCurrentLimits ( )

Definition at line 139 of file ControlModes.cpp.

140{
141 for (int i=0; i<n_cmd_joints; i++)
142 {
143 iamp->getMaxCurrent(jointsList[i],&prevcurr_some[i]);
144 }
145 for (int i=0; i<n_part_joints; i++)
146 {
147 iamp->getMaxCurrent(i,&prevcurr_tot[i]);
148 }
149 yarp::os::Time::delay(0.010);
150}

◆ goHome()

void ControlModes::goHome ( )

Definition at line 345 of file ControlModes.cpp.

346{
347 for (int i=0; i<n_cmd_joints; i++)
348 {
349 ipos->setRefSpeed(jointsList[i],20.0);
350 ipos->positionMove(jointsList[i],home_pos[i]);
351 }
352
353 int timeout = 0;
354 while (1)
355 {
356 int in_position=0;
357 for (int i=0; i<n_cmd_joints; i++)
358 {
359 ienc->getEncoder(jointsList[i],&pos_tot[jointsList[i]]);
360 if (fabs(pos_tot[jointsList[i]]-home_pos[i])<tolerance) in_position++;
361 }
362 if (in_position==n_cmd_joints) break;
363 if (timeout>100)
364 {
365 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
366 }
367 yarp::os::Time::delay(0.2);
368 timeout++;
369 }
370}

◆ resetOriginalCurrentLimits()

void ControlModes::resetOriginalCurrentLimits ( )

Definition at line 152 of file ControlModes.cpp.

153{
154 for (int i=0; i<n_part_joints; i++)
155 {
156 iamp->setMaxCurrent(i,prevcurr_tot[i]);
157 }
158 yarp::os::Time::delay(0.010);
159}

◆ run()

void ControlModes::run ( )
virtual

Definition at line 393 of file ControlModes.cpp.

394{
395 char buff[500];
396 getOriginalCurrentLimits();
397 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
398 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test0");
399 verifyAmplifier(0,"test0b"); //@@@@@@ To be completed
400 goHome();
401
402 //------ check all modes when stiff ------
403 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
404 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test1");
405 verifyAmplifier(0,"test1b");
406
407 setMode(VOCAB_CM_POSITION_DIRECT,VOCAB_IM_STIFF);
408 verifyMode(VOCAB_CM_POSITION_DIRECT,VOCAB_IM_STIFF,"test2");
409 verifyAmplifier(0,"test2b");
410
411 setMode(VOCAB_CM_VELOCITY,VOCAB_IM_STIFF);
412 verifyMode(VOCAB_CM_VELOCITY,VOCAB_IM_STIFF,"test3");
413 verifyAmplifier(0,"test3b");
414
415 //torque
416 for (int i=0; i<n_cmd_joints; i++)
417 {
418 if(jointTorqueCtrlEnabled[jointsList[i]])
419 {
420 sprintf(buff, "joint %d has torque enabled. try to set cm_torque and im_stiff", jointsList[i]);
421 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
422 setModeSingle(jointsList[i],VOCAB_CM_TORQUE,VOCAB_IM_STIFF);
423 verifyModeSingle(jointsList[i], VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test4");
424 verifyAmplifier(0,"test4b");
425 }
426 else
427 {
428 sprintf(buff, "joint %d doesn't support torque control mode. test test4 skipped", jointsList[i]);
429 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
430 }
431 }
432
433
434 setMode(VOCAB_CM_MIXED,VOCAB_IM_STIFF);
435 verifyMode(VOCAB_CM_MIXED,VOCAB_IM_STIFF,"test5");
436 verifyAmplifier(0,"test5b");
437
438 setMode(VOCAB_CM_PWM,VOCAB_IM_STIFF);
439 verifyMode(VOCAB_CM_PWM, VOCAB_IM_STIFF, "test6");
440 verifyAmplifier(0,"test6b");
441
442 setMode(VOCAB_CM_IDLE,VOCAB_IM_STIFF);
443 verifyMode(VOCAB_CM_IDLE,VOCAB_IM_STIFF,"test7");
444 verifyAmplifier(0,"test7b");
445
446 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
447 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test8");
448 verifyAmplifier(0,"test8b");
449
450 setMode(VOCAB_CM_FORCE_IDLE,VOCAB_IM_STIFF);
451 verifyMode(VOCAB_CM_IDLE,VOCAB_IM_STIFF,"test9"); //VOCAB_CM_IDLE is intentional
452 verifyAmplifier(0,"test9b");
453
454 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
455 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test10");
456 verifyAmplifier(0,"test10b");
457 goHome();
458
459 //------ check all modes when compliant ------
460 // note: not all joints can use compliant like interaction mode
461 for (int i=0; i<n_cmd_joints; i++)
462 {
463 if(jointTorqueCtrlEnabled[jointsList[i]])
464 {
465 sprintf(buff, "joint %d has torque enabled. try to set cm_torque and im_compliant", jointsList[i]);
466 ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
467 setModeSingle(jointsList[i],VOCAB_CM_POSITION,VOCAB_IM_COMPLIANT);
468 verifyModeSingle(jointsList[i], VOCAB_CM_POSITION,VOCAB_IM_COMPLIANT,"test11");
469 }
470 }
471
472
473 checkControlModeWithImCompliant(VOCAB_CM_POSITION, "test11");
474 verifyAmplifier(0,"test11b");
475
476
477 checkControlModeWithImCompliant(VOCAB_CM_POSITION_DIRECT,"test12");
478 verifyAmplifier(0,"test12b");
479
480 checkControlModeWithImCompliant(VOCAB_CM_VELOCITY,"test13");
481 verifyAmplifier(0,"test13b");
482
483 checkControlModeWithImCompliant(VOCAB_CM_TORQUE,"test4");
484 verifyAmplifier(0,"test14b");
485
486 checkControlModeWithImCompliant(VOCAB_CM_MIXED,"test15");
487 verifyAmplifier(0,"test15b");
488
489 checkControlModeWithImCompliant(VOCAB_CM_PWM, "test16");
490 verifyAmplifier(0,"test16b");
491
492 checkControlModeWithImCompliant(VOCAB_CM_IDLE,"test17");
493 verifyAmplifier(0,"test17b");
494
495 checkControlModeWithImCompliant(VOCAB_CM_POSITION,"test18");
496 verifyAmplifier(0,"test18b");
497
498 checkControlModeWithImCompliant(VOCAB_CM_IDLE,"test19"); //VOCAB_CM_IDLE is intentional
499 verifyAmplifier(0,"test19b");
500
501 checkControlModeWithImCompliant(VOCAB_CM_POSITION,"test20");
502 verifyAmplifier(0,"test20b");
503 goHome();
504
505 //------ create an intentional HW FAULT ------
506 zeroCurrentLimits();
507 verifyMode(VOCAB_CM_HW_FAULT, VOCAB_IM_STIFF,"test21");
508 verifyAmplifier(0,"test21b");
509 resetOriginalCurrentLimits();
510
511 //------ check all modes when in HW_FAULT ------
512 setMode(VOCAB_CM_POSITION,VOCAB_IM_COMPLIANT);
513 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test22");
514 verifyAmplifier(0,"test22b");
515
516 setMode(VOCAB_CM_POSITION_DIRECT,VOCAB_IM_COMPLIANT);
517 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test23");
518 verifyAmplifier(0,"test23b");
519
520 setMode(VOCAB_CM_VELOCITY,VOCAB_IM_COMPLIANT);
521 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test24");
522 verifyAmplifier(0,"test24b");
523
524 setMode(VOCAB_CM_TORQUE,VOCAB_IM_COMPLIANT);
525 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test25");
526 verifyAmplifier(0,"test25b");
527
528 setMode(VOCAB_CM_MIXED,VOCAB_IM_COMPLIANT);
529 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test26");
530 verifyAmplifier(0,"test26b");
531
532 setMode(VOCAB_CM_PWM, VOCAB_IM_COMPLIANT);
533 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test27");
534 verifyAmplifier(0,"test27b");
535
536 setMode(VOCAB_CM_IDLE,VOCAB_IM_COMPLIANT);
537 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test28");
538 verifyAmplifier(0,"test28b");
539
540 setMode(VOCAB_CM_POSITION,VOCAB_IM_COMPLIANT);
541 checkControlModeWithImCompliant(VOCAB_CM_HW_FAULT,"test29");
542 verifyAmplifier(0,"test29b");
543
544 setMode(VOCAB_CM_FORCE_IDLE,VOCAB_IM_COMPLIANT);
545 for (int i=0; i<n_cmd_joints; i++)
546 {
547 if(jointTorqueCtrlEnabled[jointsList[i]])
548 {
549 verifyModeSingle(jointsList[i], VOCAB_CM_IDLE,VOCAB_IM_COMPLIANT,"test30-comp");
550 }
551 else
552 {
553 verifyModeSingle(jointsList[i], VOCAB_CM_IDLE,VOCAB_IM_STIFF,"test30-stiff");
554 }
555 }
556 verifyAmplifier(0,"test30b");
557
558 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
559 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test31");
560 verifyAmplifier(0,"test31b");
561 goHome();
562}

◆ setMode()

void ControlModes::setMode ( int  desired_control_mode,
yarp::dev::InteractionModeEnum  desired_interaction_mode 
)

Definition at line 191 of file ControlModes.cpp.

192{
193 for (int i=0; i<n_cmd_joints; i++)
194 {
195 setModeSingle(jointsList[i], desired_control_mode, desired_interaction_mode);
196 }
197}

◆ setModeSingle()

void ControlModes::setModeSingle ( int  joint,
int  desired_control_mode,
yarp::dev::InteractionModeEnum  desired_interaction_mode 
)

Definition at line 199 of file ControlModes.cpp.

200{
201 icmd->setControlMode(joint,desired_control_mode);
202 iimd->setInteractionMode(joint,desired_interaction_mode);
203 yarp::os::Time::delay(0.010);
204}

◆ setup()

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

Definition at line 57 of file ControlModes.cpp.

57 {
58
59 if(property.check("name"))
60 setName(property.find("name").asString());
61
62 // updating parameters
63 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("home"), "The home positions must be given as the test parameter!");
67
68 robotName = property.find("robot").asString();
69 partName = property.find("part").asString();
70 if(property.check("tolerance"))
71 tolerance = property.find("tolerance").asFloat64();
72 else
73 tolerance = 0.5;
74
75 ROBOTTESTINGFRAMEWORK_TEST_REPORT(robottestingframework::Asserter::format("Tolerance of %.2f is used to check home position", tolerance));
76
77 Bottle* jointsBottle = property.find("joints").asList();
78 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
79 n_cmd_joints = jointsBottle->size();
80 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0,"invalid number of joints, it must be >0");
81
82 Property options;
83 options.put("device", "remote_controlboard");
84 options.put("remote", "/"+robotName+"/"+partName);
85 options.put("local", "/ControlModesTest/"+robotName+"/"+partName);
86
87 dd = new PolyDriver(options);
88 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
89 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(idir),"Unable to open position direct interface");
90 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
91 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iamp),"Unable to open ampliefier interface");
92 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
93 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
94 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
95 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ivel),"Unable to open velocity control interface");
96 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(itrq),"Unable to open torque control interface");
97 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ivar),"Unable to open remote variables interface");
98
99 if (!ienc->getAxes(&n_part_joints))
100 {
101 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
102 }
103
104 if (n_part_joints<=0)
105 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Error this part has in invalid (<=0) number of jonits");
106 else if (jointsBottle->size() == 1)
107 cmd_mode=single_joint;
108 else if (jointsBottle->size() < n_part_joints)
109 cmd_mode=some_joints;
110 else if (jointsBottle->size() == n_part_joints)
111 cmd_mode=all_joints;
112 else
113 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("invalid joint selection?");
114
115 cmd_tot = new double[n_part_joints];
116 pos_tot=new double[n_part_joints];
117 jointsList=new int[n_cmd_joints];
118 cmd_some=new double[n_cmd_joints];
119 prevcurr_tot=new double[n_part_joints];
120 prevcurr_some=new double[n_cmd_joints];
121 jointTorqueCtrlEnabled = new int[n_part_joints];
122 home_pos = new double[n_cmd_joints];
123
124 for (int i=0; i <n_cmd_joints; i++) jointsList[i]=jointsBottle->get(i).asInt32();
125
126 Bottle* homePosBottle = property.find("home").asList();
127 for (int i=0; i <n_cmd_joints; i++) home_pos[i]=homePosBottle->get(i).asFloat64();
128
129 checkJointWithTorqueMode();
130 return true;
131}

◆ tearDown()

void ControlModes::tearDown ( )
virtual

Definition at line 133 of file ControlModes.cpp.

134{
135 if (jointsList) {delete jointsList; jointsList =0;}
136 if (dd) {delete dd; dd =0;}
137}

◆ verifyAmplifier()

void ControlModes::verifyAmplifier ( int  desired_amplifier_mode,
std::string  title 
)

Definition at line 283 of file ControlModes.cpp.

284{
285 int amode;
286 int timeout = 0;
287
288 while (1)
289 {
290 int ok=0;
291 for (int i=0; i<n_cmd_joints; i++)
292 {
293 iamp->getAmpStatus (jointsList[i],&amode);
294 //@@@@@@
295 amode = 0; // to complete using the proper interface
296 //@@@@@@
297 if (amode==desired_amplifier_mode) ok++;
298 }
299 if (ok==n_cmd_joints) break;
300 if (timeout>100)
301 {
302 char sbuf[500];
303 sprintf(sbuf,"Test (%s) failed: amplifier mode is (%d), it should be (%d)",title.c_str(), amode, desired_amplifier_mode);
304 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
305 }
306 yarp::os::Time::delay(0.2);
307 timeout++;
308 }
309 char sbuf[500];
310 sprintf(sbuf,"Test (%s) passed: amplifier mode is (%d)",title.c_str(), desired_amplifier_mode);
311 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
312}

◆ verifyMode()

void ControlModes::verifyMode ( int  desired_control_mode,
yarp::dev::InteractionModeEnum  desired_interaction_mode,
std::string  title 
)

Definition at line 206 of file ControlModes.cpp.

207{
208 int cmode;
209 yarp::dev::InteractionModeEnum imode;
210 int timeout = 0;
211
212 while (1)
213 {
214 int ok=0;
215 for (int i=0; i<n_cmd_joints; i++)
216 {
217 icmd->getControlMode (jointsList[i],&cmode);
218 iimd->getInteractionMode(jointsList[i],&imode);
219 if (cmode==desired_control_mode && imode==desired_interaction_mode) ok++;
220 }
221 if (ok==n_cmd_joints) break;
222 if (timeout>100)
223 {
224 char sbuf[500];
225 sprintf(sbuf,"Test (%s) failed: current mode is (%s,%s), it should be (%s,%s)",title.c_str(), Vocab32::decode((NetInt32)cmode).c_str(), Vocab32::decode((NetInt32)imode).c_str(), Vocab32::decode((NetInt32)desired_control_mode).c_str(),Vocab32::decode((NetInt32)desired_interaction_mode).c_str());
226 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
227 }
228 yarp::os::Time::delay(0.2);
229 timeout++;
230 }
231 char sbuf[500];
232 sprintf(sbuf,"Test (%s) passed: current mode is (%s,%s)",title.c_str(),Vocab32::decode((NetInt32)desired_control_mode).c_str(), Vocab32::decode((NetInt32)desired_interaction_mode).c_str());
233 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
234}

◆ verifyModeSingle()

void ControlModes::verifyModeSingle ( int  joint,
int  desired_control_mode,
yarp::dev::InteractionModeEnum  desired_interaction_mode,
std::string  title 
)

Definition at line 236 of file ControlModes.cpp.

237{
238 int cmode;
239 yarp::dev::InteractionModeEnum imode;
240 int timeout = 0;
241
242 while (1)
243 {
244 bool ok = false;
245 icmd->getControlMode (joint,&cmode);
246 iimd->getInteractionMode(joint,&imode);
247 if (cmode==desired_control_mode && imode==desired_interaction_mode) ok=true;
248
249 if (ok) break;
250 if (timeout>100)
251 {
252 char sbuf[500];
253 sprintf(sbuf,"Test (%s) failed: current mode is (%s,%s), it should be (%s,%s)",title.c_str(), Vocab32::decode((NetInt32)cmode).c_str(), Vocab32::decode((NetInt32)imode).c_str(), Vocab32::decode((NetInt32)desired_control_mode).c_str(),Vocab32::decode((NetInt32)desired_interaction_mode).c_str());
254 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
255 }
256 yarp::os::Time::delay(0.2);
257 timeout++;
258 }
259 char sbuf[500];
260 sprintf(sbuf,"Test (%s) passed: current mode is (%s,%s)",title.c_str(),Vocab32::decode((NetInt32)desired_control_mode).c_str(), Vocab32::decode((NetInt32)desired_interaction_mode).c_str());
261 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
262}

◆ zeroCurrentLimits()

void ControlModes::zeroCurrentLimits ( )

Definition at line 161 of file ControlModes.cpp.

162{
163 if (cmd_mode==single_joint)
164 {
165 for (int i=0; i<n_cmd_joints; i++)
166 {
167 iamp->setMaxCurrent(jointsList[i],0.0);
168 }
169 }
170 else if (cmd_mode==some_joints)
171 {
172 for (int i=0; i<n_cmd_joints; i++)
173 {
174 iamp->setMaxCurrent(jointsList[i],0.0);
175 }
176 }
177 else if (cmd_mode==all_joints)
178 {
179 for (int i=0; i<n_part_joints; i++)
180 {
181 iamp->setMaxCurrent(i,0.0);
182 }
183 }
184 else
185 {
186 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
187 }
188 yarp::os::Time::delay(0.010);
189}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8