iCub-main
tutorial_cartesian_interface.cpp
Go to the documentation of this file.
1 // -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2 //
3 // A tutorial on how to use the Cartesian Interface to control a limb
4 // in the operational space.
5 //
6 // Author: Ugo Pattacini - <ugo.pattacini@iit.it>
7 
8 #include <cstdio>
9 #include <cmath>
10 
11 #include <yarp/os/Network.h>
12 #include <yarp/os/RFModule.h>
13 #include <yarp/os/PeriodicThread.h>
14 #include <yarp/os/Time.h>
15 #include <yarp/sig/Vector.h>
16 #include <yarp/math/Math.h>
17 
18 #include <yarp/dev/Drivers.h>
19 #include <yarp/dev/CartesianControl.h>
20 #include <yarp/dev/PolyDriver.h>
21 
22 #define CTRL_THREAD_PER 0.02 // [s]
23 #define PRINT_STATUS_PER 1.0 // [s]
24 #define MAX_TORSO_PITCH 30.0 // [deg]
25 
26 using namespace std;
27 using namespace yarp::os;
28 using namespace yarp::dev;
29 using namespace yarp::sig;
30 using namespace yarp::math;
31 
32 
33 class CtrlThread: public PeriodicThread,
34  public CartesianEvent
35 {
36 protected:
37  PolyDriver client;
38  ICartesianControl *icart;
39 
40  Vector xd;
41  Vector od;
42 
43  int startup_context_id;
44 
45  double t;
46  double t0;
47  double t1;
48 
49  // the event callback attached to the "motion-ongoing"
50  virtual void cartesianEventCallback()
51  {
52  fprintf(stdout,"20%% of trajectory attained\n");
53  }
54 
55 public:
56  CtrlThread(const double period) : PeriodicThread(period)
57  {
58  // we wanna raise an event each time the arm is at 20%
59  // of the trajectory (or 80% far from the target)
60  cartesianEventParameters.type="motion-ongoing";
61  cartesianEventParameters.motionOngoingCheckPoint=0.2;
62  }
63 
64  virtual bool threadInit()
65  {
66  // open a client interface to connect to the cartesian server of the simulator
67  // we suppose that:
68  //
69  // 1 - the iCub simulator is running
70  // (launch: iCub_SIM)
71  //
72  // 2 - the cartesian server is running
73  // (launch: yarprobotinterface --context simCartesianControl)
74  //
75  // 3 - the cartesian solver for the left arm is running too
76  // (launch: iKinCartesianSolver --context simCartesianControl --part left_arm)
77  //
78  Property option("(device cartesiancontrollerclient)");
79  option.put("remote","/icubSim/cartesianController/left_arm");
80  option.put("local","/cartesian_client/left_arm");
81 
82  if (!client.open(option))
83  return false;
84 
85  // open the view
86  client.view(icart);
87 
88  // latch the controller context in order to preserve
89  // it after closing the module
90  // the context contains the dofs status, the tracking mode,
91  // the resting positions, the limits and so on.
92  icart->storeContext(&startup_context_id);
93 
94  // set trajectory time
95  icart->setTrajTime(1.0);
96 
97  // get the torso dofs
98  Vector newDof, curDof;
99  icart->getDOF(curDof);
100  newDof=curDof;
101 
102  // enable the torso yaw and pitch
103  // disable the torso roll
104  newDof[0]=1;
105  newDof[1]=0;
106  newDof[2]=1;
107 
108  // send the request for dofs reconfiguration
109  icart->setDOF(newDof,curDof);
110 
111  // impose some restriction on the torso pitch
112  limitTorsoPitch();
113 
114  // print out some info about the controller
115  Bottle info;
116  icart->getInfo(info);
117  fprintf(stdout,"info = %s\n",info.toString().c_str());
118 
119  // register the event, attaching the callback
120  icart->registerEvent(*this);
121 
122  xd.resize(3);
123  od.resize(4);
124 
125  return true;
126  }
127 
128  virtual void afterStart(bool s)
129  {
130  if (s)
131  fprintf(stdout,"Thread started successfully\n");
132  else
133  fprintf(stdout,"Thread did not start\n");
134 
135  t=t0=t1=Time::now();
136  }
137 
138  virtual void run()
139  {
140  t=Time::now();
141 
142  generateTarget();
143 
144  // go to the target :)
145  // (in streaming)
146  icart->goToPose(xd,od);
147 
148  // some verbosity
149  printStatus();
150  }
151 
152  virtual void threadRelease()
153  {
154  // we require an immediate stop
155  // before closing the client for safety reason
156  icart->stopControl();
157 
158  // it's a good rule to restore the controller
159  // context as it was before opening the module
160  icart->restoreContext(startup_context_id);
161 
162  client.close();
163  }
164 
166  {
167  // translational target part: a circular trajectory
168  // in the yz plane centered in [-0.3,-0.1,0.1] with radius=0.1 m
169  // and frequency 0.1 Hz
170  xd[0]=-0.3;
171  xd[1]=-0.1+0.1*cos(2.0*M_PI*0.1*(t-t0));
172  xd[2]=+0.1+0.1*sin(2.0*M_PI*0.1*(t-t0));
173 
174  // we keep the orientation of the left arm constant:
175  // we want the middle finger to point forward (end-effector x-axis)
176  // with the palm turned down (end-effector y-axis points leftward);
177  // to achieve that it is enough to rotate the root frame of pi around z-axis
178  od[0]=0.0; od[1]=0.0; od[2]=1.0; od[3]=M_PI;
179  }
180 
182  {
183  int axis=0; // pitch joint
184  double min, max;
185 
186  // sometimes it may be helpful to reduce
187  // the range of variability of the joints;
188  // for example here we don't want the torso
189  // to lean out more than 30 degrees forward
190 
191  // we keep the lower limit
192  icart->getLimits(axis,&min,&max);
193  icart->setLimits(axis,min,MAX_TORSO_PITCH);
194  }
195 
196  void printStatus()
197  {
198  if (t-t1>=PRINT_STATUS_PER)
199  {
200  Vector x,o,xdhat,odhat,qdhat;
201 
202  // we get the current arm pose in the
203  // operational space
204  icart->getPose(x,o);
205 
206  // we get the final destination of the arm
207  // as found by the solver: it differs a bit
208  // from the desired pose according to the tolerances
209  icart->getDesired(xdhat,odhat,qdhat);
210 
211  double e_x=norm(xdhat-x);
212  double e_o=norm(odhat-o);
213 
214  fprintf(stdout,"+++++++++\n");
215  fprintf(stdout,"xd [m] = %s\n",xd.toString().c_str());
216  fprintf(stdout,"xdhat [m] = %s\n",xdhat.toString().c_str());
217  fprintf(stdout,"x [m] = %s\n",x.toString().c_str());
218  fprintf(stdout,"od [rad] = %s\n",od.toString().c_str());
219  fprintf(stdout,"odhat [rad] = %s\n",odhat.toString().c_str());
220  fprintf(stdout,"o [rad] = %s\n",o.toString().c_str());
221  fprintf(stdout,"norm(e_x) [m] = %g\n",e_x);
222  fprintf(stdout,"norm(e_o) [rad] = %g\n",e_o);
223  fprintf(stdout,"---------\n\n");
224 
225  t1=t;
226  }
227  }
228 };
229 
230 
231 
232 class CtrlModule: public RFModule
233 {
234 protected:
235  CtrlThread *thr;
236 
237 public:
238  virtual bool configure(ResourceFinder &rf)
239  {
240  thr=new CtrlThread(CTRL_THREAD_PER);
241  if (!thr->start())
242  {
243  delete thr;
244  return false;
245  }
246 
247  return true;
248  }
249 
250  virtual bool close()
251  {
252  thr->stop();
253  delete thr;
254 
255  return true;
256  }
257 
258  virtual double getPeriod() { return 1.0; }
259  virtual bool updateModule() { return true; }
260 };
261 
262 
263 
264 int main()
265 {
266  Network yarp;
267  if (!yarp.checkNetwork())
268  {
269  fprintf(stdout,"Error: yarp server does not seem available\n");
270  return 1;
271  }
272 
273  CtrlModule mod;
274 
275  ResourceFinder rf;
276  return mod.runModule(rf);
277 }
CtrlThread::threadInit
virtual bool threadInit()
Definition: tutorial_cartesian_interface.cpp:64
CtrlThread::cartesianEventCallback
virtual void cartesianEventCallback()
Definition: tutorial_cartesian_interface.cpp:50
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
CtrlModule::configure
virtual bool configure(ResourceFinder &rf)
Definition: tutorial_cartesian_interface.cpp:238
CtrlModule
Definition: main.cpp:368
CtrlModule::getPeriod
virtual double getPeriod()
Definition: tutorial_cartesian_interface.cpp:258
CtrlModule::updateModule
virtual bool updateModule()
Definition: tutorial_cartesian_interface.cpp:259
CtrlThread::client
PolyDriver client
Definition: tutorial_cartesian_interface.cpp:37
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
CtrlThread::t1
double t1
Definition: tutorial_cartesian_interface.cpp:47
yarp::dev
Definition: DebugInterfaces.h:52
CtrlThread::limitTorsoPitch
void limitTorsoPitch()
Definition: tutorial_cartesian_interface.cpp:181
CtrlModule::close
virtual bool close()
Definition: tutorial_cartesian_interface.cpp:250
CtrlThread::icart
ICartesianControl * icart
Definition: tutorial_cartesian_interface.cpp:38
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
MAX_TORSO_PITCH
#define MAX_TORSO_PITCH
Definition: tutorial_cartesian_interface.cpp:24
CtrlThread::printStatus
void printStatus()
Definition: tutorial_cartesian_interface.cpp:196
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
x
x
Definition: compute_ekf_sym.m:21
main
int main()
Definition: tutorial_cartesian_interface.cpp:264
CtrlThread::generateTarget
void generateTarget()
Definition: tutorial_cartesian_interface.cpp:165
CtrlThread::t
double t
Definition: tutorial_cartesian_interface.cpp:45
CtrlThread::afterStart
virtual void afterStart(bool s)
Definition: tutorial_cartesian_interface.cpp:128
PRINT_STATUS_PER
#define PRINT_STATUS_PER
Definition: tutorial_cartesian_interface.cpp:23
CtrlThread::CtrlThread
CtrlThread(const double period)
Definition: tutorial_cartesian_interface.cpp:56
CtrlThread::threadRelease
virtual void threadRelease()
Definition: tutorial_cartesian_interface.cpp:152
yarp
Copyright (C) 2008 RobotCub Consortium.
Definition: DebugInterfaces.h:51
fprintf
fprintf(fid,'\n')
M_PI
#define M_PI
Definition: XSensMTx.cpp:24
CTRL_THREAD_PER
#define CTRL_THREAD_PER
Definition: tutorial_cartesian_interface.cpp:22
CtrlThread
Definition: main.cpp:144
CtrlThread::run
virtual void run()
Definition: tutorial_cartesian_interface.cpp:138