Drake
RobotStateLcmMessageTranslator Class Reference

This is a utility class for converting bot_core::robot_state_t message to and from various eigen vectors that correspond to the generalized position, velocity and actuator torque. More...

#include <drake/manipulation/util/robot_state_msg_translator.h>

Public Member Functions

 RobotStateLcmMessageTranslator (const RigidBodyTree< double > &robot)
 Constructs a translator using the model given in robot. More...
 
void InitializeMessage (bot_core::robot_state_t *msg) const
 Initializes msg based on the rigid body tree passed to the constructor. More...
 
void DecodeMessageKinematics (const bot_core::robot_state_t &msg, Eigen::Ref< VectorX< double >> q, Eigen::Ref< VectorX< double >> v) const
 Decodes only the kinematic values (q, v) from msg to q and v. More...
 
void EncodeMessageKinematics (const Eigen::Ref< const VectorX< double >> &q, const Eigen::Ref< const VectorX< double >> &v, bot_core::robot_state_t *msg) const
 Encodes only the generalized position q and velocity v to msg. More...
 
void DecodeMessageTorque (const bot_core::robot_state_t &msg, Eigen::Ref< VectorX< double >> torque) const
 Decodes only the non-floating joint torque part from msg to torque. More...
 
void EncodeMessageTorque (const Eigen::Ref< const VectorX< double >> &torque, bot_core::robot_state_t *msg) const
 Encodes only the actuated non-floating joint torque to msg. More...
 
const RigidBodyTree< double > & get_robot () const
 Returns a const reference to the rigid body tree. More...
 
Does not allow copy, move, or assignment
 RobotStateLcmMessageTranslator (const RobotStateLcmMessageTranslator &)=delete
 
RobotStateLcmMessageTranslatoroperator= (const RobotStateLcmMessageTranslator &)=delete
 
 RobotStateLcmMessageTranslator (RobotStateLcmMessageTranslator &&)=delete
 
RobotStateLcmMessageTranslatoroperator= (RobotStateLcmMessageTranslator &&)=delete
 

Static Public Member Functions

static bool CheckMessageVectorSize (const bot_core::robot_state_t &msg)
 Checks msg non-floating joint parts have consistent dimensions. More...
 

Detailed Description

This is a utility class for converting bot_core::robot_state_t message to and from various eigen vectors that correspond to the generalized position, velocity and actuator torque.

The bot_core::robot_state_t has three major components:

  kinematics: generalized position and velocity including the floating base.
  actuator torques: joint torque for actuated joints.
  force torque sensor: foot / wrist mounted force torque sensor readings.

The pose in the message represents the transformation from the floating base frame B to the world frame W (X_WB), and the velocity is its spatial velocity V_WB. For the non-floating joints in bot_core::robot_state_t, the convention is that they (name, position, velocity, effort) are stored in equal length vectors, and the numerical values are identified by the joint name with the same index. This joint name is also used to compare against position names in a RigidBodyTree to establish correspondence between the data layouts in the message and the RigidBodyTree. Note that this class does not require one to one joint matching between the message and the RigidBodyTree. This class only translates information for joints that are shared by the message the RigidBodyTree. This design choice is made to facilitate multiple models (rigid body tree) with different complexity of the same robot communicating on the same Lcm channel. For example, a central message publisher generates a Lcm message with the most comprehensive information, and the individual subscribers can pay attention to whatever subset of interest using reduced models.

Constructor & Destructor Documentation

RobotStateLcmMessageTranslator ( const RigidBodyTree< double > &  robot)
explicit

Constructs a translator using the model given in robot.

A const reference of robot is saved, and thus its life span must be longer than this. The following assumptions must be met by robot, otherwise the constructor throws a std::logic_error:

  1. There is at least 1 non-world rigid body.
  2. The first non-world rigid body has to be attached to the world with
  either a fixed joint or a floating joint.
  3. If the first non-world rigid body is attached with a fixed joints,
  there cannot be another floating joint in the tree.
  4. There is at most 1 floating joint in the tree.
  5. There can be at most 1 rigid body attached to the world with a fixed
  joint.
  6. If there is a floating joint, its position and velocity index have
  to start from 0.
  7. All the other joints have to be 1 degree of freedom or fixed.

