robot_state.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Ioan A. Sucan
*  Copyright (c) 2013, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

/* Author: Ioan Sucan */

#ifndef MOVEIT_CORE_ROBOT_STATE_
#define MOVEIT_CORE_ROBOT_STATE_

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/attached_body.h>
#include <sensor_msgs/JointState.h>
#include <visualization_msgs/MarkerArray.h>
#include <geometry_msgs/Twist.h>
#include <cassert>

#include <boost/assert.hpp>

namespace moveit
{
namespace core
{

MOVEIT_CLASS_FORWARD(RobotState); 

typedef boost::function<bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn;

class RobotState
{
public:
  
  RobotState(const RobotModelConstPtr &robot_model);
  ~RobotState();
  
  RobotState(const RobotState &other);
  
  RobotState& operator=(const RobotState &other);

  const RobotModelConstPtr& getRobotModel() const
  {
    return robot_model_;
  }
   
  std::size_t getVariableCount() const
  {
    return robot_model_->getVariableCount();
  }
  
  const std::vector<std::string>& getVariableNames() const
  {
    return robot_model_->getVariableNames();
  }
  
  const LinkModel* getLinkModel(const std::string &link) const
  {
    return robot_model_->getLinkModel(link);
  }

  const JointModel* getJointModel(const std::string &joint) const
  {
    return robot_model_->getJointModel(joint);
  }

  const JointModelGroup* getJointModelGroup(const std::string &group) const
  {
    return robot_model_->getJointModelGroup(group);
  }
  
  double* getVariablePositions()
  {
    return position_;
  }

  const double* getVariablePositions() const
  {
    return position_;
  }

  void setVariablePositions(const double *position);
  
  void setVariablePositions(const std::vector<double> &position)
  {
    assert(robot_model_->getVariableCount() <= position.size()); // checked only in debug mode
    setVariablePositions(&position[0]);
  }
  
  void setVariablePositions(const std::map<std::string, double> &variable_map);

  void setVariablePositions(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables);

  void setVariablePositions(const std::vector<std::string>& variable_names, const std::vector<double>& variable_position);
  
  void setVariablePosition(const std::string &variable, double value)
  {
    setVariablePosition(robot_model_->getVariableIndex(variable), value);
  }
  
  void setVariablePosition(int index, double value)
  {
    position_[index] = value;
    const JointModel *jm = robot_model_->getJointOfVariable(index);
    if (jm)
    {
      markDirtyJointTransforms(jm);
      updateMimicJoint(jm);
    }
  }
  
  const double getVariablePosition(const std::string &variable) const
  {
    return position_[robot_model_->getVariableIndex(variable)];
  }

  const double getVariablePosition(int index) const
  {
    return position_[index];
  }
  
  bool hasVelocities() const
  {
    return has_velocity_;
  }

  double* getVariableVelocities()
  {
    markVelocity();
    return velocity_;
  }

  const double* getVariableVelocities() const
  {
    return velocity_;
  }
  
  void setVariableVelocities(const double *velocity)
  {
    has_velocity_ = true;
    // assume everything is in order in terms of array lengths (for efficiency reasons)
    memcpy(velocity_, velocity, robot_model_->getVariableCount() * sizeof(double));
  }

  void setVariableVelocities(const std::vector<double> &velocity)
  {
    assert(robot_model_->getVariableCount() <= velocity.size()); // checked only in debug mode
    setVariableVelocities(&velocity[0]);
  }

  void setVariableVelocities(const std::map<std::string, double> &variable_map);

  void setVariableVelocities(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);

  void setVariableVelocities(const std::vector<std::string>& variable_names, const std::vector<double>& variable_velocity);

  void setVariableVelocity(const std::string &variable, double value)
  {
    setVariableVelocity(robot_model_->getVariableIndex(variable), value);
  }

  void setVariableVelocity(int index, double value)
  {
    markVelocity();
    velocity_[index] = value;
  }
  
