iCub-main
main.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
115 #include <cstdio>
116 #include <string>
117 #include <map>
118 
119 #include <yarp/os/Network.h>
120 #include <yarp/os/RFModule.h>
121 #include <yarp/os/PeriodicThread.h>
122 #include <yarp/os/Bottle.h>
123 #include <yarp/os/BufferedPort.h>
124 #include <yarp/os/Time.h>
125 #include <yarp/sig/Vector.h>
126 #include <yarp/math/Math.h>
127 
128 #include <yarp/dev/Drivers.h>
129 #include <yarp/dev/CartesianControl.h>
130 #include <yarp/dev/PolyDriver.h>
131 
132 #include <iCub/iKin/iKinFwd.h>
133 
134 #define MAX_TORSO_PITCH 30.0 // [deg]
135 #define EXECTIME_THRESDIST 0.3 // [m]
136 #define PRINT_STATUS_PER 1.0 // [s]
137 
138 using namespace std;
139 using namespace yarp::os;
140 using namespace yarp::dev;
141 using namespace yarp::sig;
142 using namespace yarp::math;
143 using namespace iCub::iKin;
144 
145 
146 /************************************************************************/
147 class CtrlThread : public PeriodicThread {
148  protected:
149  ResourceFinder& rf;
150  PolyDriver driver;
151  ICartesianControl* iarm;
152  BufferedPort<Bottle> port_xd;
153 
156  string remote;
157  string local;
158 
161 
162  Vector xd;
163  Vector od;
164 
166  double t0;
167 
168  public:
169  /************************************************************************/
170  CtrlThread(unsigned int _period, ResourceFinder& _rf,
171  string _remote, string _local) :
172  PeriodicThread((double)_period / 1000.0), rf(_rf),
173  remote(_remote), local(_local) { }
174 
175  /************************************************************************/
176  bool threadInit() {
177  // get params from the RF
178  ctrlCompletePose = !rf.check("onlyXYZ");
179  reinstateContext = rf.check("reinstate_context");
180 
181  // open the client
182  Property option("(device cartesiancontrollerclient)");
183  option.put("remote", remote);
184  option.put("local", local);
185  if (!driver.open(option))
186  return false;
187 
188  // open the view
189  driver.view(iarm);
190 
191  // latch the controller context
192  iarm->storeContext(&startup_context_id);
193 
194  // set trajectory time
195  defaultExecTime = rf.check("T", Value(2.0)).asFloat64();
196 
197  Bottle info;
198  iarm->getInfo(info);
199  iKinLimbVersion hwver(info.find("arm_version").asString());
200  printf("Detected arm kinematics version %s\n", hwver.get_version().c_str());
201 
202  map<string, int> torsoJointsRemap{{ "pitch", 0 }, { "roll", 1 }, { "yaw", 2 }};
203  if (hwver >= iKinLimbVersion("3.0")) {
204  torsoJointsRemap["roll"] = 0;
205  torsoJointsRemap["pitch"] = 1;
206  }
207 
208  // set torso dofs
209  Vector newDof, curDof;
210  iarm->getDOF(curDof);
211  newDof = curDof;
212 
213  if (curDof.length() > 7) { // consider only robots w/ torso
214  if (rf.check("DOF10")) {
215  // torso joints completely enabled
216  newDof[torsoJointsRemap["pitch"]] = 1;
217  newDof[torsoJointsRemap["roll"]] = 1;
218  newDof[torsoJointsRemap["yaw"]] = 1;
219 
220  limitTorsoPitch(torsoJointsRemap["pitch"]);
221  } else if (rf.check("DOF9")) {
222  // torso yaw and pitch enabled
223  newDof[torsoJointsRemap["pitch"]] = 1;
224  newDof[torsoJointsRemap["roll"]] = 0;
225  newDof[torsoJointsRemap["yaw"]] = 1;
226 
227  limitTorsoPitch(torsoJointsRemap["pitch"]);
228  } else if (rf.check("DOF8")) {
229  // only torso yaw enabled
230  newDof[torsoJointsRemap["pitch"]] = 0;
231  newDof[torsoJointsRemap["roll"]] = 0;
232  newDof[torsoJointsRemap["yaw"]] = 1;
233  } else {
234  // torso joints completely disabled
235  newDof[torsoJointsRemap["pitch"]] = 0;
236  newDof[torsoJointsRemap["roll"]] = 0;
237  newDof[torsoJointsRemap["yaw"]] = 0;
238  }
239  }
240 
241  iarm->setDOF(newDof, curDof);
242 
243  // set tracking mode
244  iarm->setTrackingMode(false);
245 
246  // set cartesian tolerance
247  if (rf.check("tol"))
248  iarm->setInTargetTol(rf.find("tol").asFloat64());
249 
250  // latch the controller context for our task
251  iarm->storeContext(&task_context_id);
252 
253  // init variables
254  while (true) {
255  if (iarm->getPose(xd, od))
256  break;
257 
258  Time::delay(0.1);
259  }
260 
261  // open ports
262  port_xd.open(local + "/xd:i");
263 
264  return true;
265  }
266 
267  /************************************************************************/
268  void afterStart(bool s) {
269  if (s)
270  printf("Thread started successfully\n");
271  else
272  printf("Thread did not start\n");
273 
274  t0 = Time::now();
275  }
276 
277  /************************************************************************/
278  void run() {
279  if (Bottle* b = port_xd.read(false)) {
280  if (b->size() >= 3) {
281  for (int i = 0; i < 3; i++)
282  xd[i] = b->get(i).asFloat64();
283 
284  if (ctrlCompletePose && (b->size() >= 7)) {
285  for (int i = 0; i < 4; i++)
286  od[i] = b->get(3 + i).asFloat64();
287  }
288 
289  if (reinstateContext)
290  iarm->restoreContext(task_context_id);
291 
292  const double execTime = calcExecTime(xd);
293  if (ctrlCompletePose)
294  iarm->goToPose(xd, od, execTime);
295  else
296  iarm->goToPosition(xd, execTime);
297  }
298  }
299 
300  printStatus();
301  }
302 
303  /************************************************************************/
304  void threadRelease() {
305  iarm->stopControl();
306  iarm->restoreContext(startup_context_id);
307  driver.close();
308 
309  port_xd.interrupt();
310  port_xd.close();
311  }
312 
313  /************************************************************************/
314  void limitTorsoPitch(const int axis) {
315  double min, max;
316  iarm->getLimits(axis, &min, &max);
317  iarm->setLimits(axis, min, MAX_TORSO_PITCH);
318  }
319 
320  /************************************************************************/
321  double calcExecTime(const Vector& xd) {
322  Vector x, o;
323  iarm->getPose(x, o);
324 
325  if (norm(xd - x) < EXECTIME_THRESDIST)
326  return defaultExecTime;
327  else
328  return 1.5 * defaultExecTime;
329  }
330 
331  /************************************************************************/
332  void printStatus() {
333  double t = Time::now();
334 
335  if (t - t0 >= PRINT_STATUS_PER) {
336  Vector x, o, xdot, odot;
337  Vector xdhat, odhat, qdhat;
338 
339  iarm->getPose(x, o);
340  iarm->getTaskVelocities(xdot, odot);
341  iarm->getDesired(xdhat, odhat, qdhat);
342  double e_x = norm(xdhat - x);
343 
344  printf("xd [m] = %s\n", xd.toString().c_str());
345  printf("xdhat [m] = %s\n", xdhat.toString().c_str());
346  printf("x [m] = %s\n", x.toString().c_str());
347  printf("xdot [m/s] = %s\n", xdot.toString().c_str());
348  printf("norm(e_x) [m] = %g\n", e_x);
349 
350  if (ctrlCompletePose) {
351  Vector e_o = dcm2axis(axis2dcm(odhat) * axis2dcm(o).transposed());
352  e_o *= e_o[3];
353  e_o.pop_back();
354 
355  printf("od [rad] = %s\n", od.toString().c_str());
356  printf("odhat [rad] = %s\n", odhat.toString().c_str());
357  printf("o [rad] = %s\n", o.toString().c_str());
358  printf("odot [rad/s] = %s\n", odot.toString().c_str());
359  printf("norm(e_o) [rad] = %g\n", norm(e_o));
360  }
361 
362  printf("\n");
363 
364  t0 = t;
365  }
366  }
367 };
368 
369 
370 /************************************************************************/
371 class CtrlModule : public RFModule {
372  protected:
374  Port rpcPort;
375 
376  public:
377  /************************************************************************/
378  bool configure(ResourceFinder& rf) {
379  string slash = "/";
380  string name;
381  string robot;
382  string part;
383  string remote;
384  string local;
385 
386  // get params from the RF
387  name = rf.check("name", Value("armCtrl")).asString();
388  robot = rf.check("robot", Value("icub")).asString();
389  part = rf.check("part", Value("right_arm")).asString();
390 
391  remote = slash + robot + "/cartesianController/" + part;
392  local = slash + name + slash + part;
393 
394  thr = new CtrlThread(20, rf, remote, local);
395  if (!thr->start()) {
396  delete thr;
397  return false;
398  }
399 
400  rpcPort.open(local + "/rpc");
401  attach(rpcPort);
402 
403  return true;
404  }
405 
406  /************************************************************************/
407  bool close() {
408  thr->stop();
409  delete thr;
410 
411  rpcPort.interrupt();
412  rpcPort.close();
413 
414  return true;
415  }
416 
417  /************************************************************************/
418  double getPeriod() {
419  return 1.0;
420  }
421 
422  /************************************************************************/
423  bool updateModule() {
424  return true;
425  }
426 };
427 
428 
429 /************************************************************************/
430 int main(int argc, char* argv[]) {
431  Network yarp;
432 
433  ResourceFinder rf;
434  rf.configure(argc, argv);
435 
436  if (rf.check("help")) {
437  printf("Options:\n");
438  printf("\t--name name : controller name (default armCtrl)\n");
439  printf("\t--robot name : robot name to connect to (default: icub)\n");
440  printf("\t--part type : robot arm type, left_arm or right_arm (default: right_arm)\n");
441  printf("\t--T time : specify the task execution time in seconds (default: 2.0)\n");
442  printf("\t--tol tolerance: specify the cartesian tolerance in meters\n");
443  printf("\t--DOF10 : control the torso yaw/roll/pitch as well\n");
444  printf("\t--DOF9 : control the torso yaw/pitch as well\n");
445  printf("\t--DOF8 : control the torso yaw as well\n");
446  printf("\t--onlyXYZ : disable orientation control\n");
447  printf("\t--reinstate_context : reinstate controller context upon target reception\n");
448  printf("\n");
449 
450  return 0;
451  }
452 
453  if (!yarp.checkNetwork()) {
454  printf("YARP server not available!\n");
455  return -1;
456  }
457 
458  CtrlModule mod;
459  return mod.runModule(rf);
460 }
461 
462 
463 
bool updateModule()
Definition: main.cpp:423
bool close()
Definition: main.cpp:407
CtrlThread * thr
Definition: main.cpp:373
double getPeriod()
Definition: main.cpp:418
Port rpcPort
Definition: main.cpp:374
bool configure(ResourceFinder &rf)
Definition: main.cpp:378
CtrlThread(unsigned int _period, ResourceFinder &_rf, string _remote, string _local)
Definition: main.cpp:170
double t0
Definition: main.cpp:166
Vector od
Definition: main.cpp:163
string remote
Definition: main.cpp:156
double defaultExecTime
Definition: main.cpp:165
Vector xd
Definition: main.cpp:162
int task_context_id
Definition: main.cpp:160
ResourceFinder & rf
Definition: main.cpp:149
void afterStart(bool s)
Definition: main.cpp:268
void run()
Definition: main.cpp:278
string local
Definition: main.cpp:157
int startup_context_id
Definition: main.cpp:159
BufferedPort< Bottle > port_xd
Definition: main.cpp:152
bool threadInit()
Definition: main.cpp:176
ICartesianControl * iarm
Definition: main.cpp:151
bool reinstateContext
Definition: main.cpp:155
bool ctrlCompletePose
Definition: main.cpp:154
void limitTorsoPitch(const int axis)
Definition: main.cpp:314
void threadRelease()
Definition: main.cpp:304
PolyDriver driver
Definition: main.cpp:150
double calcExecTime(const Vector &xd)
Definition: main.cpp:321
void printStatus()
Definition: main.cpp:332
A class for defining the versions of the iCub limbs.
Definition: iKinFwd.h:1046
std::string get_version() const
Return the version string.
Definition: iKinFwd.cpp:1600
double norm(const yarp::sig::Matrix &M, int col)
Returns the norm of the vector given in the form: matrix(:,col).
int main(int argc, char *argv[])
Definition: main.cpp:31
#define PRINT_STATUS_PER
Definition: main.cpp:136
#define EXECTIME_THRESDIST
Definition: main.cpp:135
#define MAX_TORSO_PITCH
Definition: main.cpp:134
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
Copyright (C) 2008 RobotCub Consortium.