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 */
 
#pragma once
 
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/attached_body.h>
#include <moveit/transforms/transforms.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <std_msgs/msg/color_rgba.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <cassert>
 
#include <rclcpp/duration.hpp>
 
/* Terminology
   * Model Frame: RobotModel's root frame == PlanningScene's planning frame
     If the SRDF defines a virtual, non-fixed (e.g. floating) joint, this is the parent of this virtual joint.
     Otherwise, it is the root link of the URDF model.
   * Dirty Link Transforms: a caching tool for reducing the frequency of calculating forward kinematics
*/
 
namespace moveit
{
namespace core
{
MOVEIT_CLASS_FORWARD(RobotState);  // Defines RobotStatePtr, ConstPtr, WeakPtr... etc
 
typedef std::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_.data();
  }
 
  const double* getVariablePositions() const
  {
    return position_.data();
  }
 
  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_.data();
  }
 
  const double* getVariableVelocities() const
  {
    return velocity_.data();
  }
 
  void zeroVelocities();
 
  void setVariableVelocities(const double* velocity)
  {
    has_velocity_ = true;
    // assume everything is in order in terms of array lengths (for efficiency reasons)
    memcpy(velocity_.data(), 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];
  }
 
  void dropVelocities();
 
  bool hasAccelerations() const
  {
    return has_acceleration_;
  }
 
  double* getVariableAccelerations()
  {
    markAcceleration();
    return effort_or_acceleration_.data();
  }
 
  const double* getVariableAccelerations() const
  {
    return effort_or_acceleration_.data();
  }
 
  void zeroAccelerations();
 
  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(effort_or_acceleration_.data(), 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();
    effort_or_acceleration_[index] = value;
  }
 
  double getVariableAcceleration(const std::string& variable) const
  {
    return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
  }
 
  double getVariableAcceleration(int index) const
  {
    return effort_or_acceleration_[index];
  }
 
  void dropAccelerations();
 
  bool hasEffort() const
  {
    return has_effort_;
  }
 
  double* getVariableEffort()
  {
    markEffort();
    return effort_or_acceleration_.data();
  }
 
  const double* getVariableEffort() const
  {
    return effort_or_acceleration_.data();
  }
 
  void zeroEffort();
 
  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_or_acceleration_.data(), 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_or_acceleration_[index] = value;
  }
 
  double getVariableEffort(const std::string& variable) const
  {
    return effort_or_acceleration_[robot_model_->getVariableIndex(variable)];
  }
 
  double getVariableEffort(int index) const
  {
    return effort_or_acceleration_[index];
  }
 
  void dropEffort();
 
  void dropDynamics();
 
  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);
 
  void setJointPositions(const std::string& joint_name, const Eigen::Isometry3d& transform)
  {
    setJointPositions(robot_model_->getJointModel(joint_name), transform);
  }
 
  void setJointPositions(const JointModel* joint, const Eigen::Isometry3d& transform);
 
  void setJointVelocities(const JointModel* joint, const double* velocity);
 
  void setJointEfforts(const JointModel* joint, const double* effort);
 
  const double* getJointPositions(const std::string& joint_name) const
  {
    return getJointPositions(robot_model_->getJointModel(joint_name));
  }
 
  // Returns nullptr if `joint` doesn't have any active variables.
  const double* getJointPositions(const JointModel* joint) const;
 
  const double* getJointVelocities(const std::string& joint_name) const
  {
    return getJointVelocities(robot_model_->getJointModel(joint_name));
  }
 
  // Returns nullptr if `joint` doesn't have any active variables.
  const double* getJointVelocities(const JointModel* joint) const;
 
  const double* getJointAccelerations(const std::string& joint_name) const
  {
    return getJointAccelerations(robot_model_->getJointModel(joint_name));
  }
 
  // Returns nullptr if `joint` doesn't have any active variables.
  const double* getJointAccelerations(const JointModel* joint) const;
 
  const double* getJointEffort(const std::string& joint_name) const
  {
    return getJointEffort(robot_model_->getJointModel(joint_name));
  }
 
  // Returns nullptr if `joint` doesn't have any active variables.
  const double* getJointEffort(const JointModel* joint) const;
 
  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)
    {
      assert(gstate.size() == jmg->getVariableCount());
      setJointGroupPositions(jmg, &gstate[0]);
    }
  }
 
  void setJointGroupPositions(const JointModelGroup* group, const std::vector<double>& gstate)
  {
    assert(gstate.size() == group->getVariableCount());
    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)
    {
      assert(values.size() == jmg->getVariableCount());
      setJointGroupPositions(jmg, values);
    }
  }
 
  void setJointGroupPositions(const JointModelGroup* group, const Eigen::VectorXd& values);
 
  void setJointGroupActivePositions(const JointModelGroup* group, const std::vector<double>& gstate);
 
  void setJointGroupActivePositions(const std::string& joint_group_name, const std::vector<double>& gstate)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
    {
      assert(gstate.size() == jmg->getActiveVariableCount());
      setJointGroupActivePositions(jmg, gstate);
    }
  }
 
  void setJointGroupActivePositions(const JointModelGroup* group, const Eigen::VectorXd& values);
 
  void setJointGroupActivePositions(const std::string& joint_group_name, const Eigen::VectorXd& values)
  {
    const JointModelGroup* jmg = robot_model_->getJointModelGroup(joint_group_name);
    if (jmg)
    {
      assert(values.size() == jmg->getActiveVariableCount());
      setJointGroupActivePositions(jmg, 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 setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0,
                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip,
                 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
                 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
                 const std::vector<double>& consistency_limits, double timeout = 0.0,
                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
                 const std::vector<std::string>& tips, double timeout = 0.0,
                 const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIK(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
                 const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
                 double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
                 const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
                 const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  bool setFromIKSubgroups(const JointModelGroup* group, const EigenSTL::vector_Isometry3d& poses,
                          const std::vector<std::string>& tips,
                          const std::vector<std::vector<double>>& consistency_limits, 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::msg::Twist& twist, const std::string& tip,
                     double dt, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn());
 
  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::msg::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);
 
  bool setToDefaultValues(const std::string& group_name, const std::string& state_name)
  {
    const JointModelGroup* jmg = getJointModelGroup(group_name);
    if (jmg)
    {
      return setToDefaultValues(jmg, state_name);
    }
    else
    {
      return false;
    }
  }
 
  void setToRandomPositions();
 
  void setToRandomPositions(const JointModelGroup* group);
 
  void setToRandomPositions(const JointModelGroup* group, random_numbers::RandomNumberGenerator& rng);
 
  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance);
 
  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed, double distance,
                                  random_numbers::RandomNumberGenerator& rng);
 
  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
                                  const std::vector<double>& distances);
 
  void setToRandomPositionsNearBy(const JointModelGroup* group, const RobotState& seed,
                                  const std::vector<double>& distances, random_numbers::RandomNumberGenerator& rng);
 
  void updateCollisionBodyTransforms();
 
  void updateLinkTransforms();
 
  void update(bool force = false);
 
  void updateStateWithLinkAt(const std::string& link_name, const Eigen::Isometry3d& transform, bool backward = false)
  {
    updateStateWithLinkAt(robot_model_->getLinkModel(link_name), transform, backward);
  }
 
  void updateStateWithLinkAt(const LinkModel* link, const Eigen::Isometry3d& transform, bool backward = false);
 
  const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const std::string& frame) const;
 
  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name)
  {
    return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
  }
 
  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link)
  {
    if (!link)
    {
      throw Exception("Invalid link");
    }
    updateLinkTransforms();
    return global_link_transforms_[link->getLinkIndex()];
  }
 
  const Eigen::Isometry3d& getGlobalLinkTransform(const std::string& link_name) const
  {
    return getGlobalLinkTransform(robot_model_->getLinkModel(link_name));
  }
 
  const Eigen::Isometry3d& getGlobalLinkTransform(const LinkModel* link) const
  {
    if (!link)
    {
      throw Exception("Invalid link");
    }
    assert(checkLinkTransforms());
    return global_link_transforms_[link->getLinkIndex()];
  }
 
  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index)
  {
    return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
  }
 
  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index)
  {
    updateCollisionBodyTransforms();
    return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
  }
 
  const Eigen::Isometry3d& getCollisionBodyTransform(const std::string& link_name, std::size_t index) const
  {
    return getCollisionBodyTransform(robot_model_->getLinkModel(link_name), index);
  }
 
  const Eigen::Isometry3d& getCollisionBodyTransform(const LinkModel* link, std::size_t index) const
  {
    assert(checkCollisionTransforms());
    return global_collision_body_transforms_[link->getFirstCollisionBodyTransformIndex() + index];
  }
 
  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name)
  {
    return getJointTransform(robot_model_->getJointModel(joint_name));
  }
 
  const Eigen::Isometry3d& getJointTransform(const JointModel* joint);
 
  const Eigen::Isometry3d& getJointTransform(const std::string& joint_name) const
  {
    return getJointTransform(robot_model_->getJointModel(joint_name));
  }
 
  const Eigen::Isometry3d& getJointTransform(const JointModel* joint) const
  {
    assert(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_.data(), other.getVariablePositions());
  }
 
  double distance(const RobotState& other, const JointModelGroup* joint_group) const;
 
  double distance(const RobotState& other, const JointModel* joint) const;
 
  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;
 
  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);
 
  void harmonizePositions();
  void harmonizePositions(const JointModelGroup* joint_group);
  void harmonizePosition(const JointModel* joint);
 
  void enforceVelocityBounds(const JointModel* joint);
 
  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(std::unique_ptr<AttachedBody> attached_body);
 
  void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
                  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
                  const std::set<std::string>& touch_links, const std::string& link_name,
                  const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(),
                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap());
 
  void attachBody(const std::string& id, const Eigen::Isometry3d& pose,
                  const std::vector<shapes::ShapeConstPtr>& shapes, const EigenSTL::vector_Isometry3d& shape_poses,
                  const std::vector<std::string>& touch_links, const std::string& link_name,
                  const trajectory_msgs::msg::JointTrajectory& detach_posture = trajectory_msgs::msg::JointTrajectory(),
                  const moveit::core::FixedTransformsMap& subframe_poses = moveit::core::FixedTransformsMap())
  {
    std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
    attachBody(id, pose, shapes, shape_poses, touch_links_set, link_name, detach_posture, subframe_poses);
  }
 
  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies) const;
 
  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const JointModelGroup* group) const;
 
  void getAttachedBodies(std::vector<const AttachedBody*>& attached_bodies, const LinkModel* link_model) 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_ = std::make_unique<random_numbers::RandomNumberGenerator>();
    return *rng_;
  }
 
  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr);
 
  const Eigen::Isometry3d& getFrameTransform(const std::string& frame_id, bool* frame_found = nullptr) const;
 
  const Eigen::Isometry3d& getFrameInfo(const std::string& frame_id, const LinkModel*& robot_link,
                                        bool& frame_found) const;
 
  bool knowsFrameTransform(const std::string& frame_id) const;
 
  void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
                       const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::Duration& dur,
                       bool include_attached = false) const;
 
  void getRobotMarkers(visualization_msgs::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
                       const std_msgs::msg::ColorRGBA& color, const std::string& ns, const rclcpp::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::msg::MarkerArray& arr, const std::vector<std::string>& link_names,
                       bool include_attached = false) const;
 
  void getRobotMarkers(visualization_msgs::msg::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 printStatePositionsWithJointLimits(const moveit::core::JointModelGroup* jmg, 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::Isometry3d& transform, std::ostream& out = std::cout) const;
 
  void printDirtyInfo(std::ostream& out = std::cout) const;
 
  std::string getStateTreeString() const;
 
  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const kinematics::KinematicsBaseConstPtr& solver);
 
  bool setToIKSolverFrame(Eigen::Isometry3d& pose, const std::string& ik_frame);
 
