grasp
•All Data Structures Namespaces Functions Modules
handIKModule.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 "handIKModule.h"
18 
19 using namespace std;
20 using namespace yarp::os;
21 using namespace yarp::sig;
22 using namespace yarp::math;
23 using namespace iCub::ctrl;
24 using namespace iCub::iKin;
25 
26 /************************************************************************/
27 HandIKModule::HandIKModule()
28 {
29  done=true;
30  work=false;
31 }
32 
33 /************************************************************************/
34 bool HandIKModule::configure(ResourceFinder &rf)
35 {
36  string name=rf.check("name",Value("handIKModule1")).asString().c_str();
37  hand=rf.check("hand",Value("right")).asString().c_str();
38  /*hand=rf.find("hand").asString().c_str();
39  if (hand!="right" && hand!="left")
40  return false;*/
41  portName="/"+name+"/"+hand+"/out";
42  outputPort.open(portName.c_str());
43  string rpcPortName="/"+name+"/"+hand+"/rpc";
44  rpc.open(rpcPortName.c_str());
45  attach(rpc);
46  createCombinationVector();
47 
48  return true;
49 }
50 
51 /************************************************************************/
52 bool HandIKModule::extractData(const Bottle &data)
53 {
54  Bottle &b=const_cast<Bottle&>(data);
55  Bottle* c=b.find("center").asList();
56  fillVectorFromBottle(c,center);
57  Bottle* d=b.find("dim").asList();
58  fillVectorFromBottle(d,dim);
59  Bottle* c1B=b.find("c1").asList();
60  yarp::sig::Vector c1;
61  fillVectorFromBottle(c1B,c1);
62  c1.push_back(1.0);
63  Bottle* c2B=b.find("c2").asList();
64  yarp::sig::Vector c2;
65  fillVectorFromBottle(c2B,c2);
66  c2.push_back(1.0);
67  Bottle* c3B=b.find("c3").asList();
68  yarp::sig::Vector c3;
69  fillVectorFromBottle(c3B,c3);
70  c3.push_back(1.0);
71  Bottle* n1B=b.find("n1").asList();
72  yarp::sig::Vector n1;
73  fillVectorFromBottle(n1B,n1);
74  Bottle* n2B=b.find("n2").asList();
75  yarp::sig::Vector n2;
76  fillVectorFromBottle(n2B,n2);
77  Bottle* n3B=b.find("n3").asList();
78  yarp::sig::Vector n3;
79  fillVectorFromBottle(n3B,n3);
80  Bottle* rot=b.find("rot").asList();
81  fillMatrixFromBottle(rot,rotation,3,3);
82  contacts_r.push_back(c1);
83  contacts_r.push_back(c2);
84  contacts_r.push_back(c3);
85  normals_r.push_back(n1);
86  normals_r.push_back(n2);
87  normals_r.push_back(n3);
88  return true;
89 }
90 
91 /************************************************************************/
92 void HandIKModule::fillMatrixFromBottle(const Bottle* b, yarp::sig::Matrix &m, int rows, int cols)
93 {
94  int k=0;
95  m.resize(rows,cols);
96  for (int i=0; i<rows; i++)
97  {
98  for (int j=0; j<cols; j++)
99  {
100  m(i,j)=b->get(k).asDouble();
101  k++;
102  }
103  }
104 }
105 
106 /************************************************************************/
107 void HandIKModule::fillVectorFromBottle(const Bottle* b, yarp::sig::Vector &v)
108 {
109  v.resize(b->size());
110  for (int i=0; i<b->size(); i++)
111  v[i]=b->get(i).asDouble();
112 }
113 
114 /************************************************************************/
115 bool HandIKModule::interruptModule()
116 {
117  printf("interrupting\n");
118  outputPort.interrupt();
119  printf("interrupted out\n");
120  rpc.interrupt();
121  printf("interrupted rpc\n");
122  return true;
123 }
124 
125 /************************************************************************/
126 bool HandIKModule::close()
127 {
128  printf("closing\n");
129  outputPort.close();
130  printf("closed out\n");
131  rpc.close();
132  printf("closed rpc\n");
133  return true;
134 }
135 
136 /************************************************************************/
137 bool HandIKModule::updateModule()
138 {
139  if (work)
140  {
141  bestObjValue=10000;
142  int index=0;
143 
144  for (unsigned int i=0; i<combinations.size(); i++)
145  {
146  HandIK_Problem problem(hand,3);
147 
148  problem.dimensions[0]=dim[1];
149  problem.dimensions[1]=dim[0];
150  problem.dimensions[2]=dim[2];
151 
152  problem.contactPoints.push_back(contacts_o.at((unsigned int)combinations.at(i)[0]));
153  problem.contactPoints.push_back(contacts_o.at((unsigned int)combinations.at(i)[1]));
154  problem.contactPoints.push_back(contacts_o.at((unsigned int)combinations.at(i)[2]));
155 
156  problem.normalDirs.push_back(normals_o.at((unsigned int)combinations.at(i)[0]));
157  problem.normalDirs.push_back(normals_o.at((unsigned int)combinations.at(i)[1]));
158  problem.normalDirs.push_back(normals_o.at((unsigned int)combinations.at(i)[2]));
159 
160  HandIK_Solver solver(problem);
161 
162  //TODO: setting initial orientation in a smarter way
163  HandIK_Variables guess(problem.nFingers);
164  guess.xyz_ee[0]=0.0;
165  guess.xyz_ee[1]=0.0;
166  guess.xyz_ee[2]=0.1;
167 
168  if (hand=="right")
169  {
170  // the following orientation corresponds to the right hand
171  // put palm down on top of the object, with the middle finger
172  // aligned wrt the object y-axis
173  guess.rpy_ee[0]=-M_PI;
174  guess.rpy_ee[1]=0.0;
175  guess.rpy_ee[2]=M_PI/2.0;
176  }
177  else
178  {
179  guess.rpy_ee[0]=0.0;
180  guess.rpy_ee[1]=0.0;
181  guess.rpy_ee[2]=M_PI;
182  }
183  guess.joints=0.0;
184  solver.setInitialGuess(guess);
185  //printf("#### initial guess ...\n");
186  //guess.print();
187 
188  HandIK_Variables solution;
189  double t0=Time::now();
190  bool found=solver.solve(solution);
191  double t1=Time::now();
192 
193  printf("\n");
194  printf("#### %d solution found in %g [s] ...\n", i, t1-t0);
195 
196  double objFunc=solution.cost_fun;
197  printf("Cost %g\n", objFunc);
198  objFunc+=evaluateFingers(solution,i);
199  printf("Global cost %g\n", objFunc);
200  if (objFunc<bestObjValue)
201  {
202  bestObjValue=objFunc;
203  index=i;
204  bestSolution=solution;
205  }
206  }
207  winnerIndex=index;
208  bestSolution.print();
209 
210  Bottle out;
211  prepareData(out);
212 
213  outputPort.write(out);
214 
215  work=false;
216  done=true;
217  }
218  return true;
219 }
220 
221 /************************************************************************/
222 double HandIKModule::evaluateFingers(const HandIK_Variables &solution, const int id)
223 {
224  yarp::sig::Vector encoders(9,0.0);
225  encoders[0]=solution.joints[3];
226  encoders[1]=solution.joints[0];
227  encoders[2]=solution.joints[1];
228  encoders[3]=solution.joints[2];
229  encoders[4]=solution.joints[4];
230  encoders[5]=solution.joints[5];
231  encoders[6]=solution.joints[6];
232  encoders[7]=solution.joints[7];
233 
234  iCub::iKin::iCubFinger thumb(hand+"_thumb");
235  iCub::iKin::iCubFinger index(hand+"_index");
236  iCub::iKin::iCubFinger middle(hand+"_middle");
237 
238  yarp::sig::Vector joints_thumb;
239  thumb.getChainJoints(encoders,joints_thumb);
240  yarp::sig::Matrix thumbH=thumb.getH(joints_thumb);
241  yarp::sig::Vector thumbH_col3=thumbH.getCol(3);
242 
243  yarp::sig::Vector joints_index;
244  index.getChainJoints(encoders,joints_index);
245  yarp::sig::Matrix indexH=index.getH(joints_index);
246  yarp::sig::Vector indexH_col3=indexH.getCol(3);
247 
248  yarp::sig::Vector joints_middle;
249  middle.getChainJoints(encoders,joints_middle);
250  yarp::sig::Matrix middleH=middle.getH(joints_middle);
251  yarp::sig::Vector middleH_col3=middleH.getCol(3);
252 
253  yarp::sig::Matrix Hc=rpy2dcm(solution.rpy_ee);
254  Hc(0,3)=solution.xyz_ee[0];
255  Hc(1,3)=solution.xyz_ee[1];
256  Hc(2,3)=solution.xyz_ee[2];
257 
258  double res=0.0;
259  res+=norm2(contacts_o.at(combinations.at(id)[0])-(Hc*thumbH_col3));
260  res+=norm2(contacts_o.at(combinations.at(id)[1])-(Hc*indexH_col3));
261  res+=norm2(contacts_o.at(combinations.at(id)[2])-(Hc*middleH_col3));
262  res*=1e04;
263  printf("res %g\n", res);
264 
265  return res;
266 }
267 
268 /************************************************************************/
269 void HandIKModule::fromRootToObject()
270 {
271  /*Matrix rot_tran=zeros(4,4);
272 
273  rot_tran.setSubmatrix(rotation,0,0);
274  rot_tran(3,3)=1.0;
275  rot_tran(0,3)=center[0];
276  rot_tran(1,3)=center[1];
277  rot_tran(2,3)=center[2];
278  Matrix rotmat=SE3inv(rot_tran);*/
279 
280  rot_tran=zeros(4,4);
281 
282  rot_tran(1,0)=1.0;
283  rot_tran(0,1)=-1.0;
284  rot_tran(2,2)=1.0;
285  rot_tran(3,3)=1.0;
286  rot_tran(0,3)=center[0];
287  rot_tran(1,3)=center[1];
288  rot_tran(2,3)=center[2];
289  Matrix rotmat=SE3inv(rot_tran);
290 
291  Vector c1=rotmat*contacts_r.at(0);
292  Vector c2=rotmat*contacts_r.at(1);
293  Vector c3=rotmat*contacts_r.at(2);
294  Vector n1=rotmat.submatrix(0,2,0,2)*normals_r.at(0).subVector(0,2);
295  n1.push_back(1.0);
296  Vector n2=rotmat.submatrix(0,2,0,2)*normals_r.at(1).subVector(0,2);
297  n2.push_back(1.0);
298  Vector n3=rotmat.submatrix(0,2,0,2)*normals_r.at(2).subVector(0,2);
299  n3.push_back(1.0);
300  contacts_o.push_back(c1);
301  contacts_o.push_back(c2);
302  contacts_o.push_back(c3);
303  normals_o.push_back(n1);
304  normals_o.push_back(n2);
305  normals_o.push_back(n3);
306 }
307 
308 /************************************************************************/
309 void HandIKModule::createCombinationVector()
310 {
311  Vector v1(3); v1[0]=0; v1[1]=1; v1[2]=2;
312  Vector v2(3); v2[0]=0; v2[1]=2; v2[2]=1;
313  Vector v3(3); v3[0]=1; v3[1]=0; v3[2]=2;
314  Vector v4(3); v4[0]=1; v4[1]=2; v4[2]=0;
315  Vector v5(3); v5[0]=2; v5[1]=1; v5[2]=0;
316  Vector v6(3); v6[0]=2; v6[1]=0; v6[2]=1;
317  combinations.push_back(v1);
318  combinations.push_back(v2);
319  combinations.push_back(v3);
320  combinations.push_back(v4);
321  combinations.push_back(v5);
322  combinations.push_back(v6);
323 }
324 
325 /************************************************************************/
326 bool HandIKModule::respond(const Bottle& command, Bottle& reply)
327 {
328  tag=command.get(0).asString();
329  if (tag=="IK1" || tag=="IK2" || tag=="IK3" || tag=="IK4")
330  {
331  Network::connect(portName.c_str(),"/precision-grasp/rpc");
332  contacts_r.clear();
333  normals_r.clear();
334  contacts_o.clear();
335  normals_o.clear();
336  bool f=extractData(command);
337  if (f)
338  {
339  fromRootToObject();
340  work=true;
341  reply.addString("ack");
342  return true;
343  }
344  }
345  reply.addString("nack");
346  return true;
347 }
348 
349 /************************************************************************/
350 void HandIKModule::prepareData(yarp::os::Bottle &data)
351 {
352  data.addString(tag.c_str());
353  Bottle &handB=data.addList();
354  handB.addString("hand");
355  handB.addString(hand.c_str());
356  Bottle &cost=data.addList();
357  cost.addString("cost");
358  cost.addDouble(bestObjValue);
359 
360  rot_tran=zeros(4,4);
361 
362  rot_tran(1,0)=1.0;
363  rot_tran(0,1)=-1.0;
364  rot_tran(2,2)=1.0;
365  rot_tran(3,3)=1.0;
366  rot_tran(0,3)=center[0];
367  rot_tran(1,3)=center[1];
368  rot_tran(2,3)=center[2];
369 
370  yarp::sig::Vector ee_ob=bestSolution.xyz_ee;
371  ee_ob.push_back(1.0);
372 
373  yarp::sig::Vector ee_root=rot_tran*ee_ob;
374  yarp::sig::Matrix tmp=rpy2dcm(bestSolution.rpy_ee);
375  yarp::sig::Matrix tmp2=rot_tran*tmp;
376  yarp::sig::Vector or_root=dcm2axis(tmp2);
377 
378  Bottle &ee=data.addList();
379  ee.addString("ee");
380  Bottle &ee_coord=ee.addList();
381  ee_coord.addDouble(ee_root[0]);
382  ee_coord.addDouble(ee_root[1]);
383  ee_coord.addDouble(ee_root[2]);
384  Bottle &orientation=data.addList();
385  orientation.addString("or");
386  Bottle &or_coord=orientation.addList();
387  yarp::sig::Vector or_axisangle=dcm2axis(rpy2dcm(bestSolution.rpy_ee));
388  or_coord.addDouble(or_root[0]);
389  or_coord.addDouble(or_root[1]);
390  or_coord.addDouble(or_root[2]);
391  or_coord.addDouble(or_root[3]);
392  Bottle &j=data.addList();
393  j.addString("joints");
394  Bottle &joints=j.addList();
395  for (unsigned int i=0; i<bestSolution.joints.size(); i++)
396  joints.addDouble(bestSolution.joints[i]);
397  Bottle &order=data.addList();
398  order.addString("combination");
399  Bottle &combination=order.addList();
400  combination.addInt((int)combinations.at(winnerIndex)[0]);
401  combination.addInt((int)combinations.at(winnerIndex)[1]);
402  combination.addInt((int)combinations.at(winnerIndex)[2]);
403 }
404 
405 /************************************************************************/
406 double HandIKModule::getPeriod()
407 {
408  return 0.1;
409 }
410 
Generated by   doxygen 1.8.12