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 
iCub::skinDynLib::BODY_PART_UNKNOWN
@ BODY_PART_UNKNOWN
Definition: common.h:45
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
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::iDynContactSolver::contactList
iCub::skinDynLib::dynContactList contactList
list of contacts acting on the link chain
Definition: iDynContact.h:56
e
e
Definition: compute_ekf_fast.m:13
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::skinDynLib::NO_VERBOSE
@ NO_VERBOSE
Definition: common.h:38
iCub::iDyn::NewEulMode
NewEulMode
Definition: iDynInv.h:64
iDyn.h
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
iCub::iDyn::iDynContactSolver::buildA
yarp::sig::Matrix buildA(unsigned int firstContactLink, unsigned int lastContactLink)
Definition: iDynContact.cpp:188
iCub::iDyn::iDynContactSolver::~iDynContactSolver
~iDynContactSolver()
Default destructor.
Definition: iDynContact.cpp:54
iCub::iDyn::iDynContactSolver::addContact
bool addContact(const iCub::skinDynLib::dynContact &contact)
Add a new element to the contact list.
Definition: iDynContact.cpp:56
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::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::DYNAMIC
@ DYNAMIC
Definition: iDynInv.h:64
iCub::iDyn::SensorLinkNewtonEuler
Definition: iDynInv.h:718
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::iDyn::iDynContactSolver::bodyPart
iCub::skinDynLib::BodyPart bodyPart
Definition: iDynContact.h:60
iCub::iDyn::iDynSensor
Definition: iDynInv.h:1577
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::dynContactList
Definition: dynContactList.h:42
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::skinDynLib::dynContact
Definition: dynContact.h:51
iCub::iDyn::iDynContactSolver
Definition: iDynContact.h:52
iCub::iDyn::iDynContactSolver::findContactSubChain
void findContactSubChain(unsigned int &firstLink, unsigned int &lastLink)
Definition: iDynContact.cpp:308
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
dynContactList.h