private:
  void allocMemory();
  void init();
  void copyFrom(const RobotState& other);
 
  void markDirtyJointTransforms(const JointModel* joint)
  {
    dirty_joint_transforms_[joint->getJointIndex()] = 1;
    dirty_link_transforms_ =
        dirty_link_transforms_ == nullptr ? joint : robot_model_->getCommonRoot(dirty_link_transforms_, joint);
  }
 
  void markDirtyJointTransforms(const JointModelGroup* group)
  {
    for (const JointModel* jm : group->getActiveJointModels())
      dirty_joint_transforms_[jm->getJointIndex()] = 1;
    dirty_link_transforms_ = dirty_link_transforms_ == nullptr ?
                                 group->getCommonRoot() :
                                 robot_model_->getCommonRoot(dirty_link_transforms_, group->getCommonRoot());
  }
 
  void markVelocity();
  void markAcceleration();
  void markEffort();
 
  void updateMimicJoint(const JointModel* joint);
 
  void updateMimicJoints(const JointModelGroup* 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_;
 
  std::vector<double> position_;
  std::vector<double> velocity_;
  std::vector<double> effort_or_acceleration_;
  bool has_velocity_ = false;
  bool has_acceleration_ = false;
  bool has_effort_ = false;
 
  const JointModel* dirty_link_transforms_ = nullptr;
  const JointModel* dirty_collision_body_transforms_ = nullptr;
 
  // All the following transform variables point into aligned memory.
  // They are updated lazily, based on the flags in dirty_joint_transforms_
  // resp. the pointers dirty_link_transforms_ and dirty_collision_body_transforms_
  std::vector<Eigen::Isometry3d> variable_joint_transforms_;  
  std::vector<Eigen::Isometry3d> global_link_transforms_;  
  std::vector<Eigen::Isometry3d> global_collision_body_transforms_;  
  std::vector<unsigned char> dirty_joint_transforms_;
 
  std::map<std::string, std::unique_ptr<AttachedBody>> attached_body_map_;
 
  AttachedBodyCallback attached_body_update_callback_;
 
  std::unique_ptr<random_numbers::RandomNumberGenerator> rng_;
};
 
std::ostream& operator<<(std::ostream& out, const RobotState& s);
}  // namespace core
}  // namespace moveit