iCub-main
iDynContact.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2011 RobotCub Consortium
3  * Author: Andrea Del Prete
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
30 #ifndef __IDYNCONT_H__
31 #define __IDYNCONT_H__
32 
33 #include <iCub/iDyn/iDyn.h>
35 
36 
37 namespace iCub
38 {
39 
40 namespace iDyn
41 {
42 
43  // tollerance used to solve the linear system (all singular values < TOLLERANCE are considered zero)
44  static double const TOLLERANCE = 10e-08;
45 
53 {
54 protected:
57  // empty list
59  // body part related to this solver
61 
62  void findContactSubChain(unsigned int &firstLink, unsigned int &lastLink);
63 
64  yarp::sig::Matrix buildA(unsigned int firstContactLink, unsigned int lastContactLink);
65  yarp::sig::Vector buildB(unsigned int firstContactLink, unsigned int lastContactLink);
66 
67  //***************************************************************************************
68  // UTILITY METHODS
69  //***************************************************************************************
70 
74  yarp::sig::Matrix getHFromAtoB(unsigned int a, unsigned int b);
75 
80  yarp::sig::Vector projectContact2Root(const iCub::skinDynLib::dynContact &c);
81 
82 public:
83 
84 
89  iDynContactSolver(iDynChain *_c, const std::string &_info="", const NewEulMode _mode=DYNAMIC,
91 
98  iDynContactSolver(iDynChain *_c, unsigned int sensLink, SensorLinkNewtonEuler *sensor,
100  unsigned int verb=iCub::skinDynLib::NO_VERBOSE);
101 
111  iDynContactSolver(iDynChain *_c, unsigned int sensLink, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC,
112  double _m, const yarp::sig::Matrix &_I, const std::string &_info="", const NewEulMode _mode=DYNAMIC,
114 
119 
125  bool addContact(const iCub::skinDynLib::dynContact &contact);
126 
132  bool addContacts(const iCub::skinDynLib::dynContactList &contacts);
133 
137  void clearContactList();
138 
144  const iCub::skinDynLib::dynContactList& computeExternalContacts(const yarp::sig::Vector &FMsens);
145 
152 
157 
158  //***************************************************************************************
159  // GET METHODS
160  //***************************************************************************************
161 
166 
171  yarp::sig::Vector getForceMomentEndEff() const;
172 
176  unsigned int getUnknownNumber() const;
177 
178 };
179 
180 
181 }
182 
183 }//end namespace
184 
185 #endif
186 
187 
A class for setting a virtual sensor link.
Definition: iDynInv.h:719
A Base class for defining a Serial Link Chain, using dynamics and kinematics.
Definition: iDyn.h:533
This solver uses a modified version of the Newton-Euler algorithm to estimate both the external and i...
Definition: iDynContact.h:53
const iCub::skinDynLib::dynContactList & getContactList() const
bool addContacts(const iCub::skinDynLib::dynContactList &contacts)
Add the specified elements to the contact list.
Definition: iDynContact.cpp:69
~iDynContactSolver()
Default destructor.
Definition: iDynContact.cpp:54
yarp::sig::Vector getForceMomentEndEff() const
Returns the end effector force-moment as a single (6x1) vector.
iDynContactSolver(iDynChain *_c, unsigned int sensLink, const yarp::sig::Matrix &_H, const yarp::sig::Matrix &_HC, double _m, const yarp::sig::Matrix &_I, 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 with F/T sensor information.
const iCub::skinDynLib::dynContactList nullList
Definition: iDynContact.h:58
unsigned int getUnknownNumber() const
yarp::sig::Vector projectContact2Root(const iCub::skinDynLib::dynContact &c)
Compute the wrench of the specified contact expressed w.r.t.
yarp::sig::Vector buildB(unsigned int firstContactLink, unsigned int lastContactLink)
iCub::skinDynLib::dynContactList contactList
list of contacts acting on the link chain
Definition: iDynContact.h:56
yarp::sig::Matrix buildA(unsigned int firstContactLink, unsigned int lastContactLink)
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
void clearContactList()
Clear the contact list.
Definition: iDynContact.cpp:78
void computeWrenchFromSensorNewtonEuler()
Compute an estimate of the external and internal contact wrenches (joint torques included).
const iCub::skinDynLib::dynContactList & computeExternalContacts(const yarp::sig::Vector &FMsens)
Compute an estimate of the external contact wrenches.
iCub::skinDynLib::BodyPart bodyPart
Definition: iDynContact.h:60
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
void findContactSubChain(unsigned int &firstLink, unsigned int &lastLink)
bool addContact(const iCub::skinDynLib::dynContact &contact)
Add a new element to the contact list.
Definition: iDynContact.cpp:56
yarp::sig::Matrix getHFromAtoB(unsigned int a, unsigned int b)
Compute the rototraslation matrix from frame a to frame b.
A class for computing forces and torques in a iDynChain, when a force/torque sensor is placed in the ...
Definition: iDynInv.h:1578
Class representing a list of external contacts.
Class representing an external contact acting on a link of the robot body.
Definition: dynContact.h:52
static double const TOLLERANCE
Definition: iDynContact.h:44
@ DYNAMIC
Definition: iDynInv.h:64
@ BODY_PART_UNKNOWN
Definition: common.h:45
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.