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

Check IPositionControl and IEncoders. More...

#include <MotorTest.h>

Inherits yarp::robottestingframework::TestCase.

Public Member Functions

virtual bool setup (yarp::os::Property &configuration)
 
virtual void tearDown ()
 
virtual void run ()
 

Detailed Description

Check IPositionControl and IEncoders.

Check the following functions:

Accepts the following parameters:

Parameter name Type Units Default Value Required Description Notes
name string - "MotorTest" No The name of the test. -
portname string - - Yes The yarp port name of the controlboard to test. -
joints int - - Yes Number of axes in the controlboard. Must be consistent with the value returned by getAxes method.
target vector of doubles of size joints deg - Yes For each joint the position to reach for passing the test.
min vector of doubles of size joints deg - Yes For each joint the maximum lower error with respect to the target to consider the test as successful.
max vector of doubles of size joints deg - Yes For each joint the maximum upper error with respect to the target to consider the test as successful.
refvel vector of doubles of size joints deg/s - Yes For each joint the reference velocity value to set in the low level trajectory generator.
refacc vector of doubles of size joints deg/s^2 - No For each joint the reference acceleration value to set in the low level trajectory generator.
timeout vector of doubles of size joints s - Yes For each joint the maximum time to wait for the joint to reach the target.

Definition at line 59 of file MotorTest.h.

Constructor & Destructor Documentation

◆ MotorTest()

MotorTest::MotorTest ( )

Definition at line 37 of file MotorTest.cpp.

37 : yarp::robottestingframework::TestCase("MotorTest") {
38}

◆ ~MotorTest()

MotorTest::~MotorTest ( )
virtual

Definition at line 40 of file MotorTest.cpp.

40 {
41}

Member Function Documentation

◆ run()

void MotorTest::run ( )
virtual

Definition at line 159 of file MotorTest.cpp.