  const double getVariableVelocity(const std::string &variable) const
  {
    return velocity_[robot_model_->getVariableIndex(variable)];
  }
  
  const double getVariableVelocity(int index) const
  {
    return velocity_[index];
  }
  
  bool hasAccelerations() const
  {
    return has_acceleration_;
  }

  double* getVariableAccelerations()
  {
    markAcceleration();
    return acceleration_;
  }
  
  const double* getVariableAccelerations() const
  {
    return acceleration_;
  }
  
  void setVariableAccelerations(const double *acceleration)
  {
    has_acceleration_ = true;
    has_effort_ = false;
    
    // assume everything is in order in terms of array lengths (for efficiency reasons)
    memcpy(acceleration_, acceleration, robot_model_->getVariableCount() * sizeof(double));
  }
  
  void setVariableAccelerations(const std::vector<double> &acceleration)
  {
    assert(robot_model_->getVariableCount() <= acceleration.size()); // checked only in debug mode
    setVariableAccelerations(&acceleration[0]);
  }

  void setVariableAccelerations(const std::map<std::string, double> &variable_map);

  void setVariableAccelerations(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);

  void setVariableAccelerations(const std::vector<std::string>& variable_names, const std::vector<double>& variable_acceleration);
  
  void setVariableAcceleration(const std::string &variable, double value)
  {
    setVariableAcceleration(robot_model_->getVariableIndex(variable), value);
  }
  
  void setVariableAcceleration(int index, double value)
  {
    markAcceleration();
    acceleration_[index] = value;
  }
  
  const double getVariableAcceleration(const std::string &variable) const
  {
    return acceleration_[robot_model_->getVariableIndex(variable)];
  }

  const double getVariableAcceleration(int index) const
  {
    return acceleration_[index];
  }
  
  bool hasEffort() const
  {
    return has_effort_;
  }

  double* getVariableEffort()
  {
    markEffort();
    return effort_;
  }
  
  const double* getVariableEffort() const
  {
    return effort_;
  }
  
  void setVariableEffort(const double *effort)
  {
    has_effort_ = true;
    has_acceleration_ = false;
    // assume everything is in order in terms of array lengths (for efficiency reasons)
    memcpy(effort_, effort, robot_model_->getVariableCount() * sizeof(double));
  }
  
  void setVariableEffort(const std::vector<double> &effort)
  {
    assert(robot_model_->getVariableCount() <= effort.size()); // checked only in debug mode
    setVariableEffort(&effort[0]);
  }
  
  void setVariableEffort(const std::map<std::string, double> &variable_map);

  void setVariableEffort(const std::map<std::string, double> &variable_map, std::vector<std::string>& missing_variables);

  void setVariableEffort(const std::vector<std::string>& variable_names, const std::vector<double>& variable_acceleration);

  void setVariableEffort(const std::string &variable, double value)
  {
    setVariableEffort(robot_model_->getVariableIndex(variable), value);
  }

  void setVariableEffort(int index, double value)
  {
    markEffort();
    effort_[index] = value;
  }

  const double getVariableEffort(const std::string &variable) const
  {
    return effort_[robot_model_->getVariableIndex(variable)];
  }

  const double getVariableEffort(int index) const
  {
    return effort_[index];
  }
  
  void setJointPositions(const std::string &joint_name, const double *position)
  {  
    setJointPositions(robot_model_->getJointModel(joint_name), position);
  }
  
  void setJointPositions(const std::string &joint_name, const std::vector<double> &position)
  {
    setJointPositions(robot_model_->getJointModel(joint_name), &position[0]);
  }
  
  void setJointPositions(const JointModel *joint, const std::vector<double> &position)
  {
    setJointPositions(joint, &position[0]);
  }
  
  void setJointPositions(const JointModel *joint, const double *position)
  {  
    memcpy(position_ + joint->getFirstVariableIndex(), position, joint->getVariableCount() * sizeof(double));
    markDirtyJointTransforms(joint);
    updateMimicJoint(joint);
  }
  
