icub-test
Loading...
Searching...
No Matches
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
30using namespace robottestingframework;
31using namespace yarp::os;
32using namespace yarp::dev;
33
34// prepare the plugin
35ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(ControlModes)
36
37ControlModes::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
55ControlModes::~ControlModes() { }
56
57bool 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
133void ControlModes::tearDown()
134{
135 if (jointsList) {delete jointsList; jointsList =0;}
136 if (dd) {delete dd; dd =0;}
137}
138
139void 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
152void 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
161void 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
191void 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
199void 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
206void 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
236void 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
264void 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
283void 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
314void 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
345void 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
373void 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}
393void 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.8