159 {
160
161 int nJoints=0;
162 bool doneAll=false;
163 bool ret=false;
164
165 ROBOTTESTINGFRAMEWORK_TEST_REPORT("checking joints number");
166 iEncoders->getAxes(&nJoints);
167 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(m_NumJoints==nJoints, "expected number of joints is inconsistent");
168
169 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking individual joints...");
170 for (int joint=0; joint<m_NumJoints; ++joint) {
171 ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Checking joint %d", joint));
172 if (m_aRefAcc!=NULL) {
173 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefAcceleration(joint, m_aRefAcc[joint]),
174 Asserter::format("setting reference acceleration on joint %d", joint));
175 }
176
177 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefSpeed(joint, m_aRefVel[joint]),
178 Asserter::format("setting reference speed on joint %d", joint));
179
180 // wait some time
181 double timeStart=yarp::os::Time::now();
182 double timeNow=timeStart;
183 bool read=false;
184
185 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking encoders");
186 while(timeNow<timeStart+m_aTimeout[joint] && !read) {
187 // read encoders
188 read=iEncoders->getEncoder(joint,m_aHome+joint);
189 yarp::os::Time::delay(0.1);
190 }
191 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(read, "getEncoder() returned true");
192
193 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->positionMove(joint, m_aTargetVal[joint]),
194 Asserter::format("moving joint %d to %.2lf", joint, m_aTargetVal[joint]));
195
196 doneAll=false;
197 ret=iPosition->checkMotionDone(joint, &doneAll);
198 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(!doneAll&&ret, "checking checkMotionDone returns false after position move");
199
200 ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Waiting timeout %.2lf", m_aTimeout[joint]));
201 bool reached=false;
202 while(timeNow<timeStart+m_aTimeout[joint] && !reached) {
203 double pos;
204 iEncoders->getEncoder(joint,&pos);
205 reached = yarp::robottestingframework::TestAsserter::isApproxEqual(pos, m_aTargetVal[joint], m_aMinErr[joint], m_aMaxErr[joint]);
206 timeNow=yarp::os::Time::now();
207 yarp::os::Time::delay(0.1);
208 }
209 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(reached, "reached position");
210 }
211
213 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Checking multiple joints...");
214 if (m_aRefAcc!=NULL) {
215 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefAccelerations(m_aRefAcc),
216 "setting reference acceleration on all joints");
217 }
218 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefSpeeds(m_aRefVel),
219 "setting reference speed on all joints");
220
221 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->positionMove(m_aHome),
222 "moving all joints to home");
223
224 doneAll=false;
225 // make sure that checkMotionDone return false right after a movement
226 ret=iPosition->checkMotionDone(&doneAll);
227 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(!doneAll&&ret, "checking checkMotionDone returns false after position move");
228
229 // wait some time
230 double timeStart=yarp::os::Time::now();
231 double timeNow=timeStart;
232
233 double timeout=m_aTimeout[0];
234 for(int j=0; j<m_NumJoints; j++)
235 {
236 if (timeout<m_aTimeout[j])
237 timeout=m_aTimeout[j];
238 }
239
240 ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Waiting timeout %.2lf", timeout));
241 bool reached=false;
242 double *encoders;
243 encoders=new double [m_NumJoints];
244 while(timeNow<timeStart+timeout && !reached) {
245 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iEncoders->getEncoders(encoders), "getEncoders()");
246 reached = yarp::robottestingframework::TestAsserter::isApproxEqual(encoders, m_aHome, m_aMinErr, m_aMaxErr, m_NumJoints);
247 timeNow=yarp::os::Time::now();
248 yarp::os::Time::delay(0.1);
249 }
250
251 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(reached, "reached position");
252
253 if(reached) {
254 // check checkMotionDone.
255 // because the previous movement was approximate, the robot
256 // could still be moving so we need to iterate a few times
257
258 int times=10;
259 bool doneAll=false;
260 bool ret=false;
261
262 while(times>0 && !doneAll) {
263 ret=iPosition->checkMotionDone(&doneAll);
264 if (!doneAll)
265 yarp::os::Time::delay(0.1);
266 times--;
267 }
268
269 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(doneAll&&ret, "checking checkMotionDone returns true");
270 }
271
272
273 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Now checking group interface");
274
275 //shuffle encoders
276 int *jmap=new int [m_NumJoints];
277 double *swapped_refvel=new double [m_NumJoints];
278 double *swapped_target=new double [m_NumJoints];
279
280 for(int kk=0;kk<m_NumJoints;kk++)
281 {
282 swapped_refvel[kk]=m_aRefVel[m_NumJoints-kk-1];
283 swapped_target[kk]=m_aTargetVal[m_NumJoints-kk-1];
284 jmap[kk]=m_NumJoints-kk-1;
285 }
286
287 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->setRefSpeeds(m_NumJoints, jmap, swapped_refvel),
288 "setting reference speed on all joints using group interface");
289
290 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(iPosition->positionMove(m_NumJoints, jmap, swapped_target),
291 "moving all joints to home using group interface");
292
293 ret=iPosition->checkMotionDone(m_NumJoints, jmap, &doneAll);
294 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(!doneAll&&ret, "checking checkMotionDone returns false after position move");
295
296 timeStart=yarp::os::Time::now();
297 timeNow=timeStart;
298
299 ROBOTTESTINGFRAMEWORK_TEST_REPORT(Asserter::format("Waiting timeout %.2lf", timeout));
300 reached=false;
301 while(timeNow<timeStart+timeout && !reached) {
302 iEncoders->getEncoders(encoders);
303 reached = yarp::robottestingframework::TestAsserter::isApproxEqual(encoders, m_aTargetVal, m_aMinErr, m_aMaxErr, m_NumJoints);
304 timeNow=yarp::os::Time::now();
305 yarp::os::Time::delay(0.1);
306 }
307 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(reached, "reached position");
308
309 if (reached) {
310 bool *done_vector=new bool [m_NumJoints];
311 // check checkMotionDone.
312 // because the previous movement was approximate, the robot
313 // could still be moving so we need to iterate a few times
314 int times=10;
315 bool doneAll=false;
316 bool ret=false;
317
318 while(times>0 && !doneAll) {
319 ret=iPosition->checkMotionDone(m_NumJoints, jmap, &doneAll);
320 if (!doneAll)
321 yarp::os::Time::delay(0.1);
322 times--;
323 }
324
325 ROBOTTESTINGFRAMEWORK_TEST_FAIL_IF_FALSE(doneAll&&ret, "checking checkMotionDone");
326 delete [] done_vector;
327 }
328
329 //cleanup
330 delete [] jmap;
331 delete [] swapped_refvel;
332 delete [] swapped_target;
333 delete [] encoders;
334}

◆ setup()

bool MotorTest::setup ( yarp::os::Property &  configuration)
virtual

Definition at line 43 of file MotorTest.cpp.

