Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes
hrl_kinematics::Kinematics Class Reference

#include <Kinematics.h>

Inheritance diagram for hrl_kinematics::Kinematics:
Inheritance graph
[legend]

List of all members.

Classes

class  InitFailed

Public Types

enum  FootSupport { SUPPORT_DOUBLE, SUPPORT_SINGLE_RIGHT, SUPPORT_SINGLE_LEFT }

Public Member Functions

void computeCOM (const JointMap &joint_positions, tf::Point &com, double &mass, tf::Transform &tf_right_foot, tf::Transform &tf_left_foot)
void initialize ()
 Kinematics (std::string root_link_name="base_link", std::string rfoot_link_name="r_sole", std::string lfoot_link_name="l_sole", const boost::shared_ptr< const urdf::ModelInterface > &urdf_model=boost::shared_ptr< const urdf::ModelInterface >())
 Constructor.
virtual ~Kinematics ()

Protected Member Functions

void addChildren (const KDL::SegmentMap::const_iterator segment)
void computeCOMRecurs (const KDL::SegmentMap::const_iterator &currentSeg, const std::map< std::string, double > &joint_positions, const KDL::Frame &tf, KDL::Frame &tf_right_foot, KDL::Frame &tf_left_foot, double &m, KDL::Vector &cog)
void createCoGMarker (const std::string &ns, const std::string &frame_id, double radius, const KDL::Vector &cog, visualization_msgs::Marker &marker) const
bool loadKDLModel ()

Protected Attributes

KDL::Chain kdl_chain_left_
KDL::Chain kdl_chain_right_
KDL::Tree kdl_tree_
std::string lfoot_link_name_
ros::NodeHandle nh_
ros::NodeHandle nh_private_
unsigned int num_joints_
std::string rfoot_link_name_
std::string root_link_name_
std::map< std::string,
robot_state_publisher::SegmentPair
segments_
boost::shared_ptr< const
urdf::ModelInterface > 
urdf_model_

Detailed Description

Class to compute the center of mass while recursively traversing a kinematic tree of a robot (from URDF)

Definition at line 65 of file Kinematics.h.


Member Enumeration Documentation

Enumerator:
SUPPORT_DOUBLE 
SUPPORT_SINGLE_RIGHT 
SUPPORT_SINGLE_LEFT 

Definition at line 67 of file Kinematics.h.


Constructor & Destructor Documentation

hrl_kinematics::Kinematics::Kinematics ( std::string  root_link_name = "base_link",
std::string  rfoot_link_name = "r_sole",
std::string  lfoot_link_name = "l_sole",
const boost::shared_ptr< const urdf::ModelInterface > &  urdf_model = boost::shared_ptr<const urdf::ModelInterface>() 
)

Constructor.

Parameters:
root_link_name- name of link that is base of robot
rfoot_link_name- name of link that is considered the right foot
lfoot_link_name- name of link that is considered the left foot
urdf_model- a pointer to a pre-loaded URDF model that can speed up initialization if provided

Definition at line 38 of file Kinematics.cpp.

Definition at line 47 of file Kinematics.cpp.


Member Function Documentation

void hrl_kinematics::Kinematics::addChildren ( const KDL::SegmentMap::const_iterator  segment) [protected]

Definition at line 111 of file Kinematics.cpp.

void hrl_kinematics::Kinematics::computeCOM ( const JointMap joint_positions,
tf::Point com,
double &  mass,
tf::Transform tf_right_foot,
tf::Transform tf_left_foot 
)

Computes the center of mass of the given robot structure and joint configuration. Will also return the 6D transformations to the feet while traversing the robot tree

Parameters:
[in]joint_positionsangles of all joints
[out]comthe computed center of mass in the root coordinate frame (base_link)
[out]masstotal mass of all joints
[out]tf_right_footreturned transformation from body reference frame to right foot
[out]tf_left_footreturned transformation from body reference frame to left foot

Definition at line 197 of file Kinematics.cpp.

void hrl_kinematics::Kinematics::computeCOMRecurs ( const KDL::SegmentMap::const_iterator &  currentSeg,
const std::map< std::string, double > &  joint_positions,
const KDL::Frame tf,
KDL::Frame tf_right_foot,
KDL::Frame tf_left_foot,
double &  m,
KDL::Vector cog 
) [protected]

Definition at line 148 of file Kinematics.cpp.

void hrl_kinematics::Kinematics::createCoGMarker ( const std::string &  ns,
const std::string &  frame_id,
double  radius,
const KDL::Vector cog,
visualization_msgs::Marker &  marker 
) const [protected]

Definition at line 133 of file Kinematics.cpp.

Definition at line 51 of file Kinematics.cpp.

Definition at line 86 of file Kinematics.cpp.


Member Data Documentation

Definition at line 113 of file Kinematics.h.

Definition at line 112 of file Kinematics.h.

Definition at line 111 of file Kinematics.h.

Definition at line 118 of file Kinematics.h.

Definition at line 115 of file Kinematics.h.

Definition at line 115 of file Kinematics.h.

unsigned int hrl_kinematics::Kinematics::num_joints_ [protected]

Definition at line 120 of file Kinematics.h.

Definition at line 117 of file Kinematics.h.

Definition at line 116 of file Kinematics.h.

Definition at line 121 of file Kinematics.h.

boost::shared_ptr<const urdf::ModelInterface> hrl_kinematics::Kinematics::urdf_model_ [protected]

Definition at line 110 of file Kinematics.h.


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


hrl_kinematics
Author(s): Armin Hornung
autogenerated on Wed Aug 26 2015 11:51:09