iCub-main
iDynContact.cpp
Go to the documentation of this file.
1 /*
2 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3 * Author: Andrea Del Prete
4 * email: andrea.delprete@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 <iCub/iDyn/iDynContact.h>
21 #include <yarp/math/SVD.h>
22 #include <stdio.h>
23 
24 using namespace std;
25 using namespace yarp::sig;
26 using namespace yarp::math;
27 using namespace iCub::iKin;
28 using namespace iCub::iDyn;
29 using namespace iCub::ctrl;
30 using namespace iCub::skinDynLib;
31 
32 
33 
34 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
35 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
36 // IDYN CONTACT SOLVER
37 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
38 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
39 iDynContactSolver::iDynContactSolver(iDynChain *_c, const string &_info, const NewEulMode _mode, BodyPart _bodyPart, unsigned int verb)
40 :iDynSensor(_c, _info, _mode, verb), bodyPart(_bodyPart){}
41 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
43  const string &_info, const NewEulMode _mode, BodyPart _bodyPart, unsigned int verb)
44 :iDynSensor(_c, _info, _mode, verb), bodyPart(_bodyPart)
45 {
46  lSens = sensLink;
47  sens = sensor;
48 }
49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
50 iDynContactSolver::iDynContactSolver(iDynChain *_c, unsigned int sensLink, const Matrix &_H, const Matrix &_HC, double _m,
51  const Matrix &_I, const string &_info, const NewEulMode _mode, BodyPart _bodyPart, unsigned int verb)
52 :iDynSensor(_c, sensLink, _H, _HC, _m, _I, _info, _mode, verb), bodyPart(_bodyPart){}
53 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
55 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
57 {
58  // check that the contact body part is the same of the sensor
59  if(contact.getBodyPart()!=bodyPart)
60  {
61  if(verbose)
62  printf("Error: trying to add a %s contact to a %s sensor!\n", contact.getBodyPartName().c_str(), BodyPart_s[bodyPart].c_str());
63  return false;
64  }
65  contactList.push_back(contact);
66  return true;
67 }
68 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
70 {
71  //contactList.insert(contactList.end(), contacts.begin(), contacts.end());
72  bool res=true;
73  for(dynContactList::const_iterator it=contacts.begin();it!=contacts.end();it++)
74  res &= addContact(*it);
75  return res;
76 }
77 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
79 {
80  if(!contactList.empty())
81  contactList.clear();
82 }
83 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
85 {
86  if(!setSensorMeasures(FMsens)) // set the sensor measure
87  return nullList;
88  return computeExternalContacts();
89 }
90 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
92 {
93  if(contactList.size()==0)
94  {
95  // if there are no contacts assume a contact at the end effector
96  dynContact c(bodyPart, chain->getN()-1, zeros(3));
97  contactList.push_back(c);
98  }
99 
100  // initialize the dynamics at the end effector and the kinematics at the base
101  if(chain->NE == NULL)
102  {
104  chain->initNewtonEuler(); // here I assume the chain base is still
105  }
106 
107  // KINEMATIC PHASE
108  chain->computeKinematicNewtonEuler(); // compute kinematics of all the links
109  sens->ForwardAttachToLink(chain->refLink(lSens)); // compute kinematics of the sensor sub-link
110 
111  // FIND THE BOUND OF THE SUBCHAIN WHERE EXT CONTACTS ARE APPLIED
112  unsigned int firstContactLink, lastContactLink;
113  findContactSubChain(firstContactLink, lastContactLink);
114  /*if(verbose)
115  fprintf(stdout, "Estimating %d contacts between link %d and %d\n",
116  contactList.size(), firstContactLink, lastContactLink);*/
117  if(firstContactLink<lSens)
118  {
119  if(verbose)
120  fprintf(stderr, "Contacts before the sensor link. Impossible to compute the external contacts!\n");
121  return nullList;
122  }
123 
124  // PROPAGATE WRENCH FORWARD, UP TO THE FIRST LINK A CONTACT IS APPLIED TO
125  sens->ForwardForcesMomentsToLink(chain->refLink(lSens)); // propagate wrench from sensor to hosting link
126  if(firstContactLink==lSens)
127  {
128  // if there're contacts on the sensor link we need to propagate the wrench to the previous link
129  // (here we are assuming that the contacts are detected by the F/T sensor, no matter their application point)
131  }
132  else if(firstContactLink>lSens+1)
133  chain->NE->ForwardWrenchFromAtoB(lSens+1, firstContactLink-1);
134 
135  // PROPAGATE WRENCH BACKWARD, FROM THE E.E. UP TO THE LAST LINK A CONTACT IS APPLIED TO
136  chain->NE->initWrenchEnd(zeros(3), zeros(3));
137  chain->NE->BackwardWrenchFromAtoB(chain->getN()-1, lastContactLink);
138 
139  // BUILD AND SOLVE THE LINEAR SYSTEM AX=B RELATIVE TO THE CONTACT SUB-CHAIN
140  // the reference frame is the <firstContactLink-1>
141  Matrix A = buildA(firstContactLink, lastContactLink);
142  Vector B = buildB(firstContactLink, lastContactLink);
143  Matrix pinv_A = pinv(A, TOLLERANCE);
144  Vector X = pinv_A * B;
145 
146  // SET THE COMPUTED VALUES IN THE CONTACT LIST
147  unsigned int unknownInd = 0;
148  Matrix H, R;
149  for(dynContactList::iterator it = contactList.begin(); it!=contactList.end(); it++)
150  {
151  if(it->isForceDirectionKnown())
152  it->setForceModule( X(unknownInd++));
153  else
154  {
155  H = getHFromAtoB(it->getLinkNumber(), firstContactLink-1);
156  R = H.submatrix(0,2,0,2);
157  it->setForce( R * X.subVector(unknownInd, unknownInd+2));
158  unknownInd += 3;
159  if(!it->isMomentKnown())
160  {
161  it->setMoment(R * X.subVector(unknownInd, unknownInd+2));
162  unknownInd += 3;
163  }
164  }
165  }
166 
167  //propagate the wrench backward from the sensor
168  if(firstContactLink==lSens) // if there is a contact on the sensor link you have to propagate the wrench of the previous link
170  else
172 
173  // RETURN A CONST REFERENCE TO THE CONTACT LIST
174  return contactList;
175 }
176 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
178 {
179  // in the external contact computations also the internal wrenches
180  // are computed, from the base to the end effector
181  // (except for the contact subchain, but now we suppose only 1 contact at a time)
183 
184  // now we can compute all torques
185  chain->NE->computeTorques();
186 }
187 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
188 Matrix iDynContactSolver::buildA(unsigned int firstContactLink, unsigned int lastContactLink)
189 {
190  unsigned int unknownNum = getUnknownNumber();
191  Matrix A(6, unknownNum);
192 
193  // For each contact add some columns to A:
194  // * wrench: add 6 columns, 3 composed by I_3 above and the S(r_E) below, 3 composed by 0_3 above and I_3 below
195  // * pure force: add 3 columns composed by I_3 above and the S(r_E) below
196  // * force module: add 1 column composed by the force direction unit vector above and the cross product between
197  // the contact point and the force direction unit vector below
198  unsigned int colInd = 0;
199  Matrix H, R;
200  Matrix eye3x3 = eye(3,3);
201  Matrix zero3x3 = zeros(3,3);
202  Vector r, temp1, temp2;
203  dynContactList::const_iterator it = contactList.begin();
204 
205  for(; it!=contactList.end(); it++)
206  {
207  // compute the rototranslation matrix from <firstContactLink-1> to the current link
208  H = getHFromAtoB(firstContactLink-1, it->getLinkNumber());
209  R = H.submatrix(0,2,0,2);
210  r = H.subcol(0,3,3);
211 
212  if(it->isForceDirectionKnown())
213  { // 1 UNKNOWN: FORCE MODULE
214  temp1 = R*it->getForceDirection(); // force direction unit vector
215  temp2 = R*it->getCoP();
216  temp2 += r;
217  A.setSubcol(temp1, 0, colInd);
218  A.setSubcol(cross(temp2, temp1), 3, colInd++);
219  }
220  else
221  { // 3 UNKNOWNS: FORCE
222  temp1 = R*it->getCoP();
223  temp1 += r;
224  A.setSubmatrix(eye3x3, 0, colInd);
225  A.setSubmatrix(crossProductMatrix(temp1), 3, colInd);
226  colInd += 3;
227 
228  if(!it->isMomentKnown())
229  { // 6 UNKNOWNS: FORCE AND MOMENT
230  A.setSubmatrix(zero3x3, 0, colInd);
231  A.setSubmatrix(eye3x3, 3, colInd);
232  colInd += 3;
233  }
234  }
235  }
236  return A;
237 }
238 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
239 Vector iDynContactSolver::buildB(unsigned int firstContactLink, unsigned int lastContactLink)
240 {
241  Matrix H, R;
242  Vector r;
243 
244  // Initialize the force part of the B vector (first 3 components) as:
245  // * minus the force applied on the first link
246  // * plus the force exchanged by the last link on the next one
247  Matrix Hlast = getHFromAtoB(firstContactLink-1, lastContactLink);
248  Matrix Rlast = Hlast.submatrix(0,2,0,2);
249  Vector rLast = Hlast.subcol(0,3,3);
250  //Vector rLast = Hlast.submatrix(0,2,3,3).getCol(0);
251  Vector Bforce = Rlast * chain->getForce(lastContactLink);
252  Bforce -= chain->getForce(firstContactLink-1);
253 
254  // For each link add the mass multiplied by the linear accelleration of the COM
255  for(unsigned int i=firstContactLink; i<=lastContactLink; i++)
256  {
257  H = getHFromAtoB(firstContactLink-1, i);
258  R = H.submatrix(0,2,0,2);
259  Bforce += chain->getMass(i) * R * chain->getLinAccCOM(i);
260  }
261 
262  // initialize the moment part (computed w.r.t. the begin of the first link) of the B vector (last 3 components) as:
263  // * the moment exerted by the last link on the next one
264  // * minus the moment applied on the first link
265  // * the displacement between the beginning of the first link and the end of the last link,
266  // vector product the force exerted by the last link on the next one
267  Vector Bmoment = Rlast * chain->getMoment(lastContactLink);
268  Bmoment -= chain->getMoment(firstContactLink-1);
269  Bmoment += cross(rLast, Rlast* chain->getForce(lastContactLink));
270 
271  // Then for each link add:
272  // * the gravitational contribution
273  // * the inertia contribution
274  // * the Corioĺ contribution
275  iDynLink* link;
276  for(unsigned int i=firstContactLink; i<=lastContactLink; i++)
277  {
278  link = chain->refLink(i);
279 
280  H = getHFromAtoB(firstContactLink-1, i);
281  H *= link->getCOM();
282  R = H.submatrix(0,2,0,2);
283  r = H.subcol(0,3,3); // vector from <firstContactLink-1> to COM of i
284 
285  Bmoment += link->getMass() * cross(r, R * link->getLinAccC());
286  Bmoment += R * (link->getInertia() * link->getdW() +
287  cross(link->getW(), link->getInertia()*link->getW()));
288  }
289 
290  return cat(Bforce, Bmoment);
291 }
292 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
294 {
295  return contactList;
296 }
297 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
299 {
300  // returns the end effector wrench, if any, otherwise return zeros
301  int eeInd = chain->getN()-1;
302  for(dynContactList::const_iterator it=contactList.begin(); it!=contactList.end();it++)
303  if(it->getLinkNumber() == eeInd)
304  return it->getForceMoment();
305  return zeros(6);
306 }
307 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
308 void iDynContactSolver::findContactSubChain(unsigned int &firstLink, unsigned int &lastLink)
309 {
310  dynContactList::const_iterator it=contactList.begin();
311  firstLink = chain->getN()+1;
312  lastLink = 0;
313 
314  for(; it!=contactList.end(); it++)
315  {
316  unsigned int l = it->getLinkNumber();
317  if(l>lastLink)
318  lastLink = l;
319  if(l<firstLink)
320  firstLink = l;
321  }
322 }
323 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
324 Matrix iDynContactSolver::getHFromAtoB(unsigned int a, unsigned int b)
325 {
326  if(a==b)
327  return eye(4,4);
328 
329  if(a>b)
330  {
331  Matrix Hinv = getHFromAtoB(b, a);
332  return SE3inv(Hinv);
333  }
334 
335  Matrix H = chain->refLink(a+1)->getH(); // initialize H with the rototranslation from <a> to <a+1>
336  for(unsigned int i=a+2; i<=b; i++)
337  H *= chain->refLink(i)->getH();
338  return H;
339 }
340 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
342 {
343  Vector wrench = c.getForceMoment();
344  Matrix H_02L = getHFromAtoB(0, c.getLinkNumber()); // rototraslation from 0 to contact link
345  Matrix H_r20 = chain->getH0(); // rototraslation from root to 0
346  Matrix R_r2L = H_r20.submatrix(0,2,0,2) * H_02L.submatrix(0,2,0,2); // rotation from root to contact link
347  return R_r2L*wrench; // project the wrench from the link frame to the root frame
348 }
349 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
351 {
352  dynContactList::const_iterator it=contactList.begin();
353  unsigned int unknowns=0;
354 
355  for(; it!=contactList.end(); it++)
356  {
357  if(it->isMomentKnown())
358  {
359  if(it->isForceDirectionKnown())
360  unknowns++; // 1 unknown (force module)
361  else
362  unknowns+=3; // 3 unknowns (force)
363  }
364  else
365  unknowns+=6; // 6 unknowns (force and moment)
366  }
367  return unknowns;
368 }
iCub::iDyn::iDynChain::NE
OneChainNewtonEuler * NE
pointer to OneChainNewtonEuler class, to be used for computing forces and torques
Definition: iDyn.h:554
iCub::iDyn::iDynChain::initNewtonEuler
bool initNewtonEuler(const yarp::sig::Vector &w0, const yarp::sig::Vector &dw0, const yarp::sig::Vector &ddp0, const yarp::sig::Vector &Fend, const yarp::sig::Vector &Muend)
Initialize the Newton-Euler method by setting the base (virtual link) velocity and accelerations ( w0...
iCub::skinDynLib
Definition: common.h:35
H
H
Definition: compute_ekf_fast.m:27
iCub::iDyn::iDynContactSolver::getForceMomentEndEff
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
Definition: iDynContact.cpp:298
iCub::iDyn::iDynContactSolver::clearContactList
void clearContactList()
Clear the contact list.
Definition: iDynContact.cpp:78
iCub::iDyn::iDynChain
Definition: iDyn.h:532
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchFromAtoB
bool BackwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, backward of forces and mome...
Definition: iDynInv.cpp:1995
iCub::iDyn::iDynChain::computeKinematicNewtonEuler
void computeKinematicNewtonEuler()
Calls the proper method to compute kinematics variables: either ForwardKinematicFromBase() or Backwar...
Definition: iDyn.cpp:992
iCub::iDyn::iDynContactSolver::contactList
iCub::skinDynLib::dynContactList contactList
list of contacts acting on the link chain
Definition: iDynContact.h:56
iCub::iDyn::OneChainNewtonEuler::initWrenchEnd
bool initWrenchEnd(const yarp::sig::Vector &F0, const yarp::sig::Vector &Mu0)
[classic] Initialize the end-effector finalLink with measured or known wrench
Definition: iDynInv.cpp:1828
iCub::iDyn::iDynContactSolver::computeExternalContacts
const iCub::skinDynLib::dynContactList & computeExternalContacts()
Compute an estimate of the external contact wrenches (assuming the F/T sensor measure have already be...
Definition: iDynContact.cpp:91
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iCub::iDyn::iDynChain::getMass
double getMass(const unsigned int i) const
Returns the i-th link mass.
Definition: iDyn.cpp:822
iCub::iDyn::iDynContactSolver::getUnknownNumber
unsigned int getUnknownNumber() const
Definition: iDynContact.cpp:350
iCub::iDyn::iDynContactSolver::getHFromAtoB
yarp::sig::Matrix getHFromAtoB(unsigned int a, unsigned int b)
Compute the rototraslation matrix from frame a to frame b.
Definition: iDynContact.cpp:324
iDynContact.h
iCub::iDyn::iDynContactSolver::buildA
yarp::sig::Matrix buildA(unsigned int firstContactLink, unsigned int lastContactLink)
Definition: iDynContact.cpp:188
iCub::skinDynLib::dynContact::getBodyPart
virtual BodyPart getBodyPart() const
Get the body part of the contact.
Definition: dynContact.cpp:86
iCub::iDyn::iDynContactSolver::~iDynContactSolver
~iDynContactSolver()
Default destructor.
Definition: iDynContact.cpp:54
iCub::iDyn::OneChainNewtonEuler::BackwardWrenchToBase
bool BackwardWrenchToBase(unsigned int lSens)
[classic/inverse] Base function for inverse Newton-Euler: from the i-th link to the base,...
Definition: iDynInv.cpp:1947
iCub::iDyn::iDynInvSensor::sens
SensorLinkNewtonEuler * sens
the sensor
Definition: iDynInv.h:1230
iCub::iDyn::iDynChain::prepareNewtonEuler
void prepareNewtonEuler(const NewEulMode NewEulMode_s=DYNAMIC)
Prepare for the Newton-Euler recursive computation of forces and torques.
Definition: iDyn.cpp:916
iCub::iDyn::iDynContactSolver::addContact
bool addContact(const iCub::skinDynLib::dynContact &contact)
Add a new element to the contact list.
Definition: iDynContact.cpp:56
iCub::skinDynLib::dynContact::getForceMoment
virtual yarp::sig::Vector getForceMoment() const
Get the contact force and moment in a single (6x1) vector.
Definition: dynContact.cpp:72
iCub::iDyn::iDynContactSolver::projectContact2Root
yarp::sig::Vector projectContact2Root(const iCub::skinDynLib::dynContact &c)
Compute the wrench of the specified contact expressed w.r.t.
Definition: iDynContact.cpp:341
iCub::iDyn::TOLLERANCE
static const double TOLLERANCE
Definition: iDynContact.h:44
iCub::iDyn::iDynInvSensor::verbose
unsigned int verbose
verbosity flag
Definition: iDynInv.h:1236
iCub::iDyn::iDynContactSolver::buildB
yarp::sig::Vector buildB(unsigned int firstContactLink, unsigned int lastContactLink)
Definition: iDynContact.cpp:239
iCub::iDyn::iDynContactSolver::addContacts
bool addContacts(const iCub::skinDynLib::dynContactList &contacts)
Add the specified elements to the contact list.
Definition: iDynContact.cpp:69
iCub::iDyn
Definition: iDyn.h:81
iCub::iKin
Definition: iKinFwd.h:71
iCub::iDyn::OneChainNewtonEuler::ForwardWrenchFromAtoB
bool ForwardWrenchFromAtoB(unsigned int lA, unsigned int lB)
Base function for inverse Newton-Euler: from the lA-th link to the lB-th, forward of forces and momen...
Definition: iDynInv.cpp:1976
iCub::iDyn::SensorLinkNewtonEuler
Definition: iDynInv.h:718
iCub::iDyn::iDynChain::refLink
iDynLink * refLink(const unsigned int i)
Returns a pointer to the ith iDynLink.
Definition: iDyn.cpp:755
iCub::iDyn::iDynContactSolver::computeWrenchFromSensorNewtonEuler
void computeWrenchFromSensorNewtonEuler()
Compute an estimate of the external and internal contact wrenches (joint torques included).
Definition: iDynContact.cpp:177
iCub::iDyn::iDynContactSolver::iDynContactSolver
iDynContactSolver(iDynChain *_c, const std::string &_info="", const NewEulMode _mode=DYNAMIC, iCub::skinDynLib::BodyPart bodyPart=iCub::skinDynLib::BODY_PART_UNKNOWN, unsigned int verb=iCub::skinDynLib::NO_VERBOSE)
Constructor.
Definition: iDynContact.cpp:39
iCub::iKin::iKinChain::getN
unsigned int getN() const
Returns the number of Links belonging to the Chain.
Definition: iKinFwd.h:550
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::iDyn::iDynContactSolver::bodyPart
iCub::skinDynLib::BodyPart bodyPart
Definition: iDynContact.h:60
iCub::iDyn::iDynChain::getMoment
const yarp::sig::Vector & getMoment(const unsigned int iLink) const
Returns the i-th link moment.
Definition: iDyn.cpp:777
iCub::iKin::iKinChain::getH0
yarp::sig::Matrix getH0() const
Returns H0, the rigid roto-translation matrix from the root reference frame to the 0th frame.
Definition: iKinFwd.h:564
iCub::iDyn::iDynInvSensor::mode
NewEulMode mode
static/dynamic/etc..
Definition: iDynInv.h:1234
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
iCub::iDyn::iDynInvSensor::chain
iDynChain * chain
the iDynChain describing the robotic chain
Definition: iDynInv.h:1232
iCub::iDyn::OneChainNewtonEuler::computeTorques
void computeTorques()
Computes all the torques in the chain, assuming that all the internal wrenches have been already comp...
Definition: iDynInv.cpp:1892
iCub::skinDynLib::dynContact::getLinkNumber
virtual unsigned int getLinkNumber() const
Get the link number (where 0 is the first link of the chain).
Definition: dynContact.cpp:84
iCub::ctrl::cross
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
iCub::skinDynLib::dynContact::getBodyPartName
virtual std::string getBodyPartName() const
Get the name of the contact body part.
Definition: dynContact.cpp:88
A
A
Definition: sine.m:16
iCub::skinDynLib::dynContactList
Definition: dynContactList.h:42
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::iDyn::SensorLinkNewtonEuler::ForwardAttachToLink
void ForwardAttachToLink(iDynLink *link)
Compute w,dw,ddp,dppC given the reference frame of the link where the sensor is.
Definition: iDynInv.cpp:1326
iCub::iDyn::iDynChain::getForce
const yarp::sig::Vector & getForce(const unsigned int iLink) const
Returns the i-th link force.
Definition: iDyn.cpp:766
iCub::skinDynLib::dynContact
Definition: dynContact.h:51
iCub::iDyn::iDynContactSolver::findContactSubChain
void findContactSubChain(unsigned int &firstLink, unsigned int &lastLink)
Definition: iDynContact.cpp:308
iCub::skinDynLib::BodyPart_s
const std::string BodyPart_s[]
Definition: common.h:49
iCub::iDyn::iDynChain::getLinAccCOM
const yarp::sig::Vector & getLinAccCOM(const unsigned int i) const
Returns the i-th link linear acceleration of the COM.
Definition: iDyn.cpp:722
fprintf
fprintf(fid,'\n')
zeros
zeros(2, 2) eye(2
iCub::iDyn::iDynInvSensor::lSens
unsigned int lSens
the link where the sensor is attached to
Definition: iDynInv.h:1228
iCub::iDyn::SensorLinkNewtonEuler::ForwardForcesMomentsToLink
void ForwardForcesMomentsToLink(iDynLink *link)
Forward the sensor forces and moments, measured by the sensor, to the reference frame of the link whe...
Definition: iDynInv.cpp:1340
iCub::iDyn::iDynContactSolver::nullList
const iCub::skinDynLib::dynContactList nullList
Definition: iDynContact.h:58
iCub::iDyn::iDynContactSolver::getContactList
const iCub::skinDynLib::dynContactList & getContactList() const
Definition: iDynContact.cpp:293
iCub::iDyn::iDynSensor::setSensorMeasures
bool setSensorMeasures(const yarp::sig::Vector &F, const yarp::sig::Vector &Mu)
Set the sensor measured force and moment.