icub-test
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 
37 using namespace std;
38 using namespace robottestingframework;
39 using namespace yarp::os;
40 using namespace yarp::dev;
41 using namespace yarp::sig;
42 using namespace yarp::math;
43 using namespace iCub::ctrl;
44 
45 // prepare the plugin
46 ROBOTTESTINGFRAMEWORK_PREPARE_PLUGIN(DemoRedBallTest)
47 
48 
49 /***********************************************************************************/
50 DemoRedBallTest::DemoRedBallTest() : yarp::robottestingframework::TestCase("DemoRedBallTest")
51 {
52 }
53 
54 
55 /***********************************************************************************/
56 DemoRedBallTest::~DemoRedBallTest()
57 {
58 }
59 
60 
61 /***********************************************************************************/
62 bool 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 /***********************************************************************************/
180 void 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 /***********************************************************************************/
205 bool 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 /***********************************************************************************/
223 void 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 /***********************************************************************************/
356 void 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.1