  void setJointPositions(const std::string &joint_name, const Eigen::Affine3d& transform)
  {
    setJointPositions(robot_model_->getJointModel(joint_name), transform);
  }
  
  void setJointPositions(const JointModel *joint, const Eigen::Affine3d& transform)
  {
    joint->computeVariablePositions(transform, position_ + joint->getFirstVariableIndex());
    markDirtyJointTransforms(joint);
    updateMimicJoint(joint);
  }
  
  const double* getJointPositions(const std::string &joint_name) const
  {
    return getJointPositions(robot_model_->getJointModel(joint_name));
  }
  
  const double* getJointPositions(const JointModel *joint) const
  {
    return position_ + joint->getFirstVariableIndex();
  }
  
  const double* getJointVelocities(const std::string &joint_name) const
  {
    return getJointVelocities(robot_model_->getJointModel(joint_name));
  }
  
  const double* getJointVelocities(const JointModel *joint) const
  {
    return velocity_ + joint->getFirstVariableIndex();
  }

  const double* getJointAccelerations(const std::string &joint_name) const
  {
    return getJointAccelerations(robot_model_->getJointModel(joint_name));
  }
  
  const double* getJointAccelerations(const JointModel *joint) const
  {
    return acceleration_ + joint->getFirstVariableIndex();
  }

  const double* getJointEffort(const std::string &joint_name) const
  {
    return getJointEffort(robot_model_->getJointModel(joint_name));
  }
  
  const double* getJointEffort(const JointModel *joint) const
  {
    return effort_ + joint->getFirstVariableIndex();
  }

  void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupPositions(jmg, gstate);
  }
  
  void setJointGroupPositions(const std::string &joint_group_name, const std::vector<double> &gstate)
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupPositions(jmg, &gstate[0]);
  }
  
  void setJointGroupPositions(const JointModelGroup *group, const std::vector<double> &gstate)
  {
    setJointGroupPositions(group, &gstate[0]);
  }
  
  void setJointGroupPositions(const JointModelGroup *group, const double *gstate);

