iCub-main
iKinHlp.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
11 #include <yarp/math/Math.h>
12 
13 #include <iCub/ctrl/math.h>
14 #include <iCub/iKin/iKinVocabs.h>
15 #include <iCub/iKin/iKinInv.h>
16 #include <iCub/iKin/iKinHlp.h>
17 
18 using namespace yarp::os;
19 using namespace yarp::sig;
20 using namespace yarp::math;
21 using namespace iCub::ctrl;
22 using namespace iCub::iKin;
23 
24 
25 /************************************************************************/
26 void CartesianHelper::addVectorOption(Bottle &b, const int vcb, const Vector &v)
27 {
28  Bottle &part=b.addList();
29  part.addVocab(vcb);
30  Bottle &vect=part.addList();
31 
32  for (size_t i=0; i<v.length(); i++)
33  vect.addDouble(v[i]);
34 }
35 
36 
37 /************************************************************************/
38 bool CartesianHelper::getDesiredOption(const Bottle &reply, Vector &xdhat,
39  Vector &odhat, Vector &qdhat)
40 {
41  if (reply.size()==0)
42  return false;
43 
44  if (reply.get(0).asVocab()==IKINSLV_VOCAB_REP_ACK)
45  {
46  // xdhat and odhat part
47  if (reply.check(Vocab::decode(IKINSLV_VOCAB_OPT_X)))
48  {
49  Bottle *xData=getEndEffectorPoseOption(reply);
50  if (xData->size()==0)
51  return false;
52 
53  xdhat.resize(3);
54  for (size_t i=0; i<xdhat.length(); i++)
55  xdhat[i]=xData->get(i).asDouble();
56 
57  odhat.resize(4);
58  for (size_t i=0; i<odhat.length(); i++)
59  odhat[i]=xData->get(xdhat.length()+i).asDouble();
60  }
61  else
62  return false;
63 
64  // qdhat part
65  if (reply.check(Vocab::decode(IKINSLV_VOCAB_OPT_Q)))
66  {
67  Bottle *qData=getJointsOption(reply);
68  if (qData->size()==0)
69  return false;
70 
71  qdhat.resize(qData->size());
72  for (size_t i=0; i<qdhat.length(); i++)
73  qdhat[i]=qData->get(i).asDouble();
74  }
75  else
76  return false;
77 
78  return true;
79  }
80  else
81  return false;
82 }
83 
84 
85 /************************************************************************/
86 void CartesianHelper::addTargetOption(Bottle &b, const Vector &xd)
87 {
88  addVectorOption(b,IKINSLV_VOCAB_OPT_XD,xd);
89 }
90 
91 
92 /************************************************************************/
93 void CartesianHelper::addDOFOption(Bottle &b, const Vector &dof)
94 {
95  addVectorOption(b,IKINSLV_VOCAB_OPT_DOF,dof);
96 }
97 
98 
99 /************************************************************************/
100 void CartesianHelper::addJointsResPosOption(Bottle &b, const Vector &restPos)
101 {
102  addVectorOption(b,IKINSLV_VOCAB_OPT_REST_POS,restPos);
103 }
104 
105 
106 /************************************************************************/
107 void CartesianHelper::addJointsRestWeightsOption(Bottle &b, const Vector &restWeights)
108 {
109  addVectorOption(b,IKINSLV_VOCAB_OPT_REST_WEIGHTS,restWeights);
110 }
111 
112 
113 /************************************************************************/
114 void CartesianHelper::addPoseOption(Bottle &b, const unsigned int pose)
115 {
116  Bottle &posePart=b.addList();
117  posePart.addVocab(IKINSLV_VOCAB_OPT_POSE);
118 
119  if (pose==IKINCTRL_POSE_FULL)
120  posePart.addVocab(IKINSLV_VOCAB_VAL_POSE_FULL);
121  else if (pose==IKINCTRL_POSE_XYZ)
122  posePart.addVocab(IKINSLV_VOCAB_VAL_POSE_XYZ);
123 }
124 
125 
126 /************************************************************************/
127 void CartesianHelper::addModeOption(Bottle &b, const bool tracking)
128 {
129  Bottle &modePart=b.addList();
130  modePart.addVocab(IKINSLV_VOCAB_OPT_MODE);
131 
132  if (tracking)
133  modePart.addVocab(IKINSLV_VOCAB_VAL_MODE_TRACK);
134  else
135  modePart.addVocab(IKINSLV_VOCAB_VAL_MODE_SINGLE);
136 }
137 
138 
139 /************************************************************************/
140 void CartesianHelper::addTokenOption(Bottle &b, const double token)
141 {
142  Bottle &tokenPart=b.addList();
143  tokenPart.addVocab(IKINSLV_VOCAB_OPT_TOKEN);
144  tokenPart.addDouble(token);
145 }
146 
147 
148 /************************************************************************/
149 Bottle *CartesianHelper::getTargetOption(const Bottle &b)
150 {
151  return b.find(Vocab::decode(IKINSLV_VOCAB_OPT_XD)).asList();
152 }
153 
154 
155 /************************************************************************/
156 Bottle *CartesianHelper::getEndEffectorPoseOption(const Bottle &b)
157 {
158  return b.find(Vocab::decode(IKINSLV_VOCAB_OPT_X)).asList();
159 }
160 
161 
162 /************************************************************************/
163 Bottle *CartesianHelper::getJointsOption(const Bottle &b)
164 {
165  return b.find(Vocab::decode(IKINSLV_VOCAB_OPT_Q)).asList();
166 }
167 
168 
169 /************************************************************************/
170 bool CartesianHelper::getTokenOption(const Bottle &b, double *token)
171 {
172  if (b.check(Vocab::decode(IKINSLV_VOCAB_OPT_TOKEN)) && (token!=NULL))
173  {
174  *token=b.find(Vocab::decode(IKINSLV_VOCAB_OPT_TOKEN)).asDouble();
175  return true;
176  }
177  else
178  return false;
179 }
180 
181 
182 /************************************************************************/
183 bool CartesianHelper::computeFixationPointData(iKinChain &eyeL,
184  iKinChain &eyeR,
185  Vector &fp)
186 {
187  Matrix HL=eyeL.getH();
188  Matrix HR=eyeR.getH();
189  HL(3,3)=HR(3,3)=0.0;
190 
191  double qty1=dot(HR,2,HL,2);
192  Matrix H1=HL-HR;
193  Matrix H2L=HL-qty1*HR;
194  Matrix H2R=qty1*HL-HR;
195  double qty2L=dot(H2L,2,H1,3);
196  double qty2R=dot(H2R,2,H1,3);
197  double qty3=qty1*qty1-1.0;
198 
199  if (fabs(qty3)<IKIN_ALMOST_ZERO)
200  return false;
201 
202  double tL=qty2L/qty3;
203  double tR=qty2R/qty3;
204 
205  if (fp.length()!=3)
206  fp.resize(3);
207 
208  for (int i=0; i<3; i++)
209  fp[i]=0.5*(HL(i,3)+tL*HL(i,2)+HR(i,3)+tR*HR(i,2));
210 
211  return true;
212 }
213 
214 
215 /************************************************************************/
216 bool CartesianHelper::computeFixationPointData(iKinChain &eyeL,
217  iKinChain &eyeR,
218  Vector &fp, Matrix &J)
219 {
220  Vector dfp1(4), dfp2(4);
221  Vector dfpL1(4),dfpL2(4);
222  Vector dfpR1(4),dfpR2(4);
223 
224  Matrix HL=eyeL.getH();
225  Matrix HR=eyeR.getH();
226  HL(3,3)=HR(3,3)=0.0;
227 
228  double qty1=dot(HR,2,HL,2);
229  Matrix H1=HL-HR;
230  Matrix H2L=HL-qty1*HR;
231  Matrix H2R=qty1*HL-HR;
232  Matrix H3(4,4); H3(3,2)=0.0;
233  double qty2L=dot(H2L,2,H1,3);
234  double qty2R=dot(H2R,2,H1,3);
235  double qty3=qty1*qty1-1.0;
236 
237  if (fabs(qty3)<IKIN_ALMOST_ZERO)
238  return false;
239 
240  double tL=qty2L/qty3;
241  double tR=qty2R/qty3;
242 
243  Matrix GeoJacobP_L=eyeL.GeoJacobian();
244  Matrix GeoJacobP_R=eyeR.GeoJacobian();
245  Matrix AnaJacobZ_L=eyeL.AnaJacobian(2);
246  Matrix AnaJacobZ_R=eyeR.AnaJacobian(2);
247 
248  // Left part
249  {
250  double dqty1, dqty1L, dqty1R;
251  double dqty2, dqty2L, dqty2R;
252  int j;
253 
254  Vector Hz=HL.getCol(2);
255  Matrix M=GeoJacobP_L.submatrix(0,3,0,1)+tL*AnaJacobZ_L.submatrix(0,3,0,1);
256 
257  // derivative wrt eye tilt
258  j=0;
259  dqty1=dot(AnaJacobZ_R,j,HL,2)+dot(HR,2,AnaJacobZ_L,j);
260  for (int i=0; i<3; i++)
261  H3(i,2)=AnaJacobZ_L(i,j)-dqty1*HR(i,2)-qty1*AnaJacobZ_R(i,j);
262  dqty2=dot(H3,2,H1,3)+dot(H2L,2,GeoJacobP_L-GeoJacobP_R,j);
263  dfp1=M.getCol(j)+Hz*((dqty2-2.0*qty1*qty2L*dqty1/qty3)/qty3);
264 
265  // derivative wrt pan left eye
266  j=1;
267  dqty1L=dot(HR,2,AnaJacobZ_L,j);
268  for (int i=0; i<3; i++)
269  H3(i,2)=AnaJacobZ_L(i,j)-dqty1*HR(i,2);
270  dqty2L=dot(H3,2,H1,3)+dot(H2L,2,GeoJacobP_L,j);
271  dfpL1=M.getCol(j)+Hz*((dqty2L-2.0*qty1*qty2L*dqty1L/qty3)/qty3);
272 
273  // derivative wrt pan right eye
274  dqty1R=dot(AnaJacobZ_R,j,HL,2);
275  for (int i=0; i<3; i++)
276  H3(i,2)=-dqty1*HR(i,2)-qty1*AnaJacobZ_R(i,j);
277  dqty2R=dot(H3,2,H1,3)+dot(H2L,2,-1.0*GeoJacobP_R,j);
278  dfpR1=Hz*((dqty2R-2.0*qty1*qty2L*dqty1R/qty3)/qty3);
279  }
280 
281  // Right part
282  {
283  double dqty1, dqty1L, dqty1R;
284  double dqty2, dqty2L, dqty2R;
285  int j;
286 
287  Vector Hz=HR.getCol(2);
288  Matrix M=GeoJacobP_R.submatrix(0,3,0,1)+tR*AnaJacobZ_R.submatrix(0,3,0,1);
289 
290  // derivative wrt eye tilt
291  j=0;
292  dqty1=dot(AnaJacobZ_R,j,HL,2)+dot(HR,2,AnaJacobZ_L,j);
293  for (int i=0; i<3; i++)
294  H3(i,2)=qty1*AnaJacobZ_L(i,j)+dqty1*HL(i,2)-AnaJacobZ_R(i,j);
295  dqty2=dot(H3,2,H1,3)+dot(H2R,2,GeoJacobP_L-GeoJacobP_R,j);
296  dfp2=M.getCol(j)+Hz*((dqty2-2.0*qty1*qty2R*dqty1/qty3)/qty3);
297 
298  // derivative wrt pan left eye
299  j=1;
300  dqty1L=dot(HR,2,AnaJacobZ_L,j);
301  for (int i=0; i<3; i++)
302  H3(i,2)=qty1*AnaJacobZ_L(i,j)+dqty1L*HL(i,2);
303  dqty2L=dot(H3,2,H1,3)+dot(H2R,2,GeoJacobP_L,j);
304  dfpL2=Hz*((dqty2L-2.0*qty1*qty2R*dqty1L/qty3)/qty3);
305 
306  // derivative wrt pan right eye
307  dqty1R=dot(AnaJacobZ_R,j,HL,2);
308  for (int i=0; i<3; i++)
309  H3(i,2)=dqty1R*HL(i,2)-AnaJacobZ_R(i,j);
310  dqty2R=dot(H3,2,H1,3)+dot(H2R,2,-1.0*GeoJacobP_R,j);
311  dfpR2=M.getCol(j)+Hz*((dqty2R-2.0*qty1*qty2R*dqty1R/qty3)/qty3);
312  }
313 
314  if (fp.length()!=3)
315  fp.resize(3);
316 
317  if ((J.rows()!=3) || (J.cols()!=3))
318  J.resize(3,3);
319 
320  for (int i=0; i<3; i++)
321  {
322  // fixation point position
323  fp[i]=0.5*(HL(i,3)+tL*HL(i,2)+HR(i,3)+tR*HR(i,2));
324 
325  // Jacobian
326  // r=p-v/2, l=p+v/2;
327  // dfp/dp=dfp/dl*dl/dp + dfp/dr*dr/dp = dfp/dl + dfp/dr;
328  // dfp/dv=dfp/dl*dl/dv + dfp/dr*dr/dv = (dfp/dl - dfp/dr)/2;
329  J(i,0)=0.50*(dfp1[i] + dfp2[i]); // tilt
330  J(i,1)=0.50*(dfpL1[i]+dfpR1[i] + dfpL2[i]+dfpR2[i]); // pan
331  J(i,2)=0.25*(dfpL1[i]-dfpR1[i] + dfpL2[i]-dfpR2[i]); // vergence
332  }
333 
334  return true;
335 }
336 
337 
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:910
#define IKIN_ALMOST_ZERO
Definition: iKinHlp.h:27
#define IKINSLV_VOCAB_OPT_TOKEN
Definition: iKinVocabs.h:33
double dot(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
Returns the dot product between two vectors given in the form: matrix(:,col).
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
yarp::sig::Matrix getH(const unsigned int i, const bool allLink=false)
Returns the rigid roto-translation matrix from the root reference frame to the ith frame in Denavit-H...
Definition: iKinFwd.cpp:731
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
#define IKINSLV_VOCAB_REP_ACK
Definition: iKinVocabs.h:48
static int v
Definition: iCub_Sim.cpp:47
#define IKINSLV_VOCAB_OPT_Q
Definition: iKinVocabs.h:32
static struct bpf_program fp
#define IKINSLV_VOCAB_OPT_REST_POS
Definition: iKinVocabs.h:35
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
Definition: iKinVocabs.h:45
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
#define IKINSLV_VOCAB_VAL_POSE_FULL
Definition: iKinVocabs.h:40
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1011
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition: iKinVocabs.h:44
#define IKINSLV_VOCAB_OPT_X
Definition: iKinVocabs.h:31
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
Definition: iKinVocabs.h:36
#define IKINSLV_VOCAB_OPT_DOF
Definition: iKinVocabs.h:28
#define IKINSLV_VOCAB_OPT_XD
Definition: iKinVocabs.h:30
#define IKINSLV_VOCAB_VAL_POSE_XYZ
Definition: iKinVocabs.h:41
#define IKINSLV_VOCAB_OPT_MODE
Definition: iKinVocabs.h:25
#define IKINSLV_VOCAB_OPT_POSE
Definition: iKinVocabs.h:26