Drake
HumanoidStatus Class Reference

Mostly a thin wrapper on RigidBodyTree. More...

#include <drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h>

Inheritance diagram for HumanoidStatus:
[legend]
Collaboration diagram for HumanoidStatus:
[legend]

Public Member Functions

 HumanoidStatus (const RigidBodyTree< double > *robot, const RigidBodyTreeAliasGroups< double > &alias_group)
 
void Update (double t, const Eigen::Ref< const VectorX< double >> &q, const Eigen::Ref< const VectorX< double >> &v, const Eigen::Ref< const VectorX< double >> &joint_torque, const Eigen::Ref< const Vector6< double >> &l_wrench, const Eigen::Ref< const Vector6< double >> &r_wrench)
 Do kinematics and compute useful information based on kinematics and measured force torque information. More...
 
const std::unordered_map< std::string, int > & body_name_to_id () const
 
const std::unordered_map< std::string, int > & name_to_position_index () const
 
const std::unordered_map< std::string, int > & name_to_velocity_index () const
 
const std::unordered_map< std::string, int > & actuator_name_to_id () const
 
double position (int i) const
 
double velocity (int i) const
 
double joint_torque (int i) const
 
const VectorX< double > & joint_torque () const
 
const BodyOfInterestpelvis () const
 
const BodyOfInteresttorso () const
 
const BodyOfInterestfoot (Side::SideEnum s) const
 
const BodyOfInterestfoot (int s) const
 
const BodyOfInterestfoot_sensor (Side::SideEnum s) const
 
const Vector2< double > & cop () const
 
const Vector2< double > & cop_in_sole_frame (Side::SideEnum s) const
 
const Vector6< double > & foot_wrench_in_sole_frame (Side::SideEnum s) const
 
const Vector6< double > & foot_wrench_raw (Side::SideEnum s) const
 
const Vector6< double > & foot_wrench_in_world_frame (Side::SideEnum s) const
 
const BodyOfInterestfoot_sensor (int s) const
 
const Vector2< double > & cop_in_sole_frame (int s) const
 
const Vector6< double > & foot_wrench_in_sole_frame (int s) const
 
const Vector6< double > & foot_wrench_raw (int s) const
 
const Vector6< double > & foot_wrench_in_world_frame (int s) const
 
- Public Member Functions inherited from RobotKinematicState< double >
 RobotKinematicState (const RigidBodyTree< double > *robot)
 Constructor. More...
 
virtual ~RobotKinematicState ()
 
std::unique_ptr< RobotKinematicState< double > > Clone () const
 
void UpdateKinematics (doublet, const Eigen::Ref< const VectorX< double >> &q, const Eigen::Ref< const VectorX< double >> &v)
 The generalized position q and velocity v are used to update the internal KinematicsCache. More...
 
void UpdateKinematics (const Eigen::Ref< const VectorX< double >> &q, const Eigen::Ref< const VectorX< double >> &v)
 The generalized position q and velocity v are used to update the internal KinematicsCache. More...
 
double get_time () const
 
const RigidBodyTree< double > & get_robot () const
 
const KinematicsCache< double > & get_cache () const
 
const Vector3< double > & get_com () const
 
Vector3< doubleget_com_velocity () const
 
const Vector6< double > & get_centroidal_momentum () const
 
const MatrixX< double > & get_M () const
 
const VectorX< double > & get_bias_term () const
 
const Matrix6X< double > & get_centroidal_momentum_matrix () const
 
const Vector6< double > & get_centroidal_momentum_matrix_dot_times_v () const
 

Static Public Attributes

static const Vector3< doublekFootToSoleOffset
 Position offset from the foot frame to the sole frame. More...
 
static const Vector3< doublekFootToSensorPositionOffset
 Position Offset from the foot frame to force torque sensor in the foot frame. More...
 
static const Matrix3< doublekFootToSensorRotationOffset
 Rotation Offset from the foot frame to force torque sensor in the foot frame. More...
 

Protected Member Functions

Implements CopyConstructible, CopyAssignable, MoveConstructible, MoveAssignable
 HumanoidStatus (const HumanoidStatus &)=default
 
HumanoidStatusoperator= (const HumanoidStatus &)=default
 
 HumanoidStatus (HumanoidStatus &&)=default
 
HumanoidStatusoperator= (HumanoidStatus &&)=default
 