Here is the call graph for this function:

Member Function Documentation

bool CheckMessageVectorSize ( const bot_core::robot_state_t &  msg)
static

Checks msg non-floating joint parts have consistent dimensions.

Here is the caller graph for this function:

void DecodeMessageKinematics ( const bot_core::robot_state_t &  msg,
Eigen::Ref< VectorX< double >>  q,
Eigen::Ref< VectorX< double >>  v 
) const

Decodes only the kinematic values (q, v) from msg to q and v.

q and v are assumed to match the rigid body tree's generalized position and velocity size. Note that the values in q and v that correspond to the floating base will always be set to the decoded values in msg. However, for the non-floating joints, only the ones that are present in the message will be decoded and set. For example, suppose the rigid body tree has 2 non-floating joints: j1, and j2. If msg only contains j1, then j2's corresponding position and velocity in q and v will not be set. Conversely, if msg contains extra joints than in the rigid body tree, their information are simply ignored.

Parameters
[in]msgLcm message to be decoded.
[out]qVector to hold the decoded generalized position.
[out]vVector to hold the decoded generalized velocity.

Here is the call graph for this function:

Here is the caller graph for this function:

void DecodeMessageTorque ( const bot_core::robot_state_t &  msg,
Eigen::Ref< VectorX< double >>  torque 
) const

Decodes only the non-floating joint torque part from msg to torque.

torque is assumed to have the same size as the rigid body tree's number of actuators, and it is assumed to be in the same order, which is not necessarily in the same order as its generalized position or velocity. Only the actuated joints in the rigid body tree will be decoded (if they are present in msg).

Here is the call graph for this function:

Here is the caller graph for this function:

void EncodeMessageKinematics ( const Eigen::Ref< const VectorX< double >> &  q,
const Eigen::Ref< const VectorX< double >> &  v,
bot_core::robot_state_t *  msg 
) const

Encodes only the generalized position q and velocity v to msg.

The information in msg that correspond to the floating base will always be set according to q and v, or identity / zero if there is no floating base. Only joints whose names are presented in msg and in the rigid body tree will be set. Suppose msg lists 2 non-floating joints: j1 and j2, and the rigid body tree only has j1, then j2's corresponding position and velocity will not be set in msg. Conversely, if the rigid body tree has joints that are not in msg, their information will not be encoded.

Parameters
[in]qGeneralized position that corresponds to the rigid body tree passed to the constructor.
[in]vGeneralized velocity that corresponds to the rigid body tree passed to the constructor.
[in,out]msgLcm message to be encoded.

Here is the call graph for this function:

Here is the caller graph for this function:

void EncodeMessageTorque ( const Eigen::Ref< const VectorX< double >> &  torque,
bot_core::robot_state_t *  msg 
) const

Encodes only the actuated non-floating joint torque to msg.

torque needs to match the rigid body tree's actuator size, and it is assumed to be in the same order as the rigid body tree's actuators, which is not necessarily in the same order as its generalized position or velocity. Only the actuated joints listed in msg will be encoded.

Here is the call graph for this function:

Here is the caller graph for this function:

const RigidBodyTree<double>& get_robot ( ) const
inline

Returns a const reference to the rigid body tree.

Here is the call graph for this function:

Here is the caller graph for this function:

void InitializeMessage ( bot_core::robot_state_t *  msg) const

Initializes msg based on the rigid body tree passed to the constructor.

It resizes all the vectors for the non-floating joints in the order of the rigid body tree's generalized coordinate. All numerical values are initialized to zero. It is highly recommended to initialize bot_core::robot_state_t with this method before passing it to the rest of the API in this class.

Here is the call graph for this function:

Here is the caller graph for this function:


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