iCub-main
main.cpp
Go to the documentation of this file.
1 
24 #include <mutex>
25 #include <string>
26 #include <cstdio>
27 
28 #include <yarp/os/Network.h>
29 #include <yarp/os/PeriodicThread.h>
30 #include <yarp/os/RFModule.h>
31 #include <yarp/os/BufferedPort.h>
32 #include <yarp/os/Time.h>
33 #include <yarp/sig/Vector.h>
34 #include <yarp/math/Math.h>
35 
36 #include <iCub/iKin/iKinFwd.h>
37 #include <iCub/iKin/iKinInv.h>
38 #include <iCub/iKin/iKinIpOpt.h>
39 
40 using namespace std;
41 using namespace yarp::os;
42 using namespace yarp::sig;
43 using namespace yarp::math;
44 using namespace iCub::ctrl;
45 using namespace iCub::iKin;
46 
47 
48 // This inherited class handles the incoming
49 // target limb pose (xyz + axis/angle) and
50 // the joints feedback
51 /*****************************************************************/
52 class inPort : public BufferedPort<Bottle>
53 {
54 protected:
55  mutex mtx;
56  Vector vect;
57 
58  /*****************************************************************/
59  virtual void onRead(Bottle &b)
60  {
61  lock_guard<mutex> lg(mtx);
62  if (vect.length()!=b.size())
63  vect.resize(b.size());
64 
65  for (size_t i=0; i<vect.length(); i++)
66  vect[i]=b.get(i).asFloat64();
67  }
68 
69 public:
70  /*****************************************************************/
71  Vector get_vect()
72  {
73  lock_guard<mutex> lg(mtx);
74  return vect;
75  }
76 
77  /*****************************************************************/
78  void set_vect(const Vector &_vect)
79  {
80  lock_guard<mutex> lg(mtx);
81  vect=_vect;
82  }
83 };
84 
85 
86 // This class handles the data exchange
87 // between Solver and Controller
88 /*****************************************************************/
90 {
91 protected:
92  mutex mtx;
93 
94  Vector xd;
95  Vector qd;
96 
97 public:
98  /*****************************************************************/
99  void setDesired(const Vector &_xd, const Vector &_qd)
100  {
101  lock_guard<mutex> lg(mtx);
102  xd=_xd;
103  qd=_qd;
104  }
105 
106  /*****************************************************************/
107  void getDesired(Vector &_xd, Vector &_qd)
108  {
109  lock_guard<mutex> lg(mtx);
110  _xd=xd;
111  _qd=qd;
112  }
113 };
114 
115 
116 // The thread launched by the application which is
117 // in charge of inverting the limb kinematic relying
118 // on IpOpt computation.
119 /*****************************************************************/
120 class Solver : public PeriodicThread
121 {
122 protected:
123  ResourceFinder &rf;
128 
131  Port port_qd;
132 
133  Vector xd_old;
134 
135 public:
136  /*****************************************************************/
137  Solver(ResourceFinder &_rf, inPort *_port_q, exchangeData *_commData, unsigned int period) :
138  PeriodicThread((double)period/1000.0), rf(_rf), port_q(_port_q), commData(_commData)
139  {
140  limb=NULL;
141  chain=NULL;
142  slv=NULL;
143  }
144 
145  /*****************************************************************/
146  virtual bool threadInit()
147  {
148  fprintf(stdout,"Starting Solver at %g ms\n",1000.0*getPeriod());
149 
150  string name=rf.find("name").asString();
151  unsigned int ctrlPose=rf.check("onlyXYZ")?IKINCTRL_POSE_XYZ:IKINCTRL_POSE_FULL;
152 
153  Property linksOptions;
154  linksOptions.fromConfigFile(rf.findFile("config"));
155 
156  // instantiate the limb
157  limb=new iKinLimb(linksOptions);
158 
159  if (!limb->isValid())
160  {
161  fprintf(stdout,"Error: invalid links parameters!\n");
162  delete limb;
163 
164  return false;
165  }
166 
167  // get the chain object attached to the limb
168  chain=limb->asChain();
169 
170  // pose initialization with the current joints position.
171  // Remind that the representation used is the axis/angle,
172  // the default one.
173  xd_old=chain->EndEffPose();
174  commData->setDesired(xd_old,chain->getAng());
175 
176  // instantiate the optimizer with the passed chain, the ctrlPose control
177  // mode, the cost function and constraints tolerances and
178  // a maximum number of iteration
179  slv=new iKinIpOptMin(*chain,ctrlPose,1e-3,1e-6,200);
180 
181  // in order to speed up the process, a scaling for the problem
182  // is usually required (a good scaling holds each element of the jacobian
183  // of constraints and the hessian of lagrangian in norm between 0.1 and 10.0)
184  slv->setUserScaling(true,100.0,100.0,100.0);
185 
186  port_xd.open("/"+name+"/xd:i");
187  port_xd.useCallback();
188  port_xd.set_vect(xd_old);
189 
190  port_qd.open("/"+name+"/qd:o");
191 
192  return true;
193  }
194 
195  /*****************************************************************/
196  virtual void afterStart(bool s)
197  {
198  if (s)
199  fprintf(stdout,"Solver started successfully\n");
200  else
201  fprintf(stdout,"Solver did not start\n");
202  }
203 
204  /*****************************************************************/
205  virtual void run()
206  {
207  // get the target pose
208  Vector xd=port_xd.get_vect();
209 
210  // if new target is received
211  if (!(xd==xd_old))
212  {
213  // get the feedback and update the chain
214  chain->setAng(CTRL_DEG2RAD*port_q->get_vect());
215 
216  // minimize also against the current joints position
217  Vector q0=chain->getAng();
218  Vector w_3rd(chain->getDOF(),1.0);
219 
220  // call the solver and start the convergence from the current point
221  Vector dummyVect(1);
222  Vector qdhat=slv->solve(q0,xd,0.0,dummyVect,dummyVect,0.01,q0,w_3rd);
223 
224  // qdhat is an estimation of the real qd, so that xdhat is the actual achieved pose
225  Vector xdhat=chain->EndEffPose(qdhat);
226 
227  // update the exchange structure straightaway
228  commData->setDesired(xdhat,qdhat);
229 
230  // send qdhat over yarp
231  Vector qdhat_deg=CTRL_RAD2DEG*qdhat;
232  port_qd.write(qdhat_deg);
233 
234  // latch the current target
235  xd_old=xd;
236  }
237  }
238 
239  /*****************************************************************/
240  virtual void threadRelease()
241  {
242  port_xd.interrupt();
243  port_qd.interrupt();
244  port_xd.close();
245  port_qd.close();
246 
247  delete slv;
248  delete limb;
249  }
250 };
251 
252 
253 // The thread launched by the application which is
254 // in charge of computing the velocities profile
255 /*****************************************************************/
256 class Controller : public PeriodicThread
257 {
258 protected:
259  ResourceFinder &rf;
264 
266  Port port_v;
267  Port port_x;
268 
269 public:
270  /*****************************************************************/
271  Controller(ResourceFinder &_rf, inPort *_port_q, exchangeData *_commData, unsigned int period) :
272  PeriodicThread((double)period/1000.0), rf(_rf), port_q(_port_q), commData(_commData)
273  {
274  limb=NULL;
275  chain=NULL;
276  ctrl=NULL;
277  }
278 
279  /*****************************************************************/
280  virtual bool threadInit()
281  {
282  fprintf(stdout,"Starting Controller at %g ms\n",1000.0*getPeriod());
283 
284  string name=rf.find("name").asString();
285  unsigned int ctrlPose=rf.check("onlyXYZ")?IKINCTRL_POSE_XYZ:IKINCTRL_POSE_FULL;
286 
287  Property linksOptions;
288  linksOptions.fromConfigFile(rf.findFile("config"));
289 
290  // instantiate the limb
291  limb=new iKinLimb(linksOptions);
292 
293  if (!limb->isValid())
294  {
295  fprintf(stdout,"Error: invalid links parameters!\n");
296  delete limb;
297 
298  return false;
299  }
300 
301  // get the chain object attached to the limb
302  chain=limb->asChain();
303 
304  // instantiate controller
305  ctrl=new MultiRefMinJerkCtrl(*chain,ctrlPose,getPeriod());
306 
307  // set the task execution time
308  ctrl->set_execTime(rf.check("T",Value(2.0)).asFloat64(),true);
309 
310  port_v.open("/"+name+"/v:o");
311  port_x.open("/"+name+"/x:o");
312 
313  return true;
314  }
315 
316  /*****************************************************************/
317  virtual void afterStart(bool s)
318  {
319  if (s)
320  fprintf(stdout,"Controller started successfully\n");
321  else
322  fprintf(stdout,"Controller did not start\n");
323  }
324 
325  /*****************************************************************/
326  virtual void run()
327  {
328  Vector xd,qd;
329 
330  // get the current target pose (both xd and qd are required)
331  commData->getDesired(xd,qd);
332 
333  // get the feedback
334  ctrl->set_q(CTRL_DEG2RAD*port_q->get_vect());
335 
336  // control the limb and dump all available information at rate of 1/100th
337  ctrl->iterate(xd,qd,0x0064ffff);
338 
339  // send v and x through YARP ports
340  Vector qdot_deg=CTRL_RAD2DEG*ctrl->get_qdot();
341  Vector x=ctrl->get_x();
342  port_v.write(qdot_deg);
343  port_x.write(x);
344  }
345 
346  /*****************************************************************/
347  virtual void threadRelease()
348  {
349  // make sure that the limb is stopped before closing
350  Vector zeros(chain->getDOF(),0.0);
351  port_v.write(zeros);
352 
353  port_v.interrupt();
354  port_x.interrupt();
355  port_v.close();
356  port_x.close();
357 
358  delete ctrl;
359  delete limb;
360  }
361 };
362 
363 
364 /*****************************************************************/
365 class CtrlModule: public RFModule
366 {
367 protected:
372 
373 public:
374  /*****************************************************************/
375  virtual bool configure(ResourceFinder &rf)
376  {
377  string name=rf.find("name").asString();
378 
379  // Note that Solver and Controller operate on
380  // different limb objects (instantiated internally
381  // and separately) in order to avoid any interaction.
382  slv=new Solver(rf,&port_q,&commData,30);
383  ctrl=new Controller(rf,&port_q,&commData,10);
384 
385  if (!slv->start())
386  {
387  delete slv;
388  return false;
389  }
390 
391  if (!ctrl->start())
392  {
393  delete slv;
394  delete ctrl;
395  return false;
396  }
397 
398  // open the feedback port
399  port_q.open("/"+name+"/q:i");
400  port_q.useCallback();
401 
402  return true;
403  }
404 
405  /*****************************************************************/
406  virtual bool close()
407  {
408  ctrl->stop();
409  slv->stop();
410 
411  delete ctrl;
412  delete slv;
413 
414  port_q.interrupt();
415  port_q.close();
416 
417  return true;
418  }
419 
420  /*****************************************************************/
421  virtual double getPeriod()
422  {
423  return 1.0;
424  }
425 
426  /*****************************************************************/
427  virtual bool updateModule()
428  {
429  return true;
430  }
431 };
432 
433 
434 /*****************************************************************/
435 int main(int argc, char *argv[])
436 {
437  ResourceFinder rf;
438  rf.setDefault("name","ctrl");
439  rf.setDefault("config","config.ini");
440  rf.configure(argc,argv);
441 
442  if (rf.check("help"))
443  {
444  fprintf(stdout,"Options:\n\n");
445  fprintf(stdout,"\t--name name: controller name (default: \"ctrl\")\n");
446  fprintf(stdout,"\t--config file: specify the file containing the DH parameters of the links (default: \"config.ini\")\n");
447  fprintf(stdout,"\t--T time: specify the task execution time in seconds (default: 2.0)\n");
448  fprintf(stdout,"\t--onlyXYZ : disable orientation control\n");
449 
450  return 0;
451  }
452 
453  Network yarp;
454  if (!yarp.checkNetwork())
455  return 1;
456 
457  CtrlModule mod;
458  return mod.runModule(rf);
459 }
460 
461 
462 
iKinFwd.h
IKINCTRL_POSE_XYZ
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
Controller::Controller
Controller(ResourceFinder &_rf, inPort *_port_q, exchangeData *_commData, unsigned int period)
Definition: main.cpp:271
Controller::threadRelease
virtual void threadRelease()
Definition: main.cpp:347
iCub::iKin::iKinIpOptMin::setUserScaling
void setUserScaling(const bool useUserScaling, const double _obj_scaling, const double _x_scaling, const double _g_scaling)
Enables/disables user scaling factors.
Definition: iKinIpOpt.cpp:973
Controller::afterStart
virtual void afterStart(bool s)
Definition: main.cpp:317
iKinInv.h
iCub::iKin::MultiRefMinJerkCtrl::set_execTime
double set_execTime(const double _execTime, const bool warn=false)
Sets the task execution time in seconds.
Definition: iKinInv.cpp:942
CtrlModule::configure
virtual bool configure(ResourceFinder &rf)
Definition: main.cpp:375
Solver::port_q
inPort * port_q
Definition: main.cpp:129
iCub::iKin::MultiRefMinJerkCtrl::iterate
virtual yarp::sig::Vector iterate(yarp::sig::Vector &xd, yarp::sig::Vector &qd, yarp::sig::Vector *xdot_set, const unsigned int verbose)
iCub::iKin::iKinCtrl::get_x
virtual yarp::sig::Vector get_x() const
Returns the actual cartesian position of the End-Effector.
Definition: iKinInv.h:357
Controller::run
virtual void run()
Definition: main.cpp:326
CtrlModule
Definition: main.cpp:368
e
e
Definition: compute_ekf_fast.m:13
CtrlModule::getPeriod
virtual double getPeriod()
Definition: main.cpp:421
CtrlModule::updateModule
virtual bool updateModule()
Definition: main.cpp:427
inPort::onRead
virtual void onRead(Bottle &b)
Definition: main.cpp:59
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iKin::iKinIpOptMin
Definition: iKinIpOpt.h:198
main
int main(int argc, char *argv[])
Definition: main.cpp:31
CtrlModule::port_q
inPort port_q
Definition: main.cpp:370
CTRL_DEG2RAD
#define CTRL_DEG2RAD
Definition: XSensMTx.cpp:26
Solver::afterStart
virtual void afterStart(bool s)
Definition: main.cpp:196
exchangeData::qd
Vector qd
Definition: main.cpp:95
Controller::ctrl
MultiRefMinJerkCtrl * ctrl
Definition: main.cpp:262
iCub::iKin::iKinChain::getDOF
unsigned int getDOF() const
Returns the current number of Chain's DOF.
Definition: iKinFwd.h:557
iCub::iKin::iKinChain::setAng
yarp::sig::Vector setAng(const yarp::sig::Vector &q)
Sets the free joint angles to values of q[i].
Controller
Definition: controller.h:53
Controller::chain
iKinChain * chain
Definition: main.cpp:261
IKINCTRL_POSE_FULL
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
CtrlModule::close
virtual bool close()
Definition: main.cpp:406
Controller::commData
exchangeData * commData
Definition: main.cpp:263
iCub::iKin::iKinLimb
Definition: iKinFwd.h:873
Solver::run
virtual void run()
Definition: main.cpp:205
iCub::iKin::iKinChain::getAng
yarp::sig::Vector getAng()
Returns the current free joint angles values.
Definition: iKinFwd.cpp:619
CTRL_RAD2DEG
#define CTRL_RAD2DEG
Definition: XSensMTx.cpp:25
Solver::threadRelease
virtual void threadRelease()
Definition: main.cpp:240
exchangeData::mtx
mutex mtx
Definition: main.cpp:92
iCub::iKin
Definition: iKinFwd.h:71
iCub::iKin::MultiRefMinJerkCtrl
Definition: iKinInv.h:759
Controller::limb
iKinLimb * limb
Definition: main.cpp:260
iKinIpOpt.h
CtrlModule::commData
exchangeData commData
Definition: main.cpp:371
Solver::port_qd
Port port_qd
Definition: main.cpp:131
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
Solver
Definition: solver.h:119
inPort::vect
Vector vect
Definition: main.cpp:56
inPort::get_vect
Vector get_vect()
Definition: main.cpp:71
x
x
Definition: compute_ekf_sym.m:21
Controller::port_q
inPort * port_q
Definition: main.cpp:265
inPort
Definition: main.cpp:52
inPort::set_vect
void set_vect(const Vector &_vect)
Definition: main.cpp:78
Controller::port_x
Port port_x
Definition: main.cpp:267
exchangeData::getDesired
void getDesired(Vector &_xd, Vector &_qd)
Definition: main.cpp:107
Solver::limb
iKinLimb * limb
Definition: main.cpp:124
CtrlModule::ctrl
Controller * ctrl
Definition: main.cpp:369
Solver::commData
exchangeData * commData
Definition: main.cpp:127
Solver::port_xd
inPort port_xd
Definition: main.cpp:130
exchangeData::xd
Vector xd
Definition: main.cpp:94
exchangeData::setDesired
void setDesired(const Vector &_xd, const Vector &_qd)
Definition: main.cpp:99
Solver::chain
iKinChain * chain
Definition: main.cpp:125
Solver::rf
ResourceFinder & rf
Definition: main.cpp:123
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
Solver::threadInit
virtual bool threadInit()
Definition: main.cpp:146
iCub::iKin::iKinChain::EndEffPose
yarp::sig::Vector EndEffPose(const bool axisRep=true)
Returns the coordinates of end-effector.
Definition: iKinFwd.cpp:858
scripting.argc
argc
Definition: scripting.py:184
iCub::iKin::MultiRefMinJerkCtrl::get_qdot
yarp::sig::Vector get_qdot() const
Returns the actual derivative of joint angles.
Definition: iKinInv.h:913
Controller::port_v
Port port_v
Definition: main.cpp:266
Solver::Solver
Solver(ResourceFinder &_rf, inPort *_port_q, exchangeData *_commData, unsigned int period)
Definition: main.cpp:137
fprintf
fprintf(fid,'\n')
iCub::iKin::iKinLimb::isValid
bool isValid() const
Checks if the limb has been properly configured.
Definition: iKinFwd.h:998
Solver::slv
iKinIpOptMin * slv
Definition: main.cpp:126
Controller::threadInit
virtual bool threadInit()
Definition: main.cpp:280
iCub::iKin::iKinIpOptMin::solve
virtual yarp::sig::Vector solve(const yarp::sig::Vector &q0, yarp::sig::Vector &xd, double weight2ndTask, yarp::sig::Vector &xd_2nd, yarp::sig::Vector &w_2nd, double weight3rdTask, yarp::sig::Vector &qd_3rd, yarp::sig::Vector &w_3rd, int *exit_code=NULL, bool *exhalt=NULL, iKinIterateCallback *iterate=NULL)
Executes the IpOpt algorithm trying to converge on target.
Definition: iKinIpOpt.cpp:1030
zeros
zeros(2, 2) eye(2
iCub::iKin::iKinLimb::asChain
iKinChain * asChain()
Returns a pointer to the Limb seen as Chain object.
Definition: iKinFwd.h:1013
Controller::rf
ResourceFinder & rf
Definition: main.cpp:259
inPort::mtx
mutex mtx
Definition: main.cpp:55
iCub::iKin::MultiRefMinJerkCtrl::set_q
virtual void set_q(const yarp::sig::Vector &q0)
Sets the joint angles values.
Definition: iKinInv.cpp:934
Solver::xd_old
Vector xd_old
Definition: main.cpp:133
CtrlModule::slv
Solver * slv
Definition: main.cpp:368
exchangeData
Definition: main.cpp:89