icub-test
Loading...
Searching...
No Matches
TorqueControlConsistency.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
27#include "TorqueControlConsistency.h"
28
29//example1 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0)"" --zero 0"
30//example2 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0 1 2)"" --zero 0 "
31//example3 -v -t TorqueControlConsistency.dll -p "--robot icub --part head --joints ""(0 1 2 3 4 5)"" --zero 0"
32
33using namespace robottestingframework;
34using namespace yarp::os;
35using namespace yarp::dev;
36
37// prepare the plugin
38ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(TorqueControlConsistency)
39
40TorqueControlConsistency::TorqueControlConsistency() : yarp::robottestingframework::TestCase("TorqueControlConsistency") {
41 jointsList=0;
42 pos_tot=0;
43 dd=0;
44 ipos=0;
45 iamp=0;
46 icmd=0;
47 iimd=0;
48 ienc=0;
49 itrq=0;
50 cmd_some=0;
51 cmd_tot=0;
52 prevcurr_some=0;
53 prevcurr_tot=0;
54}
55
56TorqueControlConsistency::~TorqueControlConsistency() { }
57
58bool TorqueControlConsistency::setup(yarp::os::Property& property) {
59
60 //updating the test name
61 if(property.check("name"))
62 setName(property.find("name").asString());
63
64 // updating parameters
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
67 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
68 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("zero"), "The zero position must be given as the test parameter!");
69
70 robotName = property.find("robot").asString();
71 partName = property.find("part").asString();
72
73 zero = property.find("zero").asFloat64();
74
75 Bottle* jointsBottle = property.find("joints").asList();
76 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
77 n_cmd_joints = jointsBottle->size();
78 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(n_cmd_joints>0,"invalid number of joints, it must be >0");
79
80 Property options;
81 options.put("device", "remote_controlboard");
82 options.put("remote", "/"+robotName+"/"+partName);
83 options.put("local", "/TorqueControlConsistencyTest/"+robotName+"/"+partName);
84
85 dd = new PolyDriver(options);
86 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
87 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(itrq),"Unable to open torque control interface");
88 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
89 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iamp),"Unable to open ampliefier interface");
90 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
91 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
92 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
93
94 if (!ienc->getAxes(&n_part_joints))
95 {
96 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
97 }
98
99 if (n_part_joints<=0)
100 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Error this part has in invalid (<=0) number of jonits");
101 else if (jointsBottle->size() == 1)
102 cmd_mode=single_joint;
103 else if (jointsBottle->size() < n_part_joints)
104 cmd_mode=some_joints;
105 else if (jointsBottle->size() == n_part_joints)
106 cmd_mode=all_joints;
107 else
108 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("invalid joint selection?");
109
110 cmd_tot = new double[n_part_joints];
111 pos_tot=new double[n_part_joints];
112 jointsList=new int[n_cmd_joints];
113 cmd_some=new double[n_cmd_joints];
114 prevcurr_tot=new double[n_part_joints];
115 prevcurr_some=new double[n_cmd_joints];
116 for (int i=0; i <n_cmd_joints; i++) jointsList[i]=jointsBottle->get(i).asInt32();
117
118 return true;
119}
120
121void TorqueControlConsistency::tearDown()
122{
123 if (jointsList) {delete jointsList; jointsList =0;}
124 if (dd) {delete dd; dd =0;}
125}
126
127void TorqueControlConsistency::setMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
128{
129 for (int i=0; i<n_cmd_joints; i++)
130 {
131 icmd->setControlMode(jointsList[i],desired_control_mode);
132 iimd->setInteractionMode(jointsList[i],desired_interaction_mode);
133 yarp::os::Time::delay(0.010);
134 }
135}
136
137void TorqueControlConsistency::verifyMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
138{
139 int cmode;
140 yarp::dev::InteractionModeEnum imode;
141 int timeout = 0;
142
143 while (1)
144 {
145 int ok=0;
146 for (int i=0; i<n_cmd_joints; i++)
147 {
148 icmd->getControlMode (jointsList[i],&cmode);
149 iimd->getInteractionMode(jointsList[i],&imode);
150 if (cmode==desired_control_mode && imode==desired_interaction_mode) ok++;
151 }
152 if (ok==n_cmd_joints) break;
153 if (timeout>100)
154 {
155 char sbuf[500];
156 sprintf(sbuf,"Test (%s) failed: current mode is (%d,%d), it should be (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode,cmode,imode);
157 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
158 }
159 yarp::os::Time::delay(0.2);
160 timeout++;
161 }
162 char sbuf[500];
163 sprintf(sbuf,"Test (%s) passed: current mode is (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode);
164 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
165}
166
167void TorqueControlConsistency::goHome()
168{
169 for (int i=0; i<n_cmd_joints; i++)
170 {
171 ipos->setRefSpeed(jointsList[i],20.0);
172 ipos->positionMove(jointsList[i],zero);
173 }
174
175 int timeout = 0;
176 while (1)
177 {
178 int in_position=0;
179 for (int i=0; i<n_cmd_joints; i++)
180 {
181 ienc->getEncoder(jointsList[i],&pos_tot[jointsList[i]]);
182 if (fabs(pos_tot[jointsList[i]]-zero)<0.5) in_position++;
183 }
184 if (in_position==n_cmd_joints) break;
185 if (timeout>100)
186 {
187 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Timeout while reaching zero position");
188 }
189 yarp::os::Time::delay(0.2);
190 timeout++;
191 }
192}
193
194void TorqueControlConsistency::setRefTorque(double value)
195{
196 cmd_single=value;
197 if (cmd_mode==single_joint)
198 {
199 for (int i=0; i<n_cmd_joints; i++)
200 {
201 itrq->setRefTorque(jointsList[i],cmd_single);
202 }
203 }
204 else if (cmd_mode==some_joints)
205 {
206 //same of single_joint, since multiple joint is not currently supported
207 for (int i=0; i<n_cmd_joints; i++)
208 {
209 itrq->setRefTorque(jointsList[i],cmd_single);
210 }
211 }
212 else if (cmd_mode==all_joints)
213 {
214 for (int i=0; i<n_part_joints; i++)
215 {
216 cmd_tot[i]=cmd_single;
217 }
218 itrq->setRefTorques(cmd_tot);
219 }
220 else
221 {
222 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
223 }
224 yarp::os::Time::delay(0.010);
225}
226
227void TorqueControlConsistency::verifyRefTorque(double verify_val, std::string title)
228{
229 double value;
230 char sbuf[500];
231 if (cmd_mode==single_joint)
232 {
233 for (int i=0; i<n_cmd_joints; i++)
234 {
235 itrq->getRefTorque(jointsList[i],&value);
236 if (value==verify_val)
237 {
238 sprintf(sbuf,"Test (%s) passed, j%d current reference is (%f)",title.c_str(),i, verify_val);
239 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
240 }
241 else
242 {
243 sprintf(sbuf,"Test (%s) failed: current reference is (%f), it should be (%f)",title.c_str(), value, verify_val);
244 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
245 }
246 }
247 }
248 else if (cmd_mode==some_joints)
249 {
250 //same of single_joint, since multiple joint is not currently supported
251 for (int i=0; i<n_cmd_joints; i++)
252 {
253 itrq->getRefTorque(jointsList[i],&value);
254 if (value==verify_val)
255 {
256 sprintf(sbuf,"Test (%s) passed j%d current reference is (%f)",title.c_str(),i, verify_val);
257 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
258 }
259 else
260 {
261 sprintf(sbuf,"Test (%s) failed: current reference is (%f), it should be (%f)",title.c_str(), value, verify_val);
262 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
263 }
264 }
265 }
266 else if (cmd_mode==all_joints)
267 {
268 int ok=0;
269 itrq->getRefTorques(cmd_tot);
270 for (int i=0; i<n_part_joints; i++)
271 {
272 if (verify_val==cmd_tot[i]) ok++;
273 }
274 if (ok==n_part_joints)
275 {
276 sprintf(sbuf,"Test (%s) passed, current reference is (%f)",title.c_str(), verify_val);
277 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
278 }
279 else
280 {
281 sprintf(sbuf,"Test (%s) failed: only %d joints (of %d) are ok",title.c_str(),ok,n_part_joints);
282 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
283 }
284 }
285 else
286 {
287 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("Invalid cmd_mode");
288 }
289 yarp::os::Time::delay(0.010);
290}
291
292void TorqueControlConsistency::run()
293{
294 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
295 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test0");
296 goHome();
297
298 setMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF);
299 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test1");
300 setRefTorque(0.1);
301 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test2");
302 verifyRefTorque(0.1,"test2a");
303 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test2b");
304
305 setRefTorque(0);
306 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test3");
307 verifyRefTorque(0,"test3a");
308 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test3b");
309 setRefTorque(-0.1);
310 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test4");
311 verifyRefTorque(-0.1,"test4a");
312 verifyMode(VOCAB_CM_TORQUE,VOCAB_IM_STIFF,"test4b");
313
314 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
315 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test5");
316 goHome();
317
318}
Generated by doxygen 1.9.8