iCub-main
iDynTransform.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2011 RobotCub Consortium
3  * Author: Matteo Fumagalli
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
33 /* How to use the FT sensor transformation:
34 
35 istantiate your limb (example: iCubArm *arm)
36 istantiate a iFrameOnLink Variable as pointer (iFrameOnLink *Sens) and a pointer to a variable of type iGenericFrame
37 (ex: iGenericFrame *sensore)
38 Sens need a iGenericFrame and an iKinLimb variables with some properties defining the location of the sensor over a link of the
39 iKinLimb chain.
40 
41 example code:
42 
43  Matrix Rs(3,3);
44  Vector ps(3);
45  Rs=...
46  ps=...
47  iCubArm *arm = new iCubArm("left");
48  iKinChain *chain;
49  iGenericFrame *sensore = new iGenericFrame(Rs,ps);
50  iFrameOnLink *Sens = new iFrameOnLink(2);
51  chain=arm->asChain();
52 
53  Sens->attach(sensore);
54  Sens->attach(arm);
55 
56  Vector q(7);
57  q=...
58  Vector FT;
59  FT.resize(6);
60  FT = ReadFT(); //a function that reads the FT values from a sensor
61 
62  Vector FT_B; //will be the vector of forces and torqes in the base frame
63 
64  arm->setAng(q);
65  sensore->setFT(FT);
66  FT_B = Sens->getFT();
67  *
68  */
69 
70 #ifndef __IFORCECONTROL_H__
71 #define __IFORCECONTROL_H__
72 
73 #include <yarp/sig/Vector.h>
74 #include <yarp/sig/Matrix.h>
75 #include <yarp/math/Math.h>
76 #include <yarp/math/SVD.h>
77 
78 #include <iCub/ctrl/math.h>
79 //#include <iCub/iKin/iKinFwd.h>
80 #include <iCub/iDyn/iDyn.h>
81 #include <iCub/iDyn/iDynInv.h>
82 
83 #include <string>
84 //#include <deque>
85 
86 
87 namespace iCub
88 {
89 
90 namespace iDyn
91 {
92  class iDynLink;
93  class iDynChain;
94  class iDynLimb;
95  class iCubArmDyn;
96  class iCubLegDyn;
97  class iCubEyeDyn;
98  class iCubEyeNeckRefDyn;
99  class iCubInertialSensorDyn;
100  class iFakeDyn;
101  class iFakeDyn2GdL;
102  class iDynSensor;
103  class iDynSensorLeg;
104  class iDynSensorArm;
105  class iDynInvSensor;
106  class OneLinkNewtonEuler;
107  class BaseLinkNewtonEuler;
108  class FinalLinkNewtonEuler;
109  class SensorLinkNewtonEuler;
110  class OneChainNewtonEuler;
111  class iGenericFrame;
112  class iFrameOnLink;
113  class iFTransformation;
114 
115 
122 {
123 private:
124  // Default constructor: not implemented.
125  yarp::sig::Matrix R; // Rotation Matrix wrt the link reference frame on which the FT sensor is put
126  yarp::sig::Vector p; // Vector distance of the FT sensor wrt the link reference frame on which the FT sensor is put
127  yarp::sig::Matrix H; // Rototranslation matrix
128  yarp::sig::Vector FT;
129 public:
130  /* Default Constructor */
131  iGenericFrame();
132  /*
133  * Overload constructor
134  * @param _R is the rotation matrix with respect to another reference frame
135  * @param _x, _y, _z are the position vector of one reference frame with respect
136  * to another
137  */
138  iGenericFrame(const yarp::sig::Matrix &_R, double _x, double _y, double _z);
139  /*
140  * Overload constructor
141  * @param _R as previously
142  * @param _p is a vector composed by {_x, _y, _z} of the previous definition
143  */
144  iGenericFrame(const yarp::sig::Matrix &_R, const yarp::sig::Vector &_p);
145 
146  /*Initializes the variables. Used in all the constructors*/
147  void initFTransform();
148 
149  /*
150  * Set the Rotation matrix among two reference Frame
151  * @param _R is the rotation matrix with respect to another reference frame
152  */
153  void setR(const yarp::sig::Matrix &_R);
154 
155  /*
156  * Set the distance between the two reference Frame
157  * @param _x, _y, _z are the position vector of one reference frame with respect
158  * to another
159  */
160  void setP(double _x, double _y, double _z);
161  /*
162  * Set the distance between the two reference Frame
163  * where _p is composed by {_x, _y, _z} of the previous definition
164  */
165  void setP(const yarp::sig::Vector &_p);
166 
167  /* Set the homogeneous transformation among two frames */
168  /*
169  * @param _R is the rotation matrix with respect to another reference frame
170  * @param _x, _y, _z are the position vector of one reference frame with respect
171  * to another
172  */
173  void setH(const yarp::sig::Matrix &_R, double _x, double _y, double _z);
174  /*
175  * @param _R is the rotation matrix with respect to another reference frame
176  * @param _p is composed by {_x, _y, _z} of the previous definition
177  */
178  void setH(const yarp::sig::Matrix &_R, const yarp::sig::Vector &_p);
179  /*
180  * @param _H is the homogeneous transformation matrix among 2 FoR
181  */
182  void setH(const yarp::sig::Matrix &_H);
183 
184  /*set every variable (H, R, p, S, T) given H(4,4)
185  * H is set as default or using previously a setH(...)
186  */
187  void setPRH(void);
188  /*
189  * @param _R is the rotation matrix with respect to another reference frame
190  * @param _p is the distance vector among 2 FoR
191  */
192  void setPRH(const yarp::sig::Matrix &_R, const yarp::sig::Vector &_p);
193  /*
194  * @param _H is the homogeneous transformation matrix among 2 FoR
195  */
196  void setPRH(const yarp::sig::Matrix &_H);
197 
198  /* Set the FT datas */
199  /*
200  * @param _FT is the wrench to be transformed, in its native FoR
201  */
202  yarp::sig::Vector setFT(const yarp::sig::Vector &_FT);
203 
204  /* get the members defined above */
205  yarp::sig::Vector getP(){return p;}
206  yarp::sig::Matrix getR(){return R;}
207  yarp::sig::Matrix getH(){return H;}
208 
209  /*return the Wrench set with setFT*/
210  yarp::sig::Vector getFT(){return FT;}
211 
212  /* Destructor*/
214 };
215 
223 {
224 private:
225  int l;
226  yarp::sig::Matrix H;
227  yarp::sig::Vector FT;
228 
229  iGenericFrame *Sensor;
230  iGenericFrame *Link;
231  iKin::iKinChain *Limb;
232 
233  /*
234  * initializes the iFrameOnLink members to zero. Used in the constructors.
235  */
236  void initSFrame();
237 
238 protected:
239 
240  //set transformation variables of/ the sensor:
241  void setSensorKin(int _l);
242  void setSensorKin(const yarp::sig::Matrix &_H);
243  void setSensorKin();
244 
245  void setFT(const yarp::sig::Vector &_FT);
246 public:
247  // Default constructor:
248  iFrameOnLink();
249  /* Overloaded constructor:
250  * @param _l defines the link on which the FT sensor is attached
251  */
252  iFrameOnLink(int _l);
253  /*Destructor*/
254  ~iFrameOnLink();
255 
256  void setLink(int _l);
257  //Set sensor variables:
258  /* @param _Rs is the rotation matrix of the sensor FoR with respect to the one of the link the sensor is attached to.*/
259  void setRs(const yarp::sig::Matrix &_Rs){ Sensor->setR(_Rs);}
260  /* @param _ps is the position vector of the sensor FoR with respect to the one of the link the sensor is attached to.*/
261  void setPs(const yarp::sig::Vector &_ps){Sensor->setP(_ps);}
262  /* @param _Hs is the homogeneous transformation matrix of the sensor FoR with respect to the one
263  * of the link the sensor is attached to. */
264  void setHs(const yarp::sig::Matrix &_Hs){Sensor->setH(_Hs);}
265 
266  //get sensor members, previously defined
267  yarp::sig::Matrix getRs(){return Sensor->getR();}
268  yarp::sig::Vector getPs(){return Sensor->getP();}
269  yarp::sig::Matrix getHs(){return Sensor->getH();}
270 
271  //get Limb variables
272  /* @param _Rl is the rotation matrix of the link FoR with respect to the base FoR*/
273  yarp::sig::Matrix getRl(){return Link->getR();}
274  /* @param _pl is the position vector of the sensor FoR with respect to the base FoR */
275  yarp::sig::Vector getPl(){return Link->getP();}
276  /* @param _Hl is the homogeneous transformation matrix of the sensor FoR with respect to the base FoR */
277  yarp::sig::Matrix getHl(){return Link->getH();}
278 
279  //set transformation variables of the sensor, with respect to a base frame:
280  yarp::sig::Matrix getH();
281 
282  void setSensor(int _l, const yarp::sig::Vector &_FT);
283  void setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Vector &_FT);
284  void setSensor(const yarp::sig::Vector &_FT);
285 
286  // get FT variables with respect to a fixed reference frame
287  yarp::sig::Vector getFT();
288 
289  void attach(iKin::iKinChain *_Limb);
290  void attach(iGenericFrame *_Sensor);
291 };
292 
293 
299 {
300 private:
301  int l;
302  yarp::sig::Matrix Hs;
303  yarp::sig::Vector Fs;
304  yarp::sig::Matrix He;
305  yarp::sig::Vector Fe;
306  yarp::sig::Matrix Tse;
307  yarp::sig::Matrix Teb;
308 
309  iFrameOnLink *Sensor;
310  iGenericFrame *EndEffector;
311  iGenericFrame *SensorFrame;
312  iKin::iKinChain *Limb;
313 
314  yarp::sig::Vector d;
315  yarp::sig::Matrix S;
316  yarp::sig::Matrix R;
317 
318  bool ownLimb;
319 
320  void initiFTransformation();
321 
322 public:
324  iFTransformation(int _l);
325  iFTransformation(iDyn::iDynInvSensor *_iDynChainWithSensor);
327 
328  void attach(iKin::iKinChain *_Limb);
329  void attach(iGenericFrame *_Sensor);
330 
331  void setLink(int _l);
332 
333  void setSensor(const yarp::sig::Vector &_FT);
334  void setSensor(int _l, const yarp::sig::Vector &_FT);
335  void setSensor(const yarp::sig::Matrix &_H, const yarp::sig::Vector &_FT);
336 
337  void setHe();
338  void setHe(int _l);
339  void setHe(const yarp::sig::Matrix &_H);
340 
341  void setTeb();
342  void setTse();
343  void setFe();
344 
345  yarp::sig::Vector getEndEffWrench();
346  yarp::sig::Vector getEndEffWrenchAsBase();
347  yarp::sig::Vector getEndEffWrench(const yarp::sig::Vector &_FT);
348  yarp::sig::Vector getEndEffWrenchAsBase(const yarp::sig::Vector &_FT);
349  yarp::sig::Matrix getHs(){return Hs;}
350  yarp::sig::Matrix getHe(){return He;}
351 
352 
353 };
354 
355 }
356 
357 }//end namespace
358 
359 #endif
360 
361 
362 
iCub::iDyn::iGenericFrame::initFTransform
void initFTransform()
Definition: iDynTransform.cpp:50
iCub::iDyn::iDynInvSensor
Definition: iDynInv.h:1222
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
H
H
Definition: compute_ekf_fast.m:27
iCub::iDyn::iGenericFrame::getFT
yarp::sig::Vector getFT()
Definition: iDynTransform.h:210
iCub::iDyn::iFTransformation::attach
void attach(iKin::iKinChain *_Limb)
Definition: iDynTransform.cpp:265
iDynInv.h
iCub::iDyn::iFTransformation::setTeb
void setTeb()
Definition: iDynTransform.cpp:345
iCub::iKin::iKinChain
Definition: iKinFwd.h:354
iDyn.h
iCub::iDyn::iFTransformation
Definition: iDynTransform.h:298
iCub::iDyn::iGenericFrame::setR
void setR(const yarp::sig::Matrix &_R)
Definition: iDynTransform.cpp:61
iCub::iDyn::iFTransformation::getHs
yarp::sig::Matrix getHs()
Definition: iDynTransform.h:349
iCub::iDyn::iFTransformation::iFTransformation
iFTransformation()
Definition: iDynTransform.cpp:233
iCub::iDyn::iGenericFrame::iGenericFrame
iGenericFrame()
Definition: iDynTransform.cpp:32
p
p
Definition: show_eyes_axes.m:23
math.h
iCub::iDyn::iGenericFrame::getH
yarp::sig::Matrix getH()
Definition: iDynTransform.h:207
iCub::iDyn::iGenericFrame
Definition: iDynTransform.h:121
iCub::iDyn::iFTransformation::~iFTransformation
~iFTransformation()
Definition: iDynTransform.cpp:415
iCub::iDyn::iGenericFrame::setH
void setH(const yarp::sig::Matrix &_R, double _x, double _y, double _z)
iCub::iDyn::iFTransformation::setTse
void setTse()
Definition: iDynTransform.cpp:383
iCub::iDyn::iFTransformation::setFe
void setFe()
Definition: iDynTransform.cpp:379
iCub::iDyn::iGenericFrame::getR
yarp::sig::Matrix getR()
Definition: iDynTransform.h:206
iCub::iDyn::iGenericFrame::setFT
yarp::sig::Vector setFT(const yarp::sig::Vector &_FT)
Definition: iDynTransform.cpp:123
iCub::iDyn::iFTransformation::setSensor
void setSensor(const yarp::sig::Vector &_FT)
iCub::iDyn::iGenericFrame::getP
yarp::sig::Vector getP()
Definition: iDynTransform.h:205
iCub::iDyn::iGenericFrame::setP
void setP(double _x, double _y, double _z)
Definition: iDynTransform.cpp:65
iCub::iDyn::iFTransformation::getHe
yarp::sig::Matrix getHe()
Definition: iDynTransform.h:350
iCub::iDyn::iFTransformation::getEndEffWrenchAsBase
yarp::sig::Vector getEndEffWrenchAsBase()
Definition: iDynTransform.cpp:368
iCub::iDyn::iGenericFrame::setPRH
void setPRH(void)
Definition: iDynTransform.cpp:83
iCub::iDyn::iGenericFrame::~iGenericFrame
~iGenericFrame()
Definition: iDynTransform.h:213
iCub::iDyn::iFTransformation::setHe
void setHe()
Definition: iDynTransform.cpp:330
iCub::iDyn::iFTransformation::getEndEffWrench
yarp::sig::Vector getEndEffWrench()
Definition: iDynTransform.cpp:357
iCub::iDyn::iFTransformation::setLink
void setLink(int _l)
Definition: iDynTransform.cpp:306