Classes | Public Types | Public Member Functions | Static Public Member Functions | Public Attributes | Private Types | Private Member Functions | Private Attributes | Friends | List of all members
hrp::Body Class Reference

#include <Body.h>

Inheritance diagram for hrp::Body:
Inheritance graph
[legend]

Classes

struct  ExtraJoint
 

Public Types

enum  ExtraJointType { EJ_XYZ, EJ_XY, EJ_Z }
 

Public Member Functions

void addSensor (Sensor *sensor, int sensorType, int id)
 
 Body ()
 
 Body (const Body &org)
 
void calcAngularMomentumJacobian (Link *base, dmatrix &H)
 compute Angular Momentum Jacobian around CoM of base More...
 
Vector3 calcCM ()
 
void calcCMJacobian (Link *base, dmatrix &J)
 compute CoM Jacobian More...
 
void calcForwardKinematics (bool calcVelocity=false, bool calcAcceleration=false)
 
void calcInverseDynamics (Link *link, Vector3 &out_f, Vector3 &out_tau)
 
void calcMassMatrix (dmatrix &out_M)
 
double calcTotalMass ()
 
void calcTotalMomentum (Vector3 &out_P, Vector3 &out_L)
 
void calcTotalMomentumFromJacobian (Vector3 &out_P, Vector3 &out_L)
 
void clearExternalForces ()
 
void clearSensorValues ()
 
LightcreateLight (Link *link, int lightType, const std::string &name)
 
SensorcreateSensor (Link *link, int sensorType, int id, const std::string &name)
 
void getDefaultRootPosition (Vector3 &out_p, Matrix33 &out_R)
 
JointPathPtr getJointPath (Link *baseLink, Link *targetLink)
 
void initializeConfiguration ()
 
bool installCustomizer ()
 
bool installCustomizer (BodyCustomizerInterface *customizerInterface)
 
bool isStaticModel ()
 
Linkjoint (int id) const
 
const std::vector< Link * > & joints () const
 
Lightlight (const std::string &name)
 
Linklink (int index) const
 
Linklink (const std::string &name) const
 
const LinkTraverselinks () const
 
const LinkTraverselinkTraverse () const
 
const std::string & modelName ()
 
const std::string & name ()
 
unsigned int numJoints () const
 
unsigned int numLinks () const
 
unsigned int numSensors (int sensorType) const
 
unsigned int numSensorTypes () const
 
void putInformation (std::ostream &out)
 
LinkrootLink () const
 
Sensorsensor (int sensorType, int sensorId) const
 
template<class TSensor >
TSensor * sensor (int id) const
 
template<class TSensor >
TSensor * sensor (const std::string &name) const
 
void setColumnOfMassMatrix (dmatrix &M, int column)
 
void setDefaultRootPosition (const Vector3 &p, const Matrix33 &R)
 
void setModelName (const std::string &name)
 
void setName (const std::string &name)
 
void setRootLink (Link *link)
 
void setVirtualJointForces ()
 
double totalMass () const
 
void updateLinkColdetModelPositions ()
 
void updateLinkTree ()
 
virtual ~Body ()
 
- Public Member Functions inherited from hrp::Referenced
 Referenced ()
 
virtual ~Referenced ()
 

Static Public Member Functions

static BodyInterfacebodyInterface ()
 

Public Attributes

std::vector< ExtraJointextraJoints
 

Private Types

typedef std::vector< Link * > LinkArray
 
typedef std::map< std::string, Light * > NameToLightMap
 
typedef std::map< std::string, Link * > NameToLinkMap
 
typedef std::map< std::string, Sensor * > NameToSensorMap
 
typedef std::vector< Sensor * > SensorArray
 

Private Member Functions

LinkcreateEmptyJoint (int jointId)
 
void initialize ()
 
void setVirtualJointForcesSub ()
 

Private Attributes

std::vector< SensorArrayallSensors
 
BodyHandle bodyHandle
 
BodyHandleEntity bodyHandleEntity
 
BodyCustomizerHandle customizerHandle
 
BodyCustomizerInterfacecustomizerInterface
 
Matrix33 defaultRootAttitude
 
Vector3 defaultRootPosition
 
bool isStaticModel_
 
LinkArray jointIdToLinkArray
 
LinkTraverse linkTraverse_
 
std::string modelName_
 
std::string name_
 
NameToLightMap nameToLightMap
 
NameToLinkMap nameToLinkMap
 
NameToSensorMap nameToSensorMap
 
LinkrootLink_
 
double totalMass_
 

Friends

class CustomizedJointPath
 

Additional Inherited Members

- Protected Member Functions inherited from hrp::Referenced
int refCounter ()
 

Detailed Description

Definition at line 44 of file Body.h.

Member Typedef Documentation

typedef std::vector<Link*> hrp::Body::LinkArray
private

Definition at line 280 of file Body.h.

typedef std::map<std::string, Light*> hrp::Body::NameToLightMap
private

Definition at line 294 of file Body.h.

typedef std::map<std::string, Link*> hrp::Body::NameToLinkMap
private

