icub-test
ControlModes.cpp
1 /*
2  * iCub Robot Unit Tests (Robot Testing Framework)
3  *
4  * Copyright (C) 2015-2019 Istituto Italiano di Tecnologia (IIT)
5  *
6  * This library is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * This library is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with this library; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20 
21 #include <math.h>
22 #include <robottestingframework/TestAssert.h>
23 #include <robottestingframework/dll/Plugin.h>
24 #include <yarp/os/Time.h>
25 #include <yarp/os/Property.h>
26 #include <yarp/os/Vocab.h>
27 
28 #include "ControlModes.h"
29 
30 using namespace robottestingframework;
31 using namespace yarp::os;
32 using namespace yarp::dev;
33 
34 // prepare the plugin
35 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(ControlModes)
36 
37 ControlModes::ControlModes() : 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 }
54 
55 ControlModes::~ControlModes() { }
56 
57 bool ControlModes::setup(yarp::os::Property& property) {
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 }
132 
133 void ControlModes::tearDown()
134 {
135  if (jointsList) {delete jointsList; jointsList =0;}
136  if (dd) {delete dd; dd =0;}
137 }
138 
139 void ControlModes::getOriginalCurrentLimits()
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 }
151 
152 void ControlModes::resetOriginalCurrentLimits()
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 }
160 
161 void ControlModes::zeroCurrentLimits()
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 }
190 
191 void ControlModes::setMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
192 {
193  for (int i=0; i<n_cmd_joints; i++)
194  {
195  setModeSingle(jointsList[i], desired_control_mode, desired_interaction_mode);
196  }
197 }
198 
199 void ControlModes::setModeSingle(int joint, int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
200 {
201  icmd->setControlMode(joint,desired_control_mode);
202  iimd->setInteractionMode(joint,desired_interaction_mode);
203  yarp::os::Time::delay(0.010);
204 }
205 
206 void ControlModes::verifyMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
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 }
235 
236 void ControlModes::verifyModeSingle(int joint, int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
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 }
263 
264 void ControlModes::checkJointWithTorqueMode()
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 }
282 
283 void ControlModes::verifyAmplifier(int desired_amplifier_mode, std::string title)
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 }
313 
314 void ControlModes::executeCmd()
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 }
344 
345 void ControlModes::goHome()
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 }
371 
372 
373 void ControlModes::checkControlModeWithImCompliant(int desired_control_mode, std::string title)
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 }
393 void ControlModes::run()
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 }
ControlModes
The test checks if the joint is able to go in all the available control/interaction modes and if tran...
Definition: ControlModes.h:53
Generated by doxygen 1.9.1