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
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 {
31
32  for (size_t i=0; i<v.length(); i++)
34 }
35
36
37 /************************************************************************/
38 bool CartesianHelper::getDesiredOption(const Bottle &reply, Vector &xdhat,
39  Vector &odhat, Vector &qdhat)
40 {
42  return false;
43
45  {
46  // xdhat and odhat part
48  {
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).asFloat64();
56
57  odhat.resize(4);
58  for (size_t i=0; i<odhat.length(); i++)
59  odhat[i]=xData->get(xdhat.length()+i).asFloat64();
60  }
61  else
62  return false;
63
64  // qdhat part
66  {
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).asFloat64();
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 {
89 }
90
91
92 /************************************************************************/
93 void CartesianHelper::addDOFOption(Bottle &b, const Vector &dof)
94 {
96 }
97
98
99 /************************************************************************/
100 void CartesianHelper::addJointsResPosOption(Bottle &b, const Vector &restPos)
101 {
103 }
104
105
106 /************************************************************************/
107 void CartesianHelper::addJointsRestWeightsOption(Bottle &b, const Vector &restWeights)
108 {
110 }
111
112
113 /************************************************************************/
114 void CartesianHelper::addPoseOption(Bottle &b, const unsigned int pose)
115 {
118
119  if (pose==IKINCTRL_POSE_FULL)
121  else if (pose==IKINCTRL_POSE_XYZ)
123 }
124
125
126 /************************************************************************/
127 void CartesianHelper::addModeOption(Bottle &b, const bool tracking)
128 {
131
132  if (tracking)
134  else
136 }
137
138
139 /************************************************************************/
140 void CartesianHelper::addTokenOption(Bottle &b, const double token)
141 {
145 }
146
147
148 /************************************************************************/
149 Bottle *CartesianHelper::getTargetOption(const Bottle &b)
150 {
151  return b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_XD)).asList();
152 }
153
154
155 /************************************************************************/
156 Bottle *CartesianHelper::getEndEffectorPoseOption(const Bottle &b)
157 {
158  return b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_X)).asList();
159 }
160
161
162 /************************************************************************/
163 Bottle *CartesianHelper::getJointsOption(const Bottle &b)
164 {
165  return b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_Q)).asList();
166 }
167
168
169 /************************************************************************/
170 bool CartesianHelper::getTokenOption(const Bottle &b, double *token)
171 {
172  if (b.check(Vocab32::decode(IKINSLV_VOCAB_OPT_TOKEN)) && (token!=NULL))
173  {
174  *token=b.find(Vocab32::decode(IKINSLV_VOCAB_OPT_TOKEN)).asFloat64();
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
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
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:732
yarp::sig::Matrix GeoJacobian(const unsigned int i)
Returns the geometric Jacobian of the ith link.
Definition: iKinFwd.cpp:1012
yarp::sig::Matrix AnaJacobian(const unsigned int i, unsigned int col)
Returns the analitical Jacobian of the ith link.
Definition: iKinFwd.cpp:911
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).
#define IKIN_ALMOST_ZERO
Definition: iKinHlp.h:27
#define IKINCTRL_POSE_FULL
Definition: iKinInv.h:37
#define IKINCTRL_POSE_XYZ
Definition: iKinInv.h:38
#define IKINSLV_VOCAB_OPT_DOF
Definition: iKinVocabs.h:28
#define IKINSLV_VOCAB_VAL_POSE_FULL
Definition: iKinVocabs.h:40
#define IKINSLV_VOCAB_VAL_MODE_TRACK
Definition: iKinVocabs.h:44
#define IKINSLV_VOCAB_VAL_MODE_SINGLE
Definition: iKinVocabs.h:45
#define IKINSLV_VOCAB_OPT_POSE
Definition: iKinVocabs.h:26
#define IKINSLV_VOCAB_OPT_Q
Definition: iKinVocabs.h:32
#define IKINSLV_VOCAB_OPT_MODE
Definition: iKinVocabs.h:25
#define IKINSLV_VOCAB_REP_ACK
Definition: iKinVocabs.h:48
#define IKINSLV_VOCAB_OPT_TOKEN
Definition: iKinVocabs.h:33
#define IKINSLV_VOCAB_OPT_REST_POS
Definition: iKinVocabs.h:35
#define IKINSLV_VOCAB_VAL_POSE_XYZ
Definition: iKinVocabs.h:41
#define IKINSLV_VOCAB_OPT_REST_WEIGHTS
Definition: iKinVocabs.h:36
#define IKINSLV_VOCAB_OPT_X
Definition: iKinVocabs.h:31
#define IKINSLV_VOCAB_OPT_XD
Definition: iKinVocabs.h:30
static struct bpf_program fp