Definition at line 285 of file Body.h.

typedef std::map<std::string, Sensor*> hrp::Body::NameToSensorMap
private

Definition at line 292 of file Body.h.

typedef std::vector<Sensor*> hrp::Body::SensorArray
private

Definition at line 289 of file Body.h.

Member Enumeration Documentation

Enumerator
EJ_XYZ 
EJ_XY 
EJ_Z 

Definition at line 246 of file Body.h.

Constructor & Destructor Documentation

Body::Body ( )

Definition at line 85 of file Body.cpp.

Body::Body ( const Body org)

Definition at line 98 of file Body.cpp.

Body::~Body ( )
virtual

Definition at line 53 of file Body.cpp.

Member Function Documentation

void Body::addSensor ( Sensor sensor,
int  sensorType,
int  id 
)

Definition at line 600 of file Body.cpp.

BodyInterface * Body::bodyInterface ( )
static

Definition at line 766 of file Body.cpp.

void Body::calcAngularMomentumJacobian ( Link base,
dmatrix H 
)

compute Angular Momentum Jacobian around CoM of base

Parameters
baselink fixed to the environment
HAngular Momentum Jacobian
Note
Link::wc must be computed by calcCM() before calling

Definition at line 954 of file Body.cpp.

Vector3 Body::calcCM ( )

Definition at line 310 of file Body.cpp.

void Body::calcCMJacobian ( Link base,
dmatrix J 
)

compute CoM Jacobian

Parameters
baselink fixed to the environment
JCoM Jacobian
Note
Link::wc must be computed by calcCM() before calling

Definition at line 873 of file Body.cpp.

void Body::calcForwardKinematics ( bool  calcVelocity = false,
bool  calcAcceleration = false 
)

Definition at line 554 of file Body.cpp.

void Body::calcInverseDynamics ( Link link,
Vector3 out_f,
Vector3 out_tau 
)

Definition at line 439 of file Body.cpp.

void Body::calcMassMatrix ( dmatrix out_M)

calculate the mass matrix using the unit vector method

Todo:
replace the unit vector method here with a more efficient method that only requires O(n) computation time

The motion equation (dv != dvo) | | | dv | | | | fext | | out_M | * | dw | + | b1 | = | tauext | | | |ddq | | | | u |

Definition at line 338 of file Body.cpp.

double Body::calcTotalMass ( )

Definition at line 297 of file Body.cpp.

void Body::calcTotalMomentum ( Vector3 out_P,
Vector3 out_L 
)

assuming Link::v,w is already computed by calcForwardKinematics(true); assuming Link::wc is already computed by calcCM();

Definition at line 503 of file Body.cpp.

void Body::calcTotalMomentumFromJacobian ( Vector3 out_P,
Vector3 out_L 
)

Definition at line 530 of file Body.cpp.

void Body::clearExternalForces ( )

Definition at line 649 of file Body.cpp.

void Body::clearSensorValues ( )

Definition at line 620 of file Body.cpp.

Link * Body::createEmptyJoint ( int  jointId)
private

Definition at line 171 of file Body.cpp.

Light * Body::createLight ( Link link,
int  lightType,
const std::string &  name 
)

Definition at line 560 of file Body.cpp.

Sensor * Body::createSensor ( Link link,
int  sensorType,
int  id,
const std::string &  name 
)

Definition at line 567 of file Body.cpp.

void Body::getDefaultRootPosition ( Vector3 out_p,
Matrix33 out_R 
)

Definition at line 164 of file Body.cpp.

JointPathPtr Body::getJointPath ( Link baseLink,
Link targetLink 
)

Definition at line 631 of file Body.cpp.

void Body::initialize ( )
private

Definition at line 74 of file Body.cpp.

void Body::initializeConfiguration ( )

Definition at line 267 of file Body.cpp.

bool Body::installCustomizer ( )

The function installs the pre-loaded customizer corresponding to the model name.

Definition at line 687 of file Body.cpp.

bool Body::installCustomizer ( BodyCustomizerInterface customizerInterface)

Definition at line 700 of file Body.cpp.

bool hrp::Body::isStaticModel ( )
inline

This function returns true when the whole body is a static, fixed object like a floor.

Definition at line 188 of file Body.h.

Link* hrp::Body::joint ( int  id) const
inline

This function returns a link that has a given joint ID. If there is no link that has a given joint ID, the function returns a dummy link object whose ID is minus one. The maximum id can be obtained by numJoints().

Definition at line 97 of file Body.h.

const std::vector<Link*>& hrp::Body::joints ( ) const
inline

The vector<Link*> corresponding to the sequence of joint().

Definition at line 104 of file Body.h.

Light* hrp::Body::light ( const std::string &  name)
inline

Definition at line 149 of file Body.h.

Link* hrp::Body::link ( int  index) const
inline

This function returns the link of a given index in the whole link sequence. The order of the sequence corresponds to a link-tree traverse from the root link. The size of the sequence can be obtained by numLinks().

Definition at line 121 of file Body.h.

Link * Body::link ( const std::string &  name) const

This function returns a link that has a given name.

