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 <moveit/macros/deprecation.h>
#include <sensor_msgs/JointState.h>
#include <visualization_msgs/MarkerArray.h>
#include <std_msgs/ColorRGBA.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;

struct JumpThreshold
{
  double factor;
  double revolute;   // Radians
  double prismatic;  // Meters

  explicit JumpThreshold() : factor(0.0), revolute(0.0), prismatic(0.0)
  {
  }

  explicit JumpThreshold(double jt_factor) : JumpThreshold()
  {
    factor = jt_factor;
  }

  explicit JumpThreshold(double jt_revolute, double jt_prismatic) : JumpThreshold()
  {
    revolute = jt_revolute;    // Radians
    prismatic = jt_prismatic;  // Meters
  }
};

struct MaxEEFStep
{
  MaxEEFStep(double translation = 0.0, double rotation = 0.0) : translation(translation), rotation(rotation)
  {
  }

  double translation;  // Meters
  double rotation;     // Radians
};

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);
    }
  }

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

  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;
  }

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

  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;
  }

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

  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;
  }

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

  double getVariableEffort(int index) const
  {
    return effort_[index];
  }

  void invertVelocity();

  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);
  }

  void setJointVelocities(const JointModel* joint, const double* velocity)
  {
    has_velocity_ = true;
    memcpy(velocity_ + joint->getFirstVariableIndex(), velocity, joint->getVariableCount() * sizeof(double));
  }

  void setJointEfforts(const JointModel* joint, const double* effort);

  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;

  void setJointGroupVelocities(const std::string& joint_group_name, const double* gstate)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupVelocities(jmg, gstate);
  }

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

  void setJointGroupVelocities(const JointModelGroup* group, const std::vector<double>& gstate)
  {
    setJointGroupVelocities(group, &gstate[0]);
  }

  void setJointGroupVelocities(const JointModelGroup* group, const double* gstate);

  void setJointGroupVelocities(const std::string& joint_group_name, const Eigen::VectorXd& values)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupVelocities(jmg, values);
  }

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

  void copyJointGroupVelocities(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());
      copyJointGroupVelocities(jmg, &gstate[0]);
    }
  }

  void copyJointGroupVelocities(const std::string& joint_group_name, double* gstate) const
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupVelocities(jmg, gstate);
  }

  void copyJointGroupVelocities(const JointModelGroup* group, std::vector<double>& gstate) const
  {
    gstate.resize(group->getVariableCount());
    copyJointGroupVelocities(group, &gstate[0]);
  }

  void copyJointGroupVelocities(const JointModelGroup* group, double* gstate) const;

  void copyJointGroupVelocities(const std::string& joint_group_name, Eigen::VectorXd& values) const
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupVelocities(jmg, values);
  }

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

  void setJointGroupAccelerations(const std::string& joint_group_name, const double* gstate)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupAccelerations(jmg, gstate);
  }

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

  void setJointGroupAccelerations(const JointModelGroup* group, const std::vector<double>& gstate)
  {
    setJointGroupAccelerations(group, &gstate[0]);
  }

  void setJointGroupAccelerations(const JointModelGroup* group, const double* gstate);

  void setJointGroupAccelerations(const std::string& joint_group_name, const Eigen::VectorXd& values)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      setJointGroupAccelerations(jmg, values);
  }

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

  void copyJointGroupAccelerations(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());
      copyJointGroupAccelerations(jmg, &gstate[0]);
    }
  }

  void copyJointGroupAccelerations(const std::string& joint_group_name, double* gstate) const
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupAccelerations(jmg, gstate);
  }

  void copyJointGroupAccelerations(const JointModelGroup* group, std::vector<double>& gstate) const
  {
    gstate.resize(group->getVariableCount());
    copyJointGroupAccelerations(group, &gstate[0]);
  }

  void copyJointGroupAccelerations(const JointModelGroup* group, double* gstate) const;

  void copyJointGroupAccelerations(const std::string& joint_group_name, Eigen::VectorXd& values) const
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
      copyJointGroupAccelerations(jmg, values);
  }

  void copyJointGroupAccelerations(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,
                              const MaxEEFStep& max_step, const JumpThreshold& 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::Vector3d& direction, bool global_reference_frame, double distance,
                              double max_step, double jump_threshold_factor,
                              const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
  {
    return computeCartesianPath(group, traj, link, direction, global_reference_frame, distance,
                                MaxEEFStep(max_step, max_step), JumpThreshold(jump_threshold_factor), validCallback,
                                options);
  }

  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
                              const Eigen::Affine3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
                              const JumpThreshold& 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_factor,
                              const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
  {
    return computeCartesianPath(group, traj, link, target, global_reference_frame, MaxEEFStep(max_step),
                                JumpThreshold(jump_threshold_factor), validCallback, options);
  }

  double computeCartesianPath(const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
                              const EigenSTL::vector_Affine3d& waypoints, bool global_reference_frame,
                              const MaxEEFStep& max_step, const JumpThreshold& 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_factor,
                              const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
                              const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions())
  {
    return computeCartesianPath(group, traj, link, waypoints, global_reference_frame, MaxEEFStep(max_step),
                                JumpThreshold(jump_threshold_factor), validCallback, options);
  }

  static double testJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
                                   const JumpThreshold& jump_threshold);

  static double testRelativeJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
                                           double jump_threshold_factor);

  static double testAbsoluteJointSpaceJump(const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
                                           double revolute_jump_threshold, double prismatic_jump_threshold);

  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 static_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 static_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();
    static_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;

  bool isValidVelocityMove(const RobotState& other, const JointModelGroup* group, double dt) 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();
    static_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();
    static_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();
    static_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();
      markDirtyJointTransforms(mim[i]);
    }
  }

  /* use updateMimicJoints() instead, which also marks joints dirty */
  MOVEIT_DEPRECATED 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();
      // Only mark joint transform dirty, but not the associated link transform
      // as this function is always used in combination of
      // updateMimicJoint(group->getMimicJointModels()) + markDirtyJointTransforms(group);
      dirty_joint_transforms_[mim[i]->getJointIndex()] = 1;
    }
  }

  void updateMimicJoints(const JointModelGroup* group)
  {
    for (const JointModel* jm : group->getMimicJointModels())
    {
      const int fvi = jm->getFirstVariableIndex();
      position_[fvi] = jm->getMimicFactor() * position_[jm->getMimic()->getFirstVariableIndex()] + jm->getMimicOffset();
      markDirtyJointTransforms(jm);
    }
    markDirtyJointTransforms(group);
  }

  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