grasp
•All Data Structures Namespaces Functions Modules
orientationThread.cpp
1 /* Copyright: (C) 2014 iCub Facility - Istituto Italiano di Tecnologia
2  * Authors: Ilaria Gori
3  * email: ilaria.gori@iit.it
4  * Permission is granted to copy, distribute, and/or modify this program
5  * under the terms of the GNU General Public License, version 2 or any
6  * later version published by the Free Software Foundation.
7  *
8  * A copy of the license can be found in the file LICENSE located in the
9  * root directory.
10  *
11  * This program is distributed in the hope that it will be useful, but
12  * WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
14  * Public License for more details
15 */
16 
17 #include "orientationThread.h"
18 
19 using namespace std;
20 using namespace yarp::os;
21 using namespace yarp::dev;
22 using namespace yarp::sig;
23 using namespace yarp::math;
24 using namespace iCub::ctrl;
25 using namespace iCub::iKin;
26 
27 /************************************************************************/
28 OrientationThread::OrientationThread() : RateThread(20)
29 {
30  iCtrl=NULL;
31  arm=NULL;
32  done=true;
33  work=false;
34  noResult=true;
35 }
36 
37 /************************************************************************/
38 bool OrientationThread::open(string &name, string &hand, string &robot, int &nAngles)
39 {
40  this->nAngles=nAngles;
41  this->hand=hand;
42 
43  string remoteName="/"+robot+"/cartesianController/"+hand;
44  Property optCtrl;
45  optCtrl.put("device","cartesiancontrollerclient");
46  optCtrl.put("remote",remoteName.c_str());
47  optCtrl.put("local",("/"+name+"/orientationThread/"+hand+"/cartesian").c_str());
48 
49  if (!dCtrl.open(optCtrl))
50  {
51  fprintf(stdout, "%s Cartesian Interface is not open\n", hand.c_str());
52  return false;
53  }
54 
55  dCtrl.view(iCtrl);
56  Property optArm;
57  Property optTorso;
58 
59  string remoteArmName="/"+robot+"/"+hand;
60  optArm.put("device", "remote_controlboard");
61  optArm.put("remote",remoteArmName.c_str());
62  optArm.put("local",("/"+name+"/localArm/"+hand).c_str());
63 
64  string remoteTorsoName="/"+robot+"/torso";
65  optTorso.put("device", "remote_controlboard");
66  optTorso.put("remote",remoteTorsoName.c_str());
67  optTorso.put("local",("/"+name+"/grasplocalTorso/"+hand).c_str());
68 
69  robotTorso.open(optTorso);
70  robotArm.open(optArm);
71 
72  if (!robotTorso.isValid() || !robotArm.isValid())
73  {
74  fprintf(stdout, "Device not available\n");
75  close();
76  return false;
77  }
78 
79  robotArm.view(limArm);
80  robotTorso.view(limTorso);
81 
82  if (hand=="right_arm")
83  arm=new iCubArm("right");
84  else
85  arm=new iCubArm("left");
86 
87  chain=arm->asChain();
88 
89  chain->releaseLink(0);
90  chain->releaseLink(1);
91  chain->releaseLink(2);
92 
93  deque<IControlLimits*> lim;
94  lim.push_back(limTorso);
95  lim.push_back(limArm);
96  arm->alignJointsBounds(lim);
97 
98  arm->setAllConstraints(false);
99 
100  thetaMin.resize(10,0.0);
101  thetaMax.resize(10,0.0);
102  for (unsigned int i=0; i<chain->getDOF(); i++)
103  {
104  thetaMin[i]=(*chain)(i).getMin();
105  thetaMax[i]=(*chain)(i).getMax();
106  }
107 
108  getAngles(angles, nAngles);
109  q.resize(10);
110  ones.resize(q.size()); ones=1.0;
111  q0.resize(10);
112  xdhat.resize(3);
113  odhat.resize(4);
114 
115  return true;
116 }
117 
118 /************************************************************************/
119 void OrientationThread::preAskForPose()
120 {
121  iCtrl->storeContext(&currentContext);
122  yarp::sig::Vector dof;
123  iCtrl->getDOF(dof);
124  yarp::sig::Vector newDof;
125  newDof=dof;
126  newDof[0]=1.0;
127  newDof[2]=1.0;
128 
129  iCtrl->setDOF(newDof,dof);
130 }
131 
132 /************************************************************************/
133 void OrientationThread::postAskForPose()
134 {
135  iCtrl->restoreContext(currentContext);
136  iCtrl->deleteContext(currentContext);
137 }
138 
139 /************************************************************************/
140 void OrientationThread::reset()
141 {
142  bestManip=0.0;
143  noResult=true;
144 }
145 
146 /************************************************************************/
147 void OrientationThread::setInfo(yarp::sig::Vector &eePos, yarp::sig::Vector &px, yarp::sig::Vector &py, yarp::sig::Vector &pointNormal, yarp::sig::Vector &center, yarp::sig::Vector &biggestAxis)
148 {
149  this->eePos=eePos;
150  this->px=px;
151  this->py=py;
152  this->pointNormal=pointNormal;
153  this->center=center;
154  this->biggestAxis=biggestAxis;
155  done=false;
156  work=true;
157 }
158 
159 /************************************************************************/
160 void OrientationThread::getAngles(yarp::sig::Vector &angles, int nAngles)
161 {
162  angles.resize(nAngles);
163  double factor=360.0/nAngles;
164  double tmp=0.0;
165  for (int i=0; i<nAngles; i++)
166  {
167  angles[i]=tmp;
168  tmp+=factor;
169  }
170 }
171 
172 /************************************************************************/
173 void OrientationThread::run()
174 {
175  bestOrientation=eye(4,4);
176  double manip=0.0;
177  Matrix orientation=eye(4,4);
178 
179  if (work)
180  {
181  orientation(0,3)=center[0];
182  orientation(1,3)=center[1];
183  orientation(2,3)=center[2];
184  orientation(0,2)=pointNormal[0];
185  orientation(1,2)=pointNormal[1];
186  orientation(2,2)=pointNormal[2];
187 
188  for (int j=0; j<nAngles; j++)
189  {
190  orientation(0,0)=px[0]*cos(angles[j])-py[0]*sin(angles[j]);
191  orientation(1,0)=px[1]*cos(angles[j])-py[1]*sin(angles[j]);
192  orientation(2,0)=px[2]*cos(angles[j])-py[2]*sin(angles[j]);
193  orientation(0,1)=px[0]*sin(angles[j])+py[0]*cos(angles[j]);
194  orientation(1,1)=px[1]*sin(angles[j])+py[1]*cos(angles[j]);
195  orientation(2,1)=px[2]*sin(angles[j])+py[2]*cos(angles[j]);
196 
197  yarp::sig::Vector x(3); x[0]=orientation(0,0); x[1]=orientation(1,0); x[2]=orientation(2,0);
198  x=x/norm(x);
199 
200  if (dot(x,biggestAxis)<-0.4 || dot(x,biggestAxis)>0.4)
201  continue;
202 
203  if (hand=="right_arm")
204  {
205  if (orientation(0,0)>0.1 || orientation(2,1)>0.5 || orientation(2,0)<-0.1 || orientation(2,0)>0.9)
206  continue;
207  }
208  else
209  {
210  if (orientation(0,0)>0.1 || orientation(2,1)>0.5 || orientation(2,0)<-0.1 || orientation(2,0)>0.9)
211  continue;
212  }
213 
214  noResult=false;
215 
216  od=dcm2axis(orientation);
217  q=0.0;
218  manip=0.0;
219 
220  iCtrl->askForPose(eePos,od,xdhat,odhat,q);
221 
222  q=q*M_PI/180.0;
223  arm->setAng(q);
224 
225  od[0]=od[0]*od[3];
226  od[1]=od[1]*od[3];
227  od[2]=od[2]*od[3];
228  od.pop_back();
229 
230  odhat[0]=odhat[0]*odhat[3];
231  odhat[1]=odhat[1]*odhat[3];
232  odhat[2]=odhat[2]*odhat[3];
233  odhat.pop_back();
234 
235  double xdist=norm(eePos-xdhat);
236  double odist=norm(od-odhat);
237 
238  if (xdist>0.01 && odist>0.1)
239  continue;
240 
241  Jacobian=arm->GeoJacobian();
242  mulJac=Jacobian*(Jacobian.transposed());
243 
244  manip=sqrt(det(mulJac));
245 
246  double limits=0.0;
247  for (unsigned int k=0; k<thetaMin.size(); k++)
248  limits+=(q[k]-thetaMin[k])*(thetaMax[k]-q[k])/((thetaMax[k]-thetaMin[k])*(thetaMax[k]-thetaMin[k]));
249 
250  manip*=(1-exp(-limits));
251 
252  if (manip>bestManip)
253  {
254  bestManip=manip;
255  bestOrientation=orientation;
256  }
257  }
258  done=true;
259  work=false;
260  }
261  this->suspend();
262 }
263 
264 /************************************************************************/
265 void OrientationThread::getBestManip(double &manip, yarp::sig::Matrix &orientation)
266 {
267  manip=this->bestManip;
268  orientation=this->bestOrientation;
269 }
270 
271 /************************************************************************/
272 bool OrientationThread::checkDone()
273 {
274  return done;
275 }
276 
277 /************************************************************************/
278 bool OrientationThread::getResult()
279 {
280  return noResult;
281 }
282 
283 /************************************************************************/
284 bool OrientationThread::normalDirection(string &hand, yarp::sig::Vector &normal)
285 {
286  if (normal[1]<0 && hand=="left_arm")
287  return true;
288  if (normal[1]>0 && hand=="right_arm")
289  return true;
290 
291  return false;
292 }
293 
294 /************************************************************************/
295 void OrientationThread::close()
296 {
297  if (dCtrl.isValid())
298  dCtrl.close();
299 
300  if (robotArm.isValid())
301  robotArm.close();
302 
303  if (robotTorso.isValid())
304  robotTorso.close();
305 
306  delete arm;
307 }
308 
309 /************************************************************************/
310 void OrientationThread::threadRelease()
311 {
312  close();
313 }
314 
Generated by   doxygen 1.8.12