  void setJointGroupPositions(const std::string &joint_group_name, const Eigen::VectorXd& values)
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupPositions(jmg, values);
  }
  
  void setJointGroupPositions(const JointModelGroup *group, const Eigen::VectorXd& values);
  
  void copyJointGroupPositions(const std::string &joint_group_name, std::vector<double> &gstate) const
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
    {
      gstate.resize(jmg->getVariableCount());
      copyJointGroupPositions(jmg, &gstate[0]);
    }
  }
  
  void copyJointGroupPositions(const std::string &joint_group_name, double *gstate) const
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupPositions(jmg, gstate);
  }
  
  void copyJointGroupPositions(const JointModelGroup *group, std::vector<double> &gstate) const
  {
    gstate.resize(group->getVariableCount());
    copyJointGroupPositions(group, &gstate[0]);
  }
  
  void copyJointGroupPositions(const JointModelGroup *group, double *gstate) const;
  
  void copyJointGroupPositions(const std::string &joint_group_name, Eigen::VectorXd& values) const
  {
    const JointModelGroup *jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupPositions(jmg, values);
  }

  void copyJointGroupPositions(const JointModelGroup *group, Eigen::VectorXd& values) const;

  bool setToIKSolverFrame(Eigen::Affine3d &pose, const kinematics::KinematicsBaseConstPtr& solver);

  bool setToIKSolverFrame(Eigen::Affine3d &pose, const std::string &ik_frame);

  bool setFromIK(const JointModelGroup *group, 
                 const geometry_msgs::Pose &pose,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint =  GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const geometry_msgs::Pose &pose, const std::string &tip,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const Eigen::Affine3d &pose,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const Eigen::Affine3d &pose, const std::string &tip, 
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const Eigen::Affine3d &pose, const std::string &tip,
                 const std::vector<double> &consistency_limits,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const EigenSTL::vector_Affine3d &poses,
                 const std::vector<std::string> &tips,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());
  
  bool setFromIK(const JointModelGroup *group, 
                 const EigenSTL::vector_Affine3d &poses,
                 const std::vector<std::string> &tips,
                 const std::vector<std::vector<double> > &consistency_limits,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIKSubgroups(const JointModelGroup *group,
                 const EigenSTL::vector_Affine3d &poses,
                 const std::vector<std::string> &tips,
                 const std::vector<std::vector<double> > &consistency_limits,
                 unsigned int attempts = 0, double timeout = 0.0,
                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromDiffIK(const JointModelGroup *group, 
                     const Eigen::VectorXd &twist, const std::string &tip, double dt,
                     const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());
  
  bool setFromDiffIK(const JointModelGroup *group, 
                     const geometry_msgs::Twist &twist, const std::string &tip, double dt,
                     const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());


  double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
                              const Eigen::Vector3d &direction, bool global_reference_frame, double distance, double max_step, double jump_threshold,
                              const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
                              const Eigen::Affine3d &target, bool global_reference_frame, double max_step, double jump_threshold,
                              const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  double computeCartesianPath(const JointModelGroup *group, std::vector<RobotStatePtr> &traj, const LinkModel *link,
                              const EigenSTL::vector_Affine3d &waypoints, bool global_reference_frame, double max_step, double jump_threshold,
                              const GroupStateValidityCallbackFn &validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position,
                   Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false) const;

  bool getJacobian(const JointModelGroup *group, const LinkModel *link, const Eigen::Vector3d &reference_point_position,
                   Eigen::MatrixXd& jacobian, bool use_quaternion_representation = false)
  {
    updateLinkTransforms();
    return const_cast<const RobotState*>(this)->getJacobian(group, link, reference_point_position, jacobian, use_quaternion_representation);
  }

  Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0)) const;

  Eigen::MatrixXd getJacobian(const JointModelGroup *group, const Eigen::Vector3d &reference_point_position = Eigen::Vector3d(0.0, 0.0, 0.0))
  {
    updateLinkTransforms();
    return const_cast<const RobotState*>(this)->getJacobian(group, reference_point_position);
  }
  
  void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot,
                               const Eigen::VectorXd &twist, const LinkModel *tip) const;
  
  void computeVariableVelocity(const JointModelGroup *jmg, Eigen::VectorXd &qdot,
                               const Eigen::VectorXd &twist, const LinkModel *tip)
  {
    updateLinkTransforms();
    const_cast<const RobotState*>(this)->computeVariableVelocity(jmg, qdot, twist, tip);
  }
  
  bool integrateVariableVelocity(const JointModelGroup *jmg, const Eigen::VectorXd &qdot, double dt,
                                 const GroupStateValidityCallbackFn &constraint = GroupStateValidityCallbackFn());
  
  void setVariableValues(const sensor_msgs::JointState& msg)
  {
    if (!msg.position.empty())
      setVariablePositions(msg.name, msg.position);
    if (!msg.velocity.empty())
      setVariableVelocities(msg.name, msg.velocity);
  }
  
  void setToDefaultValues();

  bool setToDefaultValues(const JointModelGroup *group, const std::string &name);

  void setToRandomPositions();

  void setToRandomPositions(const JointModelGroup *group);

  void setToRandomPositions(const JointModelGroup *group, random_numbers::RandomNumberGenerator &rng);

  void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, double distance);

  void setToRandomPositionsNearBy(const JointModelGroup *group, const RobotState &near, const std::vector<double> &distances);

  void updateCollisionBodyTransforms();
  
  void updateLinkTransforms();
  
  void update(bool force = false);
  
  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Affine3d& transform, bool backward = false)
  {
    updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
  }

  void updateStateWithLinkAt(const LinkModel *link, const Eigen::Affine3d& transform, bool backward = false);

  const Eigen::Affine3d& getGlobalLinkTransform(const std::string &link_name)
  {
    return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
  }
  
  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel *link)
  {
    updateLinkTransforms();
    return global_link_transforms_[link->getLinkIndex()];
  }
  
  const Eigen::Affine3d& getCollisionBodyTransforms(const std::string &link_name, std::size_t index)
  {
    return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
  }
  
  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel *link, std::size_t index)
  {
    updateCollisionBodyTransforms();
    return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
  }
  
  const Eigen::Affine3d& getJointTransform(const std::string &joint_name)
  {
    return getJointTransform(robot_model_->getJointModel(joint_name));
  }
  
  const Eigen::Affine3d& getJointTransform(const JointModel *joint)
  {
    const int idx = joint->getJointIndex();
    unsigned char &dirty = dirty_joint_transforms_[idx];
    if (dirty)
    {
      joint->computeTransform(position_ + joint->getFirstVariableIndex(), variable_joint_transforms_[idx]);
      dirty = 0;
    }
    return variable_joint_transforms_[idx];
  }
  
  const Eigen::Affine3d& getGlobalLinkTransform(const std::string &link_name) const
  {
    return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
  }
  
  const Eigen::Affine3d& getGlobalLinkTransform(const LinkModel *link) const
  {
    BOOST_VERIFY(checkLinkTransforms());
    return global_link_transforms_[link->getLinkIndex()];
  }
  
  const Eigen::Affine3d& getCollisionBodyTransform(const std::string &link_name, std::size_t index) const
  {
    return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
  }
  
  const Eigen::Affine3d& getCollisionBodyTransform(const LinkModel *link, std::size_t index) const
  {
    BOOST_VERIFY(checkCollisionTransforms());
    return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
  }
  
  const Eigen::Affine3d& getJointTransform(const std::string &joint_name) const
  {
    return getJointTransform(robot_model_->getJointModel(joint_name));
  }
  
  const Eigen::Affine3d& getJointTransform(const JointModel *joint) const
  {
    BOOST_VERIFY(checkJointTransforms(joint));
    return variable_joint_transforms_[joint->getJointIndex()];
  }
  
  bool dirtyJointTransform(const JointModel *joint) const
  {
    return dirty_joint_transforms_[joint->getJointIndex()];
  }
  
  bool dirtyLinkTransforms() const
  {
    return dirty_link_transforms_;
  }
  
  bool dirtyCollisionBodyTransforms() const
  {
    return dirty_link_transforms_ || dirty_collision_body_transforms_;
  }
  
  bool dirty() const
  {
    return dirtyCollisionBodyTransforms();
  }
  
  double distance(const RobotState &other) const
  {
    return robot_model_->distance(position_, other.getVariablePositions());
  }

  double distance(const RobotState &other, const JointModelGroup *joint_group) const;
  
  double distance(const RobotState &other, const JointModel *joint) const
  {
    const int idx = joint->getFirstVariableIndex();
    return joint->distance(position_ + idx, other.position_ + idx);
  }
  
  void interpolate(const RobotState &to, double t, RobotState &state) const;
  
  void interpolate(const RobotState &to, double t, RobotState &state, const JointModelGroup *joint_group) const;

  void interpolate(const RobotState &to, double t, RobotState &state, const JointModel *joint) const
  {
    const int idx = joint->getFirstVariableIndex();
    joint->interpolate(position_ + idx, to.position_ + idx, t, state.position_ + idx);
    state.markDirtyJointTransforms(joint);
    state.updateMimicJoint(joint);
  }

  void enforceBounds();
  void enforceBounds(const JointModelGroup *joint_group);
  void enforceBounds(const JointModel *joint)
  {
    enforcePositionBounds(joint);
    if (has_velocity_)
      enforceVelocityBounds(joint);
  }
  void enforcePositionBounds(const JointModel *joint)
  {
    if (joint->enforcePositionBounds(position_ + joint->getFirstVariableIndex()))
    {
      markDirtyJointTransforms(joint);
      updateMimicJoint(joint);
    }
  }
  void enforceVelocityBounds(const JointModel *joint)
  {
    joint->enforceVelocityBounds(velocity_ + joint->getFirstVariableIndex());
  }
  
  bool satisfiesBounds(double margin = 0.0) const;
  bool satisfiesBounds(const JointModelGroup *joint_group, double margin = 0.0) const;
  bool satisfiesBounds(const JointModel *joint, double margin = 0.0) const
  {
    return satisfiesPositionBounds(joint, margin) && (!has_velocity_ || satisfiesVelocityBounds(joint, margin));
  }
  bool satisfiesPositionBounds(const JointModel *joint, double margin = 0.0) const
  {
    return joint->satisfiesPositionBounds(getJointPositions(joint), margin);
  }
  bool satisfiesVelocityBounds(const JointModel *joint, double margin = 0.0) const
  {
    return joint->satisfiesVelocityBounds(getJointVelocities(joint), margin);
  }
  
  std::pair<double, const JointModel*> getMinDistanceToPositionBounds() const;
  
  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const JointModelGroup *group) const;

  std::pair<double, const JointModel*> getMinDistanceToPositionBounds(const std::vector<const JointModel*> &joints) const;
  
  void attachBody(AttachedBody *attached_body);
  
  void attachBody(const std::string &id,
                  const std::vector<shapes::ShapeConstPtr> &shapes,
                  const EigenSTL::vector_Affine3d &attach_trans,
                  const std::set<std::string> &touch_links,
                  const std::string &link_name,
                  const trajectory_msgs::JointTrajectory &detach_posture = trajectory_msgs::JointTrajectory());

  void attachBody(const std::string &id,
                  const std::vector<shapes::ShapeConstPtr> &shapes,
                  const EigenSTL::vector_Affine3d &attach_trans,
                  const std::vector<std::string> &touch_links,
                  const std::string &link_name,
                  const trajectory_msgs::JointTrajectory &detach_posture = trajectory_msgs::JointTrajectory())
  {
    std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
    attachBody(id, shapes, attach_trans, touch_links_set, link_name, detach_posture);
  }

  void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies) const;

  void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const JointModelGroup *lm) const;

  void getAttachedBodies(std::vector<const AttachedBody*> &attached_bodies, const LinkModel *lm) const;

  bool clearAttachedBody(const std::string &id);

  void clearAttachedBodies(const LinkModel *link);

  void clearAttachedBodies(const JointModelGroup *group);

  void clearAttachedBodies();

  const AttachedBody* getAttachedBody(const std::string &name) const;

  bool hasAttachedBody(const std::string &id) const;

  void setAttachedBodyUpdateCallback(const AttachedBodyCallback &callback);
  void computeAABB(std::vector<double> &aabb) const;

  void computeAABB(std::vector<double> &aabb)
  {
    updateLinkTransforms();
    const_cast<const RobotState*>(this)->computeAABB(aabb);
  }
  
  random_numbers::RandomNumberGenerator& getRandomNumberGenerator()
  {
    if (!rng_)
      rng_ = new random_numbers::RandomNumberGenerator();
    return *rng_;
  }

  const Eigen::Affine3d& getFrameTransform(const std::string &id);

  const Eigen::Affine3d& getFrameTransform(const std::string &id) const;

  bool knowsFrameTransform(const std::string &id) const;
  
  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       const std_msgs::ColorRGBA& color,
                       const std::string& ns,
                       const ros::Duration& dur,
                       bool include_attached = false) const;

  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       const std_msgs::ColorRGBA& color,
                       const std::string& ns,
                       const ros::Duration& dur,
                       bool include_attached = false)
  {
    updateCollisionBodyTransforms();
    const_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, color, ns, dur, include_attached);
  }
  
  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       bool include_attached = false) const;

  void getRobotMarkers(visualization_msgs::MarkerArray& arr,
                       const std::vector<std::string> &link_names,
                       bool include_attached = false)
  {  
    updateCollisionBodyTransforms();
    const_cast<const RobotState*>(this)->getRobotMarkers(arr, link_names, include_attached);
  }
  
  void printStatePositions(std::ostream &out = std::cout) const;
  
  void printStateInfo(std::ostream &out = std::cout) const;
  
  void printTransforms(std::ostream &out = std::cout) const;

  void printTransform(const Eigen::Affine3d &transform, std::ostream &out = std::cout) const;
  
  void printDirtyInfo(std::ostream &out = std::cout) const;
  
  std::string getStateTreeString(const std::string& prefix = "") const;
  
