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