- Protected Member Functions inherited from RobotKinematicState< double >
 RobotKinematicState (const RobotKinematicState &)=default
 
 RobotKinematicState (RobotKinematicState &&)=default
 
RobotKinematicStateoperator= (const RobotKinematicState &)=default
 
RobotKinematicStateoperator= (RobotKinematicState &&)=default
 

Detailed Description

Mostly a thin wrapper on RigidBodyTree.

It has kinematic values such as spatial velocity of various body parts, some measured contact force / torque information, joint torque, etc. It also stores robot specific constants.

Constructor & Destructor Documentation

HumanoidStatus ( const HumanoidStatus )
protecteddefault
HumanoidStatus ( HumanoidStatus &&  )
protecteddefault
HumanoidStatus ( const RigidBodyTree< double > *  robot,
const RigidBodyTreeAliasGroups< double > &  alias_group 
)
Parameters
robotPointer to a RigidBodyTree, which must be valid through the lifespan of this obejct.

Here is the call graph for this function:

Member Function Documentation

const std::unordered_map<std::string, int>& actuator_name_to_id ( ) const
inline
const std::unordered_map<std::string, int>& body_name_to_id ( ) const
inline
const Vector2<double>& cop ( ) const
inline
const Vector2<double>& cop_in_sole_frame ( Side::SideEnum  s) const
inline
const Vector2<double>& cop_in_sole_frame ( int  s) const
inline
const BodyOfInterest& foot ( Side::SideEnum  s) const
inline

Here is the caller graph for this function:

const BodyOfInterest& foot ( int  s) const
inline
const BodyOfInterest& foot_sensor ( Side::SideEnum  s) const
inline

Here is the caller graph for this function:

const BodyOfInterest& foot_sensor ( int  s) const
inline
const Vector6<double>& foot_wrench_in_sole_frame ( Side::SideEnum  s) const
inline
const Vector6<double>& foot_wrench_in_sole_frame ( int  s) const
inline
const Vector6<double>& foot_wrench_in_world_frame ( Side::SideEnum  s) const
inline
const Vector6<double>& foot_wrench_in_world_frame ( int  s) const
inline

Here is the call graph for this function:

const Vector6<double>& foot_wrench_raw ( Side::SideEnum  s) const
inline
const Vector6<double>& foot_wrench_raw ( int  s) const
inline
double joint_torque ( int  i) const
inline
const VectorX<double>& joint_torque ( ) const
inline

Here is the caller graph for this function:

const std::unordered_map<std::string, int>& name_to_position_index ( ) const
inline
const std::unordered_map<std::string, int>& name_to_velocity_index ( ) const
inline
HumanoidStatus& operator= ( HumanoidStatus &&  )
protecteddefault
HumanoidStatus& operator= ( const HumanoidStatus )
protecteddefault
const BodyOfInterest& pelvis ( ) const
inline
double position ( int  i) const
inline

Here is the caller graph for this function:

const BodyOfInterest& torso ( ) const
inline
void Update ( double  t,
const Eigen::Ref< const VectorX< double >> &  q,
const Eigen::Ref< const VectorX< double >> &  v,
const Eigen::Ref< const VectorX< double >> &  joint_torque,
const Eigen::Ref< const Vector6< double >> &  l_wrench,
const Eigen::Ref< const Vector6< double >> &  r_wrench 
)

Do kinematics and compute useful information based on kinematics and measured force torque information.

Parameters
timeIn seconds
qGeneralized positions.
vGeneralized velocities.
joint_torqueJoint torque, should be in the same order as v, not in the actuator order
l_wrenchWrench measured in the sensor frame.
r_wrenchWrench measured in the sensor frame.

Here is the call graph for this function:

double velocity ( int  i) const
inline

Here is the caller graph for this function:

Member Data Documentation

const Vector3< double > kFootToSensorPositionOffset
static
Initial value:
=
Vector3<double>(0.0215646, 0.0, -0.051054)

Position Offset from the foot frame to force torque sensor in the foot frame.

const Matrix3< double > kFootToSensorRotationOffset
static
Initial value:
=
Matrix3<double>(AngleAxis<double>(-M_PI, Vector3<double>::UnitX()))

Rotation Offset from the foot frame to force torque sensor in the foot frame.

const Vector3< double > kFootToSoleOffset
static
Initial value:
=
Vector3<double>(0, 0, -0.09)

Position offset from the foot frame to the sole frame.


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