icub-test
Loading...
Searching...
No Matches
MotorStiction.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/math/Math.h>
26#include <yarp/os/Property.h>
27#include <fstream>
28#include <algorithm>
29#include <cstdlib>
30#include "MotorStiction.h"
31
32//example1 -v -t MotorStiction.dll -p "--robot icub --part left_arm --joints ""(4)"" --home ""(45)"" --outputStep ""(0.5)"" --outputMax ""(50)"" --outputDelay ""(2.0)"" --threshold ""(5.0)"" "
33
34using namespace robottestingframework;
35using namespace yarp::os;
36using namespace yarp::dev;
37
38// prepare the plugin
39ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(MotorStiction)
40
41MotorStiction::MotorStiction() : yarp::robottestingframework::TestCase("MotorStiction") {
42 jointsList=0;
43 dd=0;
44 ipos=0;
45 iamp=0;
46 icmd=0;
47 iimd=0;
48 ienc=0;
49 ipwm = 0;
50}
51
52MotorStiction::~MotorStiction() { }
53
54bool MotorStiction::setup(yarp::os::Property& property) {
55 if(property.check("name"))
56 setName(property.find("name").asString());
57
58 // updating parameters
59 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("robot"), "The robot name must be given as the test parameter!");
60 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("part"), "The part name must be given as the test parameter!");
61 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("joints"), "The joints list must be given as the test parameter!");
62 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("home"), "The home position must be given as the test parameter!");
63
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outputStep"), "The output_step must be given as the test parameter!");
65 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outputDelay") , "The output_delay must be given as the test parameter!");
66 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("outputMax"), "The output_max must be given as the test parameter!");
67 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("threshold"), "The threshold must be given as the test parameter!");
68 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(property.check("repeat"), "The repeat must be given as the test parameter!");
69
70 robotName = property.find("robot").asString();
71 partName = property.find("part").asString();
72
73 repeat = property.find("repeat").asInt32();
74 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(repeat>=0,"repeat must be greater than zero");
75
76 Bottle* homeBottle = property.find("home").asList();
77 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(homeBottle!=0,"unable to parse zero parameter");
78
79 Bottle* jointsBottle = property.find("joints").asList();
80 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(jointsBottle!=0,"unable to parse joints parameter");
81
82 Bottle* output_step_Bottle = property.find("outputStep").asList();
83 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(output_step_Bottle!=0,"unable to parse joints parameter");
84
85 Bottle* output_delay_Bottle = property.find("outputDelay").asList();
86 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(output_delay_Bottle!=0,"unable to parse joints parameter");
87
88 Bottle* output_max_Bottle = property.find("outputMax").asList();
89 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(output_max_Bottle!=0,"unable to parse joints parameter");
90
91 Bottle* threshold_Bottle = property.find("threshold").asList();
92 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(threshold_Bottle!=0,"unable to parse joints parameter");
93
94 Property options;
95 options.put("device", "remote_controlboard");
96 options.put("remote", "/"+robotName+"/"+partName);
97 options.put("local", "/MotorStictionTest/"+robotName+"/"+partName);
98
99 dd = new PolyDriver(options);
100 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->isValid(),"Unable to open device driver");
101 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipwm),"Unable to open pwm control interface");
102 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ienc),"Unable to open encoders interface");
103 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iamp),"Unable to open ampliefier interface");
104 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ipos),"Unable to open position interface");
105 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(icmd),"Unable to open control mode interface");
106 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(iimd),"Unable to open interaction mode interface");
107 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(dd->view(ilim),"Unable to open limits interface");
108
109 if (!ienc->getAxes(&n_part_joints))
110 {
111 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR("unable to get the number of joints of the part");
112 }
113
114 int n_cmd_joints = jointsBottle->size();
115 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");
116 for (int i=0; i <n_cmd_joints; i++) jointsList.push_back(jointsBottle->get(i).asInt32());
117
118 home.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) home[i]=homeBottle->get(i).asFloat64();
119 opl_step.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) opl_step[i]=output_step_Bottle->get(i).asFloat64();
120 opl_delay.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) opl_delay[i]=output_delay_Bottle->get(i).asFloat64();
121 opl_max.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) opl_max[i]=output_max_Bottle->get(i).asFloat64();
122 movement_threshold.resize (n_cmd_joints); for (int i=0; i< n_cmd_joints; i++) movement_threshold[i]=threshold_Bottle->get(i).asFloat64();
123
124 max_lims.resize(n_cmd_joints);
125 min_lims.resize(n_cmd_joints);
126 for (int i=0; i <n_cmd_joints; i++) ilim->getLimits((int)jointsList[i],&min_lims[i],&max_lims[i]);
127
128 return true;
129}
130
131void MotorStiction::tearDown()
132{
133 char buff[500];
134 sprintf(buff,"Closing test module");ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
135 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
136 verifyMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF,"test0");
137 goHome();
138 if (dd) {delete dd; dd =0;}
139}
140
141void MotorStiction::setMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
142{
143 for (unsigned int i=0; i<jointsList.size(); i++)
144 {
145 icmd->setControlMode((int)jointsList[i],desired_control_mode);
146 iimd->setInteractionMode((int)jointsList[i],desired_interaction_mode);
147 yarp::os::Time::delay(0.010);
148 }
149}
150
151void MotorStiction::setModeSingle(int i, int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode)
152{
153 icmd->setControlMode((int)jointsList[i],desired_control_mode);
154 iimd->setInteractionMode((int)jointsList[i],desired_interaction_mode);
155 yarp::os::Time::delay(0.010);
156}
157
158void MotorStiction::verifyMode(int desired_control_mode, yarp::dev::InteractionModeEnum desired_interaction_mode, std::string title)
159{
160 int cmode;
161 yarp::dev::InteractionModeEnum imode;
162 int timeout = 0;
163
164 while (1)
165 {
166 int ok=0;
167 for (unsigned int i=0; i<jointsList.size(); i++)
168 {
169 icmd->getControlMode ((int)jointsList[i],&cmode);
170 iimd->getInteractionMode((int)jointsList[i],&imode);
171 if (cmode==desired_control_mode && imode==desired_interaction_mode) ok++;
172 }
173 if (ok==jointsList.size()) break;
174 if (timeout>100)
175 {
176 char sbuf[500];
177 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);
178 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(sbuf);
179 }
180 yarp::os::Time::delay(0.2);
181 timeout++;
182 }
183 char sbuf[500];
184 sprintf(sbuf,"Test (%s) passed: current mode is (%d,%d)",title.c_str(), desired_control_mode,desired_interaction_mode);
185 ROBOTTESTINGFRAMEWORK_TEST_REPORT(sbuf);
186}
187
188void MotorStiction::goHome()
189{
190 for (unsigned int i=0; i<jointsList.size(); i++)
191 {
192 ipos->setRefSpeed((int)jointsList[i],20.0);
193 ipos->positionMove((int)jointsList[i],home[i]);
194 yarp::os::Time::delay(0.010);
195 }
196
197 char buff [500];
198 sprintf(buff,"Homing the whole part");ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
199
200 int in_position=0;
201 for (unsigned int i=0; i<jointsList.size(); i++)
202 {
203 double time_started = yarp::os::Time::now();
204 while (1)
205 {
206 double pos;
207 ienc->getEncoder((int)jointsList[i],&pos);
208 if (fabs(pos-home[i])<1.0) break;
209 if (yarp::os::Time::now()-time_started>20)
210 {
211 sprintf(buff,"Timeout while reaching zero position, joint %d, curr_enc %f, home %f", (int)jointsList[i],pos,home[i]);
212 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
213 }
214 }
215 }
216
217 sprintf(buff,"Homing succesfully completed");ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
218}
219
220void MotorStiction::saveToFile(std::string filename, yarp::os::Bottle &b)
221{
222 std::fstream fs;
223 fs.open (filename.c_str(), std::fstream::out);
224
225 for (int i=0; i<b.size(); i++)
226 {
227 std::string s = b.get(i).toString();
228 std::replace(s.begin(), s.end(), '(', ' ');
229 std::replace(s.begin(), s.end(), ')', ' ');
230 fs << s << std::endl;
231 }
232
233 fs.close();
234}
235
236void MotorStiction::OplExecute(int i, std::vector<yarp::os::Bottle>& dataToPlotList, stiction_data& current_test, bool positive_sign)
237{
238 char buff[500];
239 double time = yarp::os::Time::now();
240 double time_old = yarp::os::Time::now();
241 double enc=0;
242 double start_enc=0;
243 ienc->getEncoder((int)jointsList[i],&enc);
244 ienc->getEncoder((int)jointsList[i],&start_enc);
245 bool not_moving = true;
246 double opl=0;
247 setMode(VOCAB_CM_PWM, VOCAB_IM_STIFF);
248 ipwm->setRefDutyCycle((int)jointsList[i], opl);
249 double last_opl_cmd=yarp::os::Time::now();
250 Bottle dataToPlot;
251
252 while (not_moving)
253 {
254 Bottle& row = dataToPlot.addList();
255 Bottle& v1 = row.addList();
256 Bottle& v2 = row.addList();
257
258 ipwm->setRefDutyCycle((int)jointsList[i],opl);
259 ienc->getEncoder((int)jointsList[i],&enc);
260
261 //sprintf(buff,"%f %f %f %f",enc,start_enc,fabs(enc-start_enc),movement_threshold[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
262
263 if (fabs(enc-start_enc)>movement_threshold[i])
264 {
265 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
266 not_moving=false;
267 if (positive_sign) {current_test.pos_opl=opl; current_test.pos_test_passed=true;}
268 else {current_test.neg_opl=opl; current_test.neg_test_passed=true;}
269 dataToPlotList.push_back(dataToPlot);
270 sprintf(buff,"Test success (output=%f)",opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
271 }
272 else if (opl>=opl_max[i])
273 {
274 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
275 not_moving=false;
276 if (positive_sign) {current_test.pos_opl=opl; current_test.pos_test_passed=false;}
277 else {current_test.neg_opl=opl; current_test.neg_test_passed=false;}
278 dataToPlotList.push_back(dataToPlot);
279 sprintf(buff,"Test failed failed because max output was reached(output=%f)",opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
280 }
281 else if (fabs(enc-max_lims[i]) < 1.0 ||
282 fabs(enc-min_lims[i]) < 1.0 )
283 {
284 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
285 not_moving=false;
286 if (positive_sign) {current_test.pos_opl=opl; current_test.pos_test_passed=false;}
287 else {current_test.neg_opl=opl; current_test.neg_test_passed=false;}
288 dataToPlotList.push_back(dataToPlot);
289 sprintf(buff,"Test failed because hw limit was touched (enc=%f)",enc);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
290 }
291
292 if (yarp::os::Time::now()-last_opl_cmd>opl_delay[i])
293 {
294 if (positive_sign)
295 {opl+=opl_step[i];}
296 else
297 {opl-=opl_step[i];}
298 last_opl_cmd=yarp::os::Time::now();
299 }
300
301 time = yarp::os::Time::now();
302 v1.addFloat64(time);
303 v2.addFloat64(enc);
304 v2.addFloat64(opl);
305 yarp::os::Time::delay(0.010);
306
307 if (time-time_old>5.0 && not_moving==true)
308 {
309 sprintf(buff,"test in progress on joint %d, current output value = %f",(int)jointsList[i],opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
310 time_old=time;
311 }
312 }
313}
314
315void MotorStiction::OplExecute2(int i, std::vector<yarp::os::Bottle>& dataToPlotList, stiction_data& current_test, bool positive_sign)
316{
317 char buff[500];
318 double time = yarp::os::Time::now();
319 double time_old = yarp::os::Time::now();
320 double enc=0;
321 double start_enc=0;
322 ienc->getEncoder((int)jointsList[i],&enc);
323 ienc->getEncoder((int)jointsList[i],&start_enc);
324 bool not_moving = true;
325 double opl=0;
326 setMode(VOCAB_CM_PWM,VOCAB_IM_STIFF);
327 ipwm->setRefDutyCycle((int)jointsList[i], opl);
328 double last_opl_cmd=yarp::os::Time::now();
329 Bottle dataToPlot;
330
331 while (not_moving)
332 {
333 Bottle& row = dataToPlot.addList();
334 Bottle& v1 = row.addList();
335 Bottle& v2 = row.addList();
336
337 ipwm->setRefDutyCycle((int)jointsList[i], opl);
338 ienc->getEncoder((int)jointsList[i],&enc);
339
340 //sprintf(buff,"%f %f %f %f",enc,start_enc,fabs(enc-start_enc),movement_threshold[i]);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
341
342 if (opl>=opl_max[i])
343 {
344 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
345 not_moving=false;
346 if (positive_sign) {current_test.pos_opl=opl; current_test.pos_test_passed=false;}
347 else {current_test.neg_opl=opl; current_test.neg_test_passed=false;}
348 dataToPlotList.push_back(dataToPlot);
349 sprintf(buff,"Test failed failed because max output was reached(output=%f)",opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
350 }
351 else if (fabs(enc-max_lims[i]) < 1.0 ||
352 fabs(enc-min_lims[i]) < 1.0 )
353 {
354 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
355 not_moving=false;
356 if (positive_sign) {current_test.pos_opl=opl; current_test.pos_test_passed=true;}
357 else {current_test.neg_opl=opl; current_test.neg_test_passed=true;}
358 dataToPlotList.push_back(dataToPlot);
359 sprintf(buff,"Test success (output=%f)",opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
360 }
361
362 if (yarp::os::Time::now()-last_opl_cmd>opl_delay[i])
363 {
364 if (positive_sign)
365 {opl+=opl_step[i];}
366 else
367 {opl-=opl_step[i];}
368 last_opl_cmd=yarp::os::Time::now();
369 }
370
371 time = yarp::os::Time::now();
372 v1.addFloat64(time);
373 v2.addFloat64(enc);
374 v2.addFloat64(opl);
375 yarp::os::Time::delay(0.010);
376
377 if (time-time_old>5.0 && not_moving==true)
378 {
379 sprintf(buff,"test in progress on joint %d, current output value = %f",(int)jointsList[i],opl);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
380 time_old=time;
381 }
382 }
383}
384
385void MotorStiction::run()
386{
387 //yarp::os::Time::delay(10);
388
389 char buff[500];
390 std::vector<Bottle> dataToPlotList;
391
392 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
393 goHome();
394
395 for (unsigned int i=0 ; i<jointsList.size(); i++)
396 {
397 for (int repeat_count=0; repeat_count<repeat; repeat_count++)
398 {
399 stiction_data current_test;
400 current_test.jnt=(int)jointsList[i];
401 current_test.cycle= repeat_count;
402
403 setModeSingle(i,VOCAB_CM_PWM,VOCAB_IM_STIFF);
404 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
405
406 sprintf(buff,"Testing joint %d, cycle %d, positive output",(int)jointsList[i],repeat_count);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
407 OplExecute(i,dataToPlotList,current_test, true);
408
409 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
410 goHome();
411
412 setModeSingle(i, VOCAB_CM_PWM,VOCAB_IM_STIFF);
413 ipwm->setRefDutyCycle((int)jointsList[i], 0.0);
414
415 sprintf(buff,"Testing joint %d, cycle %d, negative output",(int)jointsList[i],repeat_count);ROBOTTESTINGFRAMEWORK_TEST_REPORT(buff);
416 OplExecute(i,dataToPlotList,current_test, false);
417
418 setMode(VOCAB_CM_POSITION,VOCAB_IM_STIFF);
419 goHome();
420
421 //test cycle complete, save data
422 stiction_data_list.push_back(current_test);
423
424 char filename[500];
425 sprintf (filename, "plot_stiction_%s_j%d_n_c%d.txt",partName.c_str(),(int)jointsList[i],repeat_count);
426 saveToFile(filename,dataToPlotList.rbegin()[0]); //last element
427 sprintf (filename, "plot_stiction_%s_j%d_p_c%d.txt",partName.c_str(),(int)jointsList[i],repeat_count);
428 saveToFile(filename,dataToPlotList.rbegin()[1]); //second last element
429 }
430 }
431
432 goHome();
433
434 for (unsigned int i=0 ; i<jointsList.size(); i++)
435 {
436 char filename[500];
437 char plotstring[1000];
438 sprintf (filename, "plot_stiction_j%d.txt",(int)jointsList[i]);
439 //gnuplot -e "unset key; plot for [col=1:6] 'C:\software\icub-tests\build\plugins\Debug\plot.txt' using col with lines" -persist
440 sprintf (plotstring, "gnuplot -e \" unset key; plot for [col=1:%d] '%s' using col with lines \" -persist", n_part_joints,filename);
441 //system (plotstring);
442 }
443
444 //stiction_data_list.size() include tests for all joints, multiple cycles
445 for (unsigned int i=0; i <stiction_data_list.size(); i++)
446 {
447 if (stiction_data_list[i].neg_test_passed==false)
448 {
449 char buff [500];
450 sprintf(buff, "test failed on joint %d, cycle %d, negative output value: %f",stiction_data_list[i].jnt,stiction_data_list[i].cycle,stiction_data_list[i].neg_opl);
451 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
452 }
453 else if (stiction_data_list[i].pos_test_passed==false)
454 {
455 char buff [500];
456 sprintf(buff, "test failed on joint %d, cycle %d, positive output value: %f",stiction_data_list[i].jnt,stiction_data_list[i].cycle,stiction_data_list[i].pos_opl);
457 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(buff);
458 }
459 }
460
461}
Generated by doxygen 1.9.8