43 {
44
45 // initialization goes here ...
46 m_aTargetVal=NULL;
47 m_aMaxErr=NULL;
48 m_aMinErr=NULL;
49 m_aRefVel=NULL;
50 m_aRefAcc=NULL;
51 m_aTimeout=NULL;
52 m_aHome=NULL;
53 iEncoders=NULL;
54 iPosition=NULL;
55 m_initialized=false;
56
57 if(configuration.check("name"))
58 setName(configuration.find("name").asString());
59
60 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("portname"),
61 "Missing 'portname' parameter, cannot open device");
62 m_portname = configuration.find("portname").asString();
63
64 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("joints"),
65 "Missing 'joints' parameter, cannot open device");
66 m_NumJoints=configuration.find("joints").asInt32();
67
68 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("target"),
69 "Missing 'target' parameter, cannot open device");
70 yarp::os::Bottle bot=configuration.findGroup("target").tail();
71 int n = m_NumJoints<bot.size()? m_NumJoints:bot.size();
72 m_aTargetVal=new double[m_NumJoints];
73 m_aHome=new double [m_NumJoints];
74 for (int i=0; i<n; ++i) {
75 m_aTargetVal[i]=bot.get(i).asFloat64();
76 m_aHome[i]=0.0;
77 }
78
79 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("min"),
80 "Missing 'min' parameter, cannot open device");
81 bot = configuration.findGroup("min").tail();
82 n = m_NumJoints<bot.size()?m_NumJoints:bot.size();
83 m_aMinErr=new double[m_NumJoints];
84 for (int i=0; i<n; ++i)
85 m_aMinErr[i]=bot.get(i).asFloat64();
86
87 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("max"),
88 "Missing 'max' parameter, cannot open device");
89 bot=configuration.findGroup("max").tail();
90 n = m_NumJoints<bot.size()? m_NumJoints:bot.size();
91 m_aMaxErr=new double[m_NumJoints];
92 for (int i=0; i<n; ++i)
93 m_aMaxErr[i]=bot.get(i).asFloat64();
94
95 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("refvel"),
96 "Missing 'max' parameter, cannot open device");
97 bot = configuration.findGroup("refvel").tail();
98 n = m_NumJoints<bot.size()?m_NumJoints:bot.size();
99 m_aRefVel=new double[m_NumJoints];
100 for (int i=0; i<n; ++i)
101 m_aRefVel[i]=bot.get(i).asFloat64();
102
103 if(configuration.check("refacc")) {
104 bot = configuration.findGroup("refacc").tail();
105 n = m_NumJoints<bot.size()?m_NumJoints:bot.size();
106 m_aRefAcc=new double[m_NumJoints];
107 for (int i=0; i<n; ++i)
108 m_aRefAcc[i]=bot.get(i).asFloat64();
109 }
110
111 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(configuration.check("timeout"),
112 "Missing 'timeout' parameter, cannot open device");
113 bot = configuration.findGroup("timeout").tail();
114 n = m_NumJoints<bot.size()?m_NumJoints:bot.size();
115 m_aTimeout = new double[m_NumJoints];
116 for (int i=0; i<n; ++i)
117 m_aTimeout[i]=bot.get(i).asFloat64();
118
119 // opening interfaces
120 yarp::os::Property options;
121 options.put("device","remote_controlboard");
122 options.put("local",m_portname+"/client");
123 options.put("remote",m_portname);
124
125 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_driver.open(options),
126 "cannot open driver");
127
128 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_driver.view(iEncoders), "cannot view iEncoder");
129 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(m_driver.view(iPosition), "cannot view iPosition");
130
131 return true;
132}

◆ tearDown()

void MotorTest::tearDown ( )
virtual

Definition at line 134 of file MotorTest.cpp.

134 {
135 // finalization goes her ...
136 if(iPosition) {
137 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Homing robot");
138 iPosition->positionMove(m_aHome);
139
140 bool reached=false;
141 double timeStart=yarp::os::Time::now();
142 double timeNow=timeStart;
143 while(timeNow<timeStart+m_aTimeout[0] && !reached) {
144 iPosition->checkMotionDone(&reached);
145 timeNow=yarp::os::Time::now();
146 yarp::os::Time::delay(0.1);
147 }
148 }
149
150 if (m_aTargetVal) delete [] m_aTargetVal;
151 if (m_aMaxErr) delete [] m_aMaxErr;
152 if (m_aMinErr) delete [] m_aMinErr;
153 if (m_aRefVel) delete [] m_aRefVel;
154 if (m_aRefAcc) delete [] m_aRefAcc;
155 if (m_aTimeout) delete [] m_aTimeout;
156 if (m_aHome) delete [] m_aHome;
157}
The documentation for this class was generated from the following files:
Generated by doxygen 1.9.8