private:

  void allocMemory();

  void copyFrom(const RobotState &other);
  
  void markDirtyJointTransforms(const JointModel *joint)
  {
    dirty_joint_transforms_[joint->getJointIndex()] = 1;
    dirty_link_transforms_ = dirty_link_transforms_ == NULL ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
  }
  
  void markDirtyJointTransforms(const JointModelGroup *group)
  {
    const std::vector<const JointModel*> &jm = group->getActiveJointModels();
    for (std::size_t i = 0 ; i < jm.size() ; ++i)
      dirty_joint_transforms_[jm[i]->getJointIndex()] = 1;
    dirty_link_transforms_ = dirty_link_transforms_ == NULL ? group->getCommonRoot() : robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
  }
  
  void markVelocity();
  void markAcceleration();
  void markEffort();
 
  void updateMimicJoint(const JointModel *joint)
  {
    const std::vector<const JointModel*> &mim = joint->getMimicRequests();
    double v = position_[joint->getFirstVariableIndex()];
    for (std::size_t i = 0 ; i < mim.size() ; ++i)
    {
      position_[mim[i]->getFirstVariableIndex()] =  mim[i]->getMimicFactor() * v + mim[i]->getMimicOffset();
      dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
    }
  }
  
  void updateMimicJoint(const std::vector<const JointModel*> &mim)
  {
    for (std::size_t i = 0 ; i < mim.size() ; ++i)
    {
      const int fvi = mim[i]->getFirstVariableIndex();      
      position_[fvi] =  mim[i]->getMimicFactor() * position_[mim[i]->getMimic()->getFirstVariableIndex()] + mim[i]->getMimicOffset(); 
      dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
    }
  }
  
  void updateLinkTransformsInternal(const JointModel *start);
  
  void getMissingKeys(const std::map<std::string, double> &variable_map, std::vector<std::string> &missing_variables) const;
  void getStateTreeJointString(std::ostream& ss, const JointModel* jm, const std::string& pfx0, bool last) const;

  bool checkJointTransforms(const JointModel *joint) const;

  bool checkLinkTransforms() const;

  bool checkCollisionTransforms() const;
    
  RobotModelConstPtr                     robot_model_;
  void                                  *memory_;
  
  double                                *position_;
  double                                *velocity_;
  double                                *acceleration_; 
  double                                *effort_;
  bool                                   has_velocity_;
  bool                                   has_acceleration_;
  bool                                   has_effort_;
  
  const JointModel                      *dirty_link_transforms_;
  const JointModel                      *dirty_collision_body_transforms_;
  
  Eigen::Affine3d                       *variable_joint_transforms_; // this points to an element in transforms_, so it is aligned 
  Eigen::Affine3d                       *global_link_transforms_;  // this points to an element in transforms_, so it is aligned 
  Eigen::Affine3d                       *global_collision_body_transforms_;  // this points to an element in transforms_, so it is aligned 
  unsigned char                         *dirty_joint_transforms_;
  
  std::map<std::string, AttachedBody*>   attached_body_map_;

  AttachedBodyCallback                   attached_body_update_callback_;

  random_numbers::RandomNumberGenerator *rng_;
};

std::ostream& operator<<(std::ostream &out, const RobotState &s);

}
}

#endif