This function returns a link object whose name of Joint node matches a given name. Null is returned when the body has no joint of the given name.

Definition at line 260 of file Body.cpp.

const LinkTraverse& hrp::Body::links ( ) const
inline

Definition at line 125 of file Body.h.

const LinkTraverse& hrp::Body::linkTraverse ( ) const
inline

LinkTraverse object that traverses all the links from the root link

Definition at line 132 of file Body.h.

const std::string& hrp::Body::modelName ( )
inline

Definition at line 64 of file Body.h.

const std::string& hrp::Body::name ( )
inline

Definition at line 56 of file Body.h.

unsigned int hrp::Body::numJoints ( ) const
inline

The number of the links that work as a joint. Note that the acutal value is the maximum joint ID plus one. Thus there may be a case where the value does not correspond to the actual number of the joint-links. In other words, the value represents the size of the link sequence obtained by joint() function.

Definition at line 87 of file Body.h.

unsigned int hrp::Body::numLinks ( ) const
inline

The number of all the links the body has. The value corresponds to the size of the sequence obtained by link() function.

Definition at line 112 of file Body.h.

unsigned int hrp::Body::numSensors ( int  sensorType) const
inline

Definition at line 162 of file Body.h.

unsigned int hrp::Body::numSensorTypes ( ) const
inline

Definition at line 166 of file Body.h.

void Body::putInformation ( std::ostream &  out)

Definition at line 672 of file Body.cpp.

Link* hrp::Body::rootLink ( ) const
inline

The root link of the body

Definition at line 144 of file Body.h.

Sensor* hrp::Body::sensor ( int  sensorType,
int  sensorId 
) const
inline

Definition at line 158 of file Body.h.

template<class TSensor >
TSensor* hrp::Body::sensor ( int  id) const
inline

Definition at line 172 of file Body.h.

template<class TSensor >
TSensor* hrp::Body::sensor ( const std::string &  name) const
inline

Definition at line 176 of file Body.h.

void Body::setColumnOfMassMatrix ( dmatrix M,
int  column 
)

Definition at line 416 of file Body.cpp.

void Body::setDefaultRootPosition ( const Vector3 p,
const Matrix33 R 
)

Definition at line 157 of file Body.cpp.

void hrp::Body::setModelName ( const std::string &  name)
inline

Definition at line 68 of file Body.h.

void hrp::Body::setName ( const std::string &  name)
inline

Definition at line 60 of file Body.h.

void Body::setRootLink ( Link link)

Definition at line 146 of file Body.cpp.

void hrp::Body::setVirtualJointForces ( )
inline

Definition at line 228 of file Body.h.

void Body::setVirtualJointForcesSub ( )
private

Definition at line 641 of file Body.cpp.

double hrp::Body::totalMass ( ) const
inline

Definition at line 194 of file Body.h.

void Body::updateLinkColdetModelPositions ( )

This function must be called before the collision detection. It updates the positions and orientations of the models for detecting collisions between links.

Definition at line 660 of file Body.cpp.

void Body::updateLinkTree ( )

This function must be called when the structure of the link tree is changed.

Definition at line 211 of file Body.cpp.

Friends And Related Function Documentation

friend class CustomizedJointPath
friend

Definition at line 312 of file Body.h.

Member Data Documentation

std::vector<SensorArray> hrp::Body::allSensors
private

Definition at line 290 of file Body.h.

BodyHandle hrp::Body::bodyHandle
private

Definition at line 306 of file Body.h.

BodyHandleEntity hrp::Body::bodyHandleEntity
private

Definition at line 305 of file Body.h.

BodyCustomizerHandle hrp::Body::customizerHandle
private

Definition at line 303 of file Body.h.

BodyCustomizerInterface* hrp::Body::customizerInterface
private

Definition at line 304 of file Body.h.

Matrix33 hrp::Body::defaultRootAttitude
private

Definition at line 300 of file Body.h.

Vector3 hrp::Body::defaultRootPosition
private

Definition at line 299 of file Body.h.

std::vector<ExtraJoint> hrp::Body::extraJoints

Definition at line 256 of file Body.h.

bool hrp::Body::isStaticModel_
private

Definition at line 274 of file Body.h.

LinkArray hrp::Body::jointIdToLinkArray
private

Definition at line 281 of file Body.h.

LinkTraverse hrp::Body::linkTraverse_
private

Definition at line 283 of file Body.h.

std::string hrp::Body::modelName_
private

Definition at line 278 of file Body.h.

std::string hrp::Body::name_
private

Definition at line 277 of file Body.h.

NameToLightMap hrp::Body::nameToLightMap
private

Definition at line 295 of file Body.h.

NameToLinkMap hrp::Body::nameToLinkMap
private

Definition at line 286 of file Body.h.

NameToSensorMap hrp::Body::nameToSensorMap
private

Definition at line 293 of file Body.h.

Link* hrp::Body::rootLink_
private

Definition at line 275 of file Body.h.

double hrp::Body::totalMass_
private

Definition at line 297 of file Body.h.


The documentation for this class was generated from the following files:


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:45