icub-test
Loading...
Searching...
No Matches
DemoRedBallTest.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 <memory>
22#include <algorithm>
23#include <robottestingframework/dll/Plugin.h>
24#include <robottestingframework/TestAssert.h>
25#include <yarp/os/Time.h>
26#include <yarp/os/Network.h>
27#include <yarp/os/BufferedPort.h>
28#include <yarp/os/ResourceFinder.h>
29#include <yarp/dev/GazeControl.h>
30#include <yarp/sig/Matrix.h>
31#include <yarp/math/Math.h>
32
33#include <iCub/ctrl/filters.h>
34
35#include "DemoRedBallTest.h"
36
37using namespace std;
38using namespace robottestingframework;
39using namespace yarp::os;
40using namespace yarp::dev;
41using namespace yarp::sig;
42using namespace yarp::math;
43using namespace iCub::ctrl;
44
45// prepare the plugin
46ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(DemoRedBallTest)
47
48
49/***********************************************************************************/
50DemoRedBallTest::DemoRedBallTest() : yarp::robottestingframework::TestCase("DemoRedBallTest")
51{
52}
53
54
55/***********************************************************************************/
56DemoRedBallTest::~DemoRedBallTest()
57{
58}
59
60
61/***********************************************************************************/
62bool DemoRedBallTest::setup(Property &property)
63{
64 Time::useNetworkClock("/clock");
65
66 string context=property.check("context",Value("demoRedBall")).asString();
67 string from=property.check("from",Value("config-test.ini")).asString();
68
69 // retrieve demoRedBall parameters
70 ResourceFinder rf;
71 rf.setDefaultContext(context.c_str());
72 rf.setDefaultConfigFile(from.c_str());
73 rf.configure(0,NULL);
74
75 // fallback values
76 params.robot="icubSim";
77 params.eye="left";
78 params.reach_tol=0.01;
79 params.use_torso=true;
80 params.use_left=true;
81 params.use_right=true;
82 params.home_arm.resize(7,0.0);
83
84 Bottle &general=rf.findGroup("general");
85 if (!general.isNull())
86 {
87 params.robot=general.check("robot",Value(params.robot)).asString();
88 params.eye=general.check("eye",Value(params.eye)).asString();
89 params.reach_tol=general.check("reach_tol",Value(params.reach_tol)).asFloat64();
90 params.use_torso=(general.check("torso",Value(params.use_torso?"on":"off")).asString()=="on");
91 params.use_left=(general.check("left_arm",Value(params.use_left?"on":"off")).asString()=="on");
92 params.use_right=(general.check("right_arm",Value(params.use_right?"on":"off")).asString()=="on");
93 }
94
95 Bottle &home_arm=rf.findGroup("home_arm");
96 if (!home_arm.isNull())
97 {
98 if (home_arm.check("poss"))
99 {
100 Bottle &poss=home_arm.findGroup("poss");
101 for (size_t i=0; i<std::min(params.home_arm.length(),(size_t)poss.size()-1); i++)
102 params.home_arm[i]=poss.get(1+i).asFloat64();
103 }
104 }
105
106 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Opening Clients");
107 if (params.use_left)
108 {
109 Property optJoint;
110 optJoint.put("device","remote_controlboard");
111 optJoint.put("remote",("/"+params.robot+"/"+"left_arm"));
112 optJoint.put("local",("/"+getName()+"/joint/left_arm"));
113
114 Property optCart;
115 optCart.put("device","cartesiancontrollerclient");
116 optCart.put("remote",("/"+params.robot+"/"+"cartesianController/left_arm"));
117 optCart.put("local",("/"+getName()+"/cartesian/left_arm"));
118
119 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvJointArmL.open(optJoint)&&drvCartArmL.open(optCart),
120 "Unable to open clients for left_arm!");
121 }
122
123 if (params.use_right)
124 {
125 Property optJoint;
126 optJoint.put("device","remote_controlboard");
127 optJoint.put("remote",("/"+params.robot+"/"+"right_arm"));
128 optJoint.put("local",("/"+getName()+"/joint/right_arm"));
129
130 Property optCart;
131 optCart.put("device","cartesiancontrollerclient");
132 optCart.put("remote",("/"+params.robot+"/"+"cartesianController/right_arm"));
133 optCart.put("local",("/"+getName()+"/cartesian/right_arm"));
134
135 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvJointArmR.open(optJoint)&&drvCartArmR.open(optCart),
136 "Unable to open clients for right_arm!");
137 }
138
139 {
140 Property optJoint;
141 optJoint.put("device","remote_controlboard");
142 optJoint.put("remote",("/"+params.robot+"/"+"head"));
143 optJoint.put("local",("/"+getName()+"/joint/head"));
144
145 Property optGaze;
146 optGaze.put("device","gazecontrollerclient");
147 optGaze.put("remote","/iKinGazeCtrl");
148 optGaze.put("local",("/"+getName()+"/gaze"));
149
150 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvJointHead.open(optJoint)&&drvGaze.open(optGaze),
151 "Unable to open clients for head!");
152 }
153
154 if (params.use_torso)
155 {
156 Property optJoint;
157 optJoint.put("device","remote_controlboard");
158 optJoint.put("remote",("/"+params.robot+"/"+"torso"));
159 optJoint.put("local",("/"+getName()+"/joint/torso"));
160
161 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(drvJointTorso.open(optJoint),
162 "Unable to open clients for torso!");
163 }
164
165 rpcPort.open("/"+getName()+"/rpc");
166 string dest="/demoRedBall/rpc";
167 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(Network::connect(rpcPort.getName(),dest),
168 Asserter::format("Unable to connect to %s!",dest.c_str()));
169
170 guiPort.open("/"+getName()+"/gui:i");
171 string src="/demoRedBall/gui:o";
172 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(Network::connect(src,guiPort.getName()),
173 Asserter::format("Unable to connect to %s!",src.c_str()));
174
175 return true;
176}
177
178
179/***********************************************************************************/
180void DemoRedBallTest::tearDown()
181{
182 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Closing Clients");
183 rpcPort.close();
184 guiPort.close();
185 if (params.use_left)
186 {
187 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJointArmL.close()&&drvCartArmL.close(),
188 "Unable to close client for left_arm!");
189 }
190 if (params.use_right)
191 {
192 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJointArmR.close()&&drvCartArmR.close(),
193 "Unable to close client for right_arm!");
194 }
195 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJointHead.close()&&drvGaze.close(),
196 "Unable to close client for head!");
197 if (params.use_torso)
198 {
199 ROBOTTESTINGFRAMEWORK_ASSERT_FAIL_IF_FALSE(drvJointTorso.close(),"Unable to close client for left_arm!");
200 }
201}
202
203
204/***********************************************************************************/
205bool DemoRedBallTest::getBallPosition(const Bottle* b, Vector& pos)
206{
207 if (b->size()>=15)
208 {
209 if (b->get(0).isString() && (b->get(0).asString()=="object"))
210 {
211 pos.resize(3);
212 pos[0]=b->get(5).asFloat64()/1000.;
213 pos[1]=b->get(6).asFloat64()/1000.;
214 pos[2]=b->get(7).asFloat64()/1000.;
215 return true;
216 }
217 }
218 return false;
219}
220
221
222/***********************************************************************************/
223void DemoRedBallTest::testBallPosition(const Vector &dpos)
224{
225 Vector x,o,encs;
226 int nEncs; IEncoders* ienc;
227 bool done=false;
228 double t0;
229
230 Bottle cmd,rep;
231 cmd.addString("update_pose");
232 cmd.addFloat64(dpos[0]);
233 cmd.addFloat64(dpos[1]);
234 cmd.addFloat64(dpos[2]);
235 rpcPort.write(cmd,rep);
236
237 Time::delay(3.0);
238
239 cmd.clear();
240 cmd.addString("start");
241 cmd.addFloat64(0.);
242 cmd.addFloat64(-50.);
243 cmd.addFloat64(10.);
244 rpcPort.write(cmd,rep);
245
246 auto filt = make_unique<MedianFilter>(5, Vector{0., 0., 0.});
247
248 IGazeControl* igaze;
249 drvGaze.view(igaze);
250
251 t0=Time::now();
252 while (Time::now()-t0<10.0)
253 {
254 if (auto* gui=guiPort.read(false))
255 {
256 Vector pos;
257 if (getBallPosition(gui,pos))
258 {
259 filt->filt(pos);
260 }
261 }
262 igaze->getFixationPoint(x);
263 if (norm(filt->output()-x)<2.0*params.reach_tol)
264 {
265 done=true;
266 break;
267 }
268 Time::delay(0.01);
269 }
270 ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Ball gazed at with the eyes!");
271
272 filt->init(Vector{0., 0., 0.});
273 done=false;
274 t0=Time::now();
275 while (Time::now()-t0<10.0)
276 {
277 if (auto* gui=guiPort.read(false))
278 {
279 Vector pos;
280 if (getBallPosition(gui,pos))
281 {
282 filt->filt(pos);
283 }
284 }
285 arm_under_test.iarm->getPose(x,o);
286 if (norm(filt->output()-x)<params.reach_tol)
287 {
288 done=true;
289 break;
290 }
291 Time::delay(0.01);
292 }
293 ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Ball reached with the hand!");
294
295 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Going home");
296 cmd.clear();
297 cmd.addString("stop");
298 rpcPort.write(cmd,rep);
299
300 arm_under_test.ienc->getAxes(&nEncs);
301 encs.resize(nEncs,0.0);
302 done=false;
303 t0=Time::now();
304 while (Time::now()-t0<10.0)
305 {
306 arm_under_test.ienc->getEncoders(encs.data());
307 if (norm(params.home_arm-encs.subVector(0,params.home_arm.length()-1))<5.0)
308 {
309 done=true;
310 break;
311 }
312 Time::delay(1.0);
313 }
314 ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Arm has reached home!");
315
316 drvJointHead.view(ienc);
317 ienc->getAxes(&nEncs);
318 encs.resize(nEncs,0.0);
319 done=false;
320 t0=Time::now();
321 while (Time::now()-t0<10.0)
322 {
323 ienc->getEncoders(encs.data());
324 if (norm(encs.subVector(0,3))<5.0)
325 {
326 done=true;
327 break;
328 }
329 Time::delay(1.0);
330 }
331 ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Head has reached home!");
332
333 if (params.use_torso)
334 {
335 drvJointTorso.view(ienc);
336 ienc->getAxes(&nEncs);
337 encs.resize(nEncs,0.0);
338 done=false;
339 t0=Time::now();
340 while (Time::now()-t0<10.0)
341 {
342 ienc->getEncoders(encs.data());
343 if (norm(encs.subVector(0,3))<5.0)
344 {
345 done=true;
346 break;
347 }
348 Time::delay(1.0);
349 }
350 ROBOTTESTINGFRAMEWORK_TEST_CHECK(done,"Torso has reached home!");
351 }
352}
353
354
355/***********************************************************************************/
356void DemoRedBallTest::run()
357{
358 if (params.use_torso || params.use_left)
359 {
360 Vector dpos(3,0.0);
361 dpos[1]=-0.06;
362 dpos[2]=-0.3;
363 drvJointArmL.view(arm_under_test.ienc);
364 drvCartArmL.view(arm_under_test.iarm);
365 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Reaching with the left hand");
366 testBallPosition(dpos);
367 }
368
369 if (params.use_torso || params.use_right)
370 {
371 Vector dpos(3,0.0);
372 dpos[1]=+0.06;
373 dpos[2]=-0.3;
374 drvJointArmR.view(arm_under_test.ienc);
375 drvCartArmR.view(arm_under_test.iarm);
376 ROBOTTESTINGFRAMEWORK_TEST_REPORT("Reaching with the right hand");
377 testBallPosition(dpos);
378 }
379}
380
DemoRedBallTest
This test verifies the point-to-point cartesian movement.
Definition DemoRedBallTest.h:51
Generated by doxygen 1.9.8