iCub-main
iDynTransform.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010-2011 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Matteo Fumagalli
4 * email: matteo.fumagalli@iit.it
5 * website: www.robotcub.org
6 * Permission is granted to copy, distribute, and/or modify this program
7 * under the terms of the GNU General Public License, version 2 or any
8 * later version published by the Free Software Foundation.
9 *
10 * A copy of the license can be found at
11 * http://www.robotcub.org/icub/license/gpl.txt
12 *
13 * This program is distributed in the hope that it will be useful, but
14 * WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16 * Public License for more details
17 */
18 
19 #include <iostream>
20 #include <iomanip>
22 
23 using namespace std;
24 using namespace yarp::sig;
25 using namespace yarp::math;
26 using namespace iCub::iKin;
27 using namespace iCub::iDyn;
28 
32 iGenericFrame::iGenericFrame()
33 {
34  initFTransform();
35 }
36 iGenericFrame::iGenericFrame(const Matrix &_R, double _x, double _y, double _z)
37 {
38  initFTransform();
39  setP(_x,_y,_z);
40  setR(_R);
41  setH(R,p);
42 }
43 iGenericFrame::iGenericFrame(const Matrix &_R, const Vector &_p)
44 {
45  initFTransform();
46  setP(_p);
47  setR(_R);
48  setH(R,p);
49 }
50 void iGenericFrame::initFTransform()
51 {
52  R.resize(3,3);
53  H.resize(4,4);
54  p.resize(3);
55  FT.resize(6);
56  p=0.0;
57  R=eye(3,3);
58  H=eye(4,4);
59  FT=0.0;
60 }
61 void iGenericFrame::setR(const Matrix &_R)
62 {
63  R=_R;
64 }
65 void iGenericFrame::setP(double _x, double _y, double _z)
66 {
67  p(0)=_x;
68  p(1)=_y;
69  p(2)=_z;
70 }
71 void iGenericFrame::setP(const Vector &_p)
72 {
73  p=_p;
74 }
75 
76 void iGenericFrame::setPRH(const Matrix &_H)
77 {
78  for(int i=0;i<3;i++)
79  p(i)=_H(i,3);
80  R=_H.submatrix(0,2,0,2);
81  setH(_H);
82 }
83 void iGenericFrame::setPRH()
84 {
85  for(int i=0;i<3;i++)
86  p(i)=H(i,3);
87  R=H.submatrix(0,2,0,2);
88 }
89 void iGenericFrame::setPRH(const Matrix &_R,const Vector &_p)
90 {
91  setH(_R,_p);
92  setPRH(H);
93 }
94 void iGenericFrame::setH(const Matrix &_R, double _x, double _y, double _z)
95 {
96  H=eye(4,4);
97  for(int i=0; i<3; i++)
98  for(int j=0; j<3; j++)
99  H(i,j)=_R(i,j);
100  H(0,3)=_x;
101  H(1,3)=_y;
102  H(2,3)=_z;
103 }
104 void iGenericFrame::setH(const Matrix &_R, const Vector &_p)
105 {
106  H=eye(4,4);
107  for(int i=0; i<3; i++)
108  {
109  for(int j=0; j<3; j++)
110  {
111  H(i,j)=_R(i,j);
112  }
113  H(i,3)=_p(i);
114  }
115 }
116 void iGenericFrame::setH(const Matrix &_H)
117 {
118  R=_H.submatrix(0,2,0,2);
119  for(int i=0;i<3;i++)
120  p(i)=_H(i,3);
121  H=_H;
122 }
123 yarp::sig::Vector iGenericFrame::setFT(const Vector &_FT)
124 {
125  return FT=_FT;
126 }
127 
131 // Definition of iFrameOnLink
132 
133 iFrameOnLink::iFrameOnLink()
134 {
135  l=0;
136  initSFrame();
137 }
138 
139 iFrameOnLink::iFrameOnLink(int _l)
140 {
141  l=_l;
142  initSFrame();
143 }
144 void iFrameOnLink::initSFrame()
145 {
146  H.resize(4,4);
147  FT.resize(6);
148  H=0.0;
149  FT=0.0;
150  Sensor=0;
151  Limb=0;
152  Link=new iGenericFrame();
153 }
154 
155 void iFrameOnLink::setLink(int _l)
156 {
157  l=_l;
158 }
159 void iFrameOnLink::setFT(const yarp::sig::Vector &_FT)
160 {
161  FT=Sensor->setFT(_FT);
162  //fprintf(stderr,"");
163 }
164 
165 void iFrameOnLink::setSensorKin(int _l)
166 {
167  Link->setPRH(Limb->getH(_l));
168  H=Link->getH()*Sensor->getH();
169 }
170 
171 void iFrameOnLink::setSensorKin()
172 {
173  Link->setPRH(Limb->getH(l));
174  Matrix H1 = Link->getH();
175  Matrix H2 = Sensor->getH();
176  H=Link->getH()*Sensor->getH();
177 }
178 
179 void iFrameOnLink::setSensorKin(const Matrix &_H)
180 {
181  Link->setPRH(_H);
182  H=_H*Sensor->getH();
183 }
184 
185 void iFrameOnLink::setSensor(int _l, const yarp::sig::Vector &_FT)
186 {
187  setSensorKin(_l);
188  setFT(_FT);
189 }
190 
191 void iFrameOnLink::setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Vector &_FT)
192 {
193  setSensorKin(_H);
194  setFT(_FT);
195 }
196 
197 void iFrameOnLink::setSensor(const yarp::sig::Vector &_FT)
198 {
199  setSensorKin();
200  setFT(_FT);
201 }
202 
203 Vector iFrameOnLink::getFT()
204 {
205  return FT;
206 }
207 Matrix iFrameOnLink::getH()
208 {
209  return H;
210 }
211 
212 void iFrameOnLink::attach(iKinChain *_Limb)
213 {
214  Limb=_Limb;
215 }
216 
217 void iFrameOnLink::attach(iGenericFrame *_Sensor)
218 {
219  Sensor=_Sensor;
220 }
221 
222 iFrameOnLink::~iFrameOnLink()
223 {
224  if (Link)
225  delete Link;
226 }
227 
231 
232 
233 iFTransformation::iFTransformation()
234 {
235  ownLimb=true;
236  Sensor=new iFrameOnLink();
237  EndEffector=new iGenericFrame();
238  Limb= new iDynChain();
239  initiFTransformation();
240 }
241 iFTransformation::iFTransformation(int _l)
242 {
243  l=_l;
244  ownLimb=true;
245  Sensor = new iFrameOnLink(l);
246  Limb= new iKinChain();
247  EndEffector=new iGenericFrame();
248  initiFTransformation();
249  //Sensor->setLink(l);
250 }
251 iFTransformation::iFTransformation(iDynInvSensor *_iDynChainWithSensor)
252 {
253  ownLimb=false;
254  initiFTransformation();
255  l=_iDynChainWithSensor->getSensorLink();
256  Limb = _iDynChainWithSensor->chain;
257  SensorFrame = new iGenericFrame(_iDynChainWithSensor->getH().submatrix(0,2,0,2),_iDynChainWithSensor->getH().submatrix(0,2,0,3).getCol(3));
258  Sensor = new iFrameOnLink(l);
259  EndEffector=new iGenericFrame();
260 
261  Sensor->attach(Limb);
262  Sensor->attach(SensorFrame);
263  //fprintf(stderr,"set up sensor transformation\n");
264 }
265 void iFTransformation::attach(iKinChain *_Limb)
266 {
267  if (Limb && ownLimb)
268  delete Limb;
269 
270  Limb=_Limb;
271  Sensor->attach(_Limb);
272  ownLimb=false;
273 }
274 void iFTransformation::attach(iGenericFrame *_Sensor)
275 {
276  Sensor->attach(_Sensor);
277 }
278 void iFTransformation::initiFTransformation()
279 {
280  //Sensor=0;
281  //Limb=0;
282  //EndEffector=0;
283  l=0;
284  Hs.resize(4,4);
285  Hs=0.0;
286  Fs.resize(6);
287  Fs=0.0;
288  He.resize(4,4);
289  He=0.0;
290  Fe.resize(6);
291  Fe=0.0;
292  Tse.resize(6,6);
293  Tse=0.0;
294  Teb.resize(6,6);
295  Teb=0.0;
296 
297  d.resize(3);
298  d=0.0;
299  S.resize(3,3);
300  S=0.0;
301  R.resize(3,3);
302  R=0.0;
303 
304  SensorFrame=0;
305 }
306 void iFTransformation::setLink(int _l)
307 {
308  Sensor->setLink(_l);
309  l=_l;
310 }
311 void iFTransformation::setSensor(const Vector &_FT)
312 {
313  Fs=_FT;
314  Sensor->setSensor(_FT);
315  Hs=Sensor->getH();
316 }
317 void iFTransformation::setSensor(int _l, const Vector &_FT)
318 {
319  Fs=_FT;
320  l=_l;
321  Sensor->setSensor(_l, _FT);
322  Hs=Sensor->getH();
323 }
324 void iFTransformation::setSensor(const Matrix &_H, const Vector &_FT)
325 {
326  Fs=_FT;
327  Sensor->setSensor(_H, _FT);
328  Hs=Sensor->getH();
329 }
330 void iFTransformation::setHe()
331 {
332  EndEffector->setH(Limb->getH());
333  He=EndEffector->getH();
334 }
335 void iFTransformation::setHe(int _l)
336 {
337  EndEffector->setH(Limb->getH(_l));
338  He=EndEffector->getH();
339 }
340 void iFTransformation::setHe(const Matrix &_H)
341 {
342  EndEffector->setH(_H);
343  He=EndEffector->getH();
344 }
345 void iFTransformation::setTeb()
346 {
347  setHe();
348  for(int i=0;i<3;i++)
349  {
350  for(int j=0; j<3; j++)
351  {
352  Teb(i,j)=He(i,j);
353  Teb(i+3,j+3)=He(i,j);
354  }
355  }
356 }
357 yarp::sig::Vector iFTransformation::getEndEffWrench()
358 {
359  setTse();
360  setFe();
361  return Fe;
362 }
363 yarp::sig::Vector iFTransformation::getEndEffWrench(const Vector &_FT)
364 {
365  setSensor(_FT);
366  return getEndEffWrench();
367 }
368 yarp::sig::Vector iFTransformation::getEndEffWrenchAsBase()
369 {
370  getEndEffWrench();
371  setTeb();
372  return Teb*Fe;
373 }
374 yarp::sig::Vector iFTransformation::getEndEffWrenchAsBase(const Vector &_FT)
375 {
376  setSensor(_FT);
377  return getEndEffWrenchAsBase();
378 }
379 void iFTransformation::setFe()
380 {
381  Fe=Tse*Fs;
382 }
383 void iFTransformation::setTse()
384 {
385  S=0.0;
386  Tse=0.0;
387  R=0.0;
388  for(int i=0; i<3;i++)
389  {
390  d(i)=(Hs(i,3)-He(i,3));
391  }
392  d=He.submatrix(0,2,0,2).transposed()*d;
393  S(0,1)=-d(2);
394  S(0,2)=d(1);
395  S(1,0)=d(2);
396  S(1,2)=-d(0);
397  S(2,0)=-d(1);
398  S(2,1)=d(0);
399 
400  R=He.submatrix(0,2,0,2).transposed()*Hs.submatrix(0,2,0,2);
401  S=S*R;
402 
403  for(int i=0; i<3;i++)
404  {
405  for(int j=0; j<3;j++)
406  {
407  Tse(i,j)=R(i,j);
408  Tse(i+3,j+3)=R(i,j);
409  Tse(i+3,j)=S(i,j);
410  }
411 
412  }
413 
414 }
415 iFTransformation::~iFTransformation()
416 {
417  if (Limb && ownLimb)
418  {
419  delete Limb;
420  Limb = 0;
421  }
422  if (Sensor)
423  {
424  delete Sensor;
425  Sensor = 0;
426  }
427  if (EndEffector)
428  {
429  delete EndEffector;
430  EndEffector = 0;
431  }
432  if (SensorFrame)
433  {
434  delete SensorFrame;
435  SensorFrame = 0;
436  }
437 }
438 
439 
iCub::iDyn::iDynInvSensor
Definition: iDynInv.h:1222
H
H
Definition: compute_ekf_fast.m:27
iCub::iDyn::iDynChain
Definition: iDyn.h:532
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iCub::iDyn::iDynInvSensor::getH
yarp::sig::Matrix getH() const
Get the sensor roto-translational matrix defining its position/orientation wrt the link.
Definition: iDynInv.cpp:2197
iCub::iKin::iKinChain::getH
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:740
p
p
Definition: show_eyes_axes.m:23
iCub::iDyn::iDynInvSensor::getSensorLink
unsigned int getSensorLink() const
Definition: iDynInv.cpp:2277
iCub::iDyn
Definition: iDyn.h:81
iCub::iKin
Definition: iKinFwd.h:71
iCub::iDyn::iGenericFrame
Definition: iDynTransform.h:121
iCub::iDyn::iDynInvSensor::chain
iDynChain * chain
the iDynChain describing the robotic chain
Definition: iDynInv.h:1232
iDynTransform.h