grasp
•All Data Structures Namespaces Functions Modules
testHandIK.cpp
1 
2 #include <cmath>
3 #include <iostream>
4 #include <fstream>
5 #include <sstream>
6 #include <vector>
7 #include <cstdio>
8 #include <cstdlib>
9 
10 #include <yarp/os/Time.h>
11 #include <yarp/os/ResourceFinder.h>
12 #include <yarp/sig/all.h>
13 #include <iCub/ctrl/math.h>
14 #include "handIK.h"
15 
16 using namespace std;
17 using namespace yarp::os;
18 using namespace yarp::sig;
19 using namespace yarp::math;
20 using namespace iCub::ctrl;
21 
22 std::vector<yarp::sig::Vector> combinations;
23 std::vector<yarp::sig::Vector> contacts_o;
24 std::vector<yarp::sig::Vector> normals_o;
25 string hand="right";
26 
27 void vectorFromBottle(Bottle* b, Vector &v)
28 {
29  for (unsigned int i=0; i<b->size(); i++)
30  v[i]=b->get(i).asDouble();
31 }
32 
33 void readGrasp(ResourceFinder &rf, Vector &center, Vector &dim, std::vector<yarp::sig::Vector> &contacts_r, std::vector<yarp::sig::Vector> &normals_r)
34 {
35  Bottle* centerB=rf.find("center").asList();
36  vectorFromBottle(centerB,center);
37 
38  Bottle* dimB=rf.find("dim").asList();
39  vectorFromBottle(dimB,dim);
40 
41  Bottle* contact1=rf.find("contact1").asList();
42  Vector c1(4,1.0);
43  vectorFromBottle(contact1,c1);
44 
45  Bottle* contact2=rf.find("contact2").asList();
46  Vector c2(4,1.0);
47  vectorFromBottle(contact2,c2);
48 
49  Bottle* contact3=rf.find("contact3").asList();
50  Vector c3(4,1.0);
51  vectorFromBottle(contact3,c3);
52 
53  Bottle* normal1=rf.find("normal1").asList();
54  Vector n1(4,1.0);
55  vectorFromBottle(normal1,n1);
56 
57  Bottle* normal2=rf.find("normal2").asList();
58  Vector n2(4,1.0);
59  vectorFromBottle(normal2,n2);
60 
61  Bottle* normal3=rf.find("normal3").asList();
62  Vector n3(4,1.0);
63  vectorFromBottle(normal3,n3);
64 
65  contacts_r.push_back(c1);
66  contacts_r.push_back(c2);
67  contacts_r.push_back(c3);
68 
69  normals_r.push_back(n1);
70  normals_r.push_back(n2);
71  normals_r.push_back(n3);
72 }
73 
74 double evaluateFingers(const HandIK_Variables &solution, const int id)
75 {
76  yarp::sig::Vector encoders(9,0.0);
77  encoders[0]=solution.joints[3];
78  encoders[1]=solution.joints[0];
79  encoders[2]=solution.joints[1];
80  encoders[3]=solution.joints[2];
81  encoders[4]=solution.joints[4];
82  encoders[5]=solution.joints[5];
83  encoders[6]=solution.joints[6];
84  encoders[7]=solution.joints[7];
85 
86  iCub::iKin::iCubFinger thumb(hand+"_thumb");
87  iCub::iKin::iCubFinger index(hand+"_index");
88  iCub::iKin::iCubFinger middle(hand+"_middle");
89 
90  yarp::sig::Vector joints_thumb;
91  thumb.getChainJoints(encoders,joints_thumb);
92  yarp::sig::Matrix thumbH=thumb.getH(joints_thumb);
93  yarp::sig::Vector thumbH_col3=thumbH.getCol(3);
94 
95  yarp::sig::Vector joints_index;
96  index.getChainJoints(encoders,joints_index);
97  yarp::sig::Matrix indexH=index.getH(joints_index);
98  yarp::sig::Vector indexH_col3=indexH.getCol(3);
99 
100  yarp::sig::Vector joints_middle;
101  middle.getChainJoints(encoders,joints_middle);
102  yarp::sig::Matrix middleH=middle.getH(joints_middle);
103  yarp::sig::Vector middleH_col3=middleH.getCol(3);
104 
105  yarp::sig::Matrix Hc=rpy2dcm(solution.rpy_ee);
106  Hc(0,3)=solution.xyz_ee[0];
107  Hc(1,3)=solution.xyz_ee[1];
108  Hc(2,3)=solution.xyz_ee[2];
109 
110  double res=0.0;
111  res+=norm2(contacts_o.at(combinations.at(id)[0])-(Hc*thumbH_col3));
112  res+=norm2(contacts_o.at(combinations.at(id)[1])-(Hc*indexH_col3));
113  res+=norm2(contacts_o.at(combinations.at(id)[2])-(Hc*middleH_col3));
114  res*=1e04;
115  printf("res %g\n", res);
116 
117  return res;
118 }
119 
120 int test(int argc, char *argv[])
121 {
122  yarp::os::ResourceFinder rf;
123  rf.setVerbose(true);
124  rf.setDefaultContext("handIK");
125  rf.setDefaultConfigFile("contactPoints_fitness1.ini");
126  rf.configure(argc,argv);
127 
128  Vector center(3),dim(3);
129  std::vector<yarp::sig::Vector> contacts_r;
130  std::vector<yarp::sig::Vector> normals_r;
131 
132  readGrasp(rf,center,dim,contacts_r,normals_r);
133 
134  //Matrix to pass from root reference frame to the object reference frame
135 
136  // object reference frame:
137  // x-axis along the robot y
138  // y-axis along the robot -x
139  // z-axis along the robot z
140  Matrix tmp=zeros(4,4);
141 
142  tmp(1,0)=1.0;
143  tmp(0,1)=-1.0;
144  tmp(2,2)=1.0;
145  tmp(3,3)=1.0;
146  tmp(0,3)=center[0];
147  tmp(1,3)=center[1];
148  tmp(2,3)=center[2];
149 
150  Matrix rotmat=SE3inv(tmp);
151 
152  Vector c1=rotmat*contacts_r.at(0);
153  Vector c2=rotmat*contacts_r.at(1);
154  Vector c3=rotmat*contacts_r.at(2);
155 
156  Vector n1=rotmat.submatrix(0,2,0,2)*normals_r.at(0).subVector(0,2);
157  n1.push_back(1.0);
158 
159  Vector n2=rotmat.submatrix(0,2,0,2)*normals_r.at(1).subVector(0,2);
160  n2.push_back(1.0);
161 
162  Vector n3=rotmat.submatrix(0,2,0,2)*normals_r.at(2).subVector(0,2);
163  n3.push_back(1.0);
164 
165  contacts_o.push_back(c1);
166  contacts_o.push_back(c2);
167  contacts_o.push_back(c3);
168 
169  normals_o.push_back(n1);
170  normals_o.push_back(n2);
171  normals_o.push_back(n3);
172 
173  Vector v1(3); v1[0]=0; v1[1]=1; v1[2]=2;
174  Vector v2(3); v2[0]=0; v2[1]=2; v2[2]=1;
175  Vector v3(3); v3[0]=1; v3[1]=0; v3[2]=2;
176  Vector v4(3); v4[0]=1; v4[1]=2; v4[2]=0;
177  Vector v5(3); v5[0]=2; v5[1]=1; v5[2]=0;
178  Vector v6(3); v6[0]=2; v6[1]=0; v6[2]=1;
179  combinations.push_back(v1);
180  combinations.push_back(v2);
181  combinations.push_back(v3);
182  combinations.push_back(v4);
183  combinations.push_back(v5);
184  combinations.push_back(v6);
185 
186  HandIK_Variables bestSolution;
187  double bestObjValue=10000;
188  int index=0;
189 
190  for (unsigned int i=0; i<combinations.size(); i++)
191  {
192  //HandIK_Problem problem("right",2);
193  HandIK_Problem problem(hand,3);
194 
195  problem.dimensions[0]=dim[1];
196  problem.dimensions[1]=dim[0];
197  problem.dimensions[2]=dim[2];
198 
199  problem.contactPoints.push_back(contacts_o.at(combinations.at(i)[0]));
200  problem.contactPoints.push_back(contacts_o.at(combinations.at(i)[1]));
201  problem.contactPoints.push_back(contacts_o.at(combinations.at(i)[2]));
202 
203  problem.normalDirs.push_back(normals_o.at(combinations.at(i)[0]));
204  problem.normalDirs.push_back(normals_o.at(combinations.at(i)[1]));
205  problem.normalDirs.push_back(normals_o.at(combinations.at(i)[2]));
206 
207  HandIK_Solver solver(problem);
208 
209  //TODO: setting initial orientation in a smarter way
210  HandIK_Variables guess(problem.nFingers);
211  guess.xyz_ee[0]=0.0;
212  guess.xyz_ee[1]=0.0;
213  guess.xyz_ee[2]=0.1;
214  if (hand=="right")
215  {
216  // the following orientation corresponds to the right hand
217  // put palm down on top of the object, with the middle finger
218  // aligned wrt the object y-axis
219  guess.rpy_ee[0]=-M_PI;
220  guess.rpy_ee[1]=0.0;
221  guess.rpy_ee[2]=M_PI/2.0;
222  }
223  else
224  {
225  guess.rpy_ee[0]=0.0;
226  guess.rpy_ee[1]=0.0;
227  guess.rpy_ee[2]=M_PI;
228  }
229  guess.joints=0.0;
230  solver.setInitialGuess(guess);
231  //printf("#### initial guess ...\n");
232  //guess.print();
233 
234  HandIK_Variables solution;
235  double t0=Time::now();
236  bool found=solver.solve(solution);
237  double t1=Time::now();
238 
239  printf("\n");
240  printf("#### %d solution found in %g [s] ...\n", i, t1-t0);
241 
242  double objFunc=solution.cost_fun;
243  printf("Cost %g\n", objFunc);
244  objFunc+=evaluateFingers(solution,i);
245  printf("Global cost %g\n", objFunc);
246  if (objFunc<bestObjValue)
247  {
248  bestObjValue=objFunc;
249  index=i;
250  bestSolution=solution;
251  }
252  /*if (i==3)
253  {
254  index=i;
255  bestSolution=solution;
256  }*/
257  }
258 
259  printf("\n\nWinner index %d\n", index);
260  bestSolution.print();
261  printf("axis-angle_ee = (%s) [rad]\n",dcm2axis(rpy2dcm(bestSolution.rpy_ee)).toString(3,3).c_str());
262 
263  Vector joints=bestSolution.joints;
264 
265  Vector ee=bestSolution.xyz_ee;
266  ee.push_back(1.0);
267  ee=tmp*ee;
268  ee=ee.subVector(0,2);
269 
270  Matrix r=rpy2dcm(bestSolution.rpy_ee);
271  Vector axsangle=dcm2axis(tmp*r);
272 
273  std::string filename="C:/Users/Utente/Desktop/IKresult.txt";
274  ofstream file;
275  file.open(filename.c_str());
276 
277  file << "joints=[";
278  for (unsigned int i=0; i<joints.size(); i++)
279  file << joints[i] << " ";
280  file <<"];\n";
281 
282  file << "ee=[" << ee[0] << " " << ee[1] << " " << ee[2] << "];\n";
283  file << "axisangle=[" << axsangle[0] << " " << axsangle[1] << " " << axsangle[2] << " " << axsangle[3] << "];\n";
284  file << "center=[" << center[0] << " " << center[1] << " " << center[2] << "];\n";
285  file << "dim=[" << dim[0] << " " << dim[1] << " " << dim[2] << "];\n";
286  file << "c1=[" << contacts_r.at(combinations.at(index)[0])[0] << " " << contacts_r.at(combinations.at(index)[0])[1] << " " << contacts_r.at(combinations.at(index)[0])[2] << "];\n";
287  file << "c2=[" << contacts_r.at(combinations.at(index)[1])[0] << " " << contacts_r.at(combinations.at(index)[1])[1] << " " << contacts_r.at(combinations.at(index)[1])[2] << "];\n";
288  file << "c3=[" << contacts_r.at(combinations.at(index)[2])[0] << " " << contacts_r.at(combinations.at(index)[2])[1] << " " << contacts_r.at(combinations.at(index)[2])[2] << "];\n";
289  file << "n1=[" << normals_r.at(combinations.at(index)[0])[0] << " " << normals_r.at(combinations.at(index)[0])[1] << " " << normals_r.at(combinations.at(index)[0])[2] << "];\n";
290  file << "n2=[" << normals_r.at(combinations.at(index)[1])[0] << " " << normals_r.at(combinations.at(index)[1])[1] << " " << normals_r.at(combinations.at(index)[1])[2] << "];\n";
291  file << "n3=[" << normals_r.at(combinations.at(index)[2])[0] << " " << normals_r.at(combinations.at(index)[2])[1] << " " << normals_r.at(combinations.at(index)[2])[2] << "];\n";
292 
293  if (file.is_open())
294  file.close();
295 
296  return 0;
297 }
298 
299 
Generated by   doxygen 1.8.12