Computes forward kinematics for the robot. More...
#include <forward_kinematics.h>
Public Member Functions | |
ForwardKinematics (const urdf::Model &model) | |
void | GetTransforms (const std::map< std::string, double > &joint_positions, std::vector< geometry_msgs::TransformStamped > *transforms) const |
void | Init () |
Private Member Functions | |
void | AddChildren (const KDL::SegmentMap::const_iterator segment) |
Private Attributes | |
urdf::Model | model_ |
std::map< std::string, SegmentPair > | segments_ |
KDL::Tree | tree_ |
Computes forward kinematics for the robot.
Based on code from robot_state_publisher.
Definition at line 26 of file forward_kinematics.h.
robot_markers::ForwardKinematics::ForwardKinematics | ( | const urdf::Model & | model | ) | [explicit] |
Constructor.
[in] | model | The URDF model. |
Definition at line 19 of file forward_kinematics.cpp.
void robot_markers::ForwardKinematics::AddChildren | ( | const KDL::SegmentMap::const_iterator | segment | ) | [private] |
Definition at line 62 of file forward_kinematics.cpp.
void robot_markers::ForwardKinematics::GetTransforms | ( | const std::map< std::string, double > & | joint_positions, |
std::vector< geometry_msgs::TransformStamped > * | transforms | ||
) | const |
Outputs the transforms for each set of joint angles passed in.
If a joint is not part of the URDF, then it is skipped.
[in] | joint_positions | The joint angles to query. |
[out] | transforms | The transforms, one for each joint angle. |
Definition at line 27 of file forward_kinematics.cpp.
Initializes the forward kinematics.
This should be called before calling any other methods.
Definition at line 22 of file forward_kinematics.cpp.
Definition at line 49 of file forward_kinematics.h.
std::map<std::string, SegmentPair> robot_markers::ForwardKinematics::segments_ [private] |
Definition at line 51 of file forward_kinematics.h.
Definition at line 50 of file forward_kinematics.h.