joint_state_group.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, 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_ROBOT_STATE_JOINT_STATE_GROUP_
#define MOVEIT_ROBOT_STATE_JOINT_STATE_GROUP_

#include <moveit/robot_model/joint_model_group.h>
#include <moveit/robot_state/link_state.h>
#include <moveit/robot_state/joint_state.h>
#include <sensor_msgs/JointState.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <geometry_msgs/Twist.h>
#include <boost/scoped_ptr.hpp>
#include <boost/function.hpp>

namespace robot_state
{

class JointStateGroup;

typedef boost::function<bool(JointStateGroup *joint_state_group, const std::vector<double> &joint_group_variable_values)> StateValidityCallbackFn;
typedef boost::function<bool(const JointStateGroup *joint_state_group, Eigen::VectorXd &stvector)> SecondaryTaskFn;

class RobotState;

class JointStateGroup
{
public:

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  JointStateGroup(RobotState *state, const robot_model::JointModelGroup *jmg);
  ~JointStateGroup();

  const RobotState *getRobotState() const
  {
    return kinematic_state_;
  }

  RobotState *getRobotState()
  {
    return kinematic_state_;
  }

  const robot_model::JointModelGroup* getJointModelGroup() const
  {
    return joint_model_group_;
  }

  const std::string& getName() const
  {
    return joint_model_group_->getName();
  }

  unsigned int getVariableCount() const
  {
    return joint_model_group_->getVariableCount();
  }

  bool setVariableValues(const std::vector<double>& joint_state_values);

  bool setVariableValues(const Eigen::VectorXd& joint_state_values);

  void setVariableValues(const std::map<std::string, double>& joint_state_map);

  void setVariableValues(const sensor_msgs::JointState& js);

  void updateLinkTransforms();

  bool hasJointState(const std::string &joint) const;

  JointState* getJointState(const std::string &joint) const;

  void getVariableValues(std::vector<double>& joint_state_values) const;

  void getVariableValues(Eigen::VectorXd& joint_state_values) const;

  void getVariableValues(std::map<std::string, double>& joint_state_values) const;

  void setToDefaultValues();

  bool setToDefaultState(const std::string &name);

  void setToRandomValues();

  void setToRandomValuesNearBy(const std::vector<double> &near, const std::map<robot_model::JointModel::JointType, double> &distance_map);

  void setToRandomValuesNearBy(const std::vector<double> &near, const std::vector<double> &distances);

  bool satisfiesBounds(double margin = 0.0) const;

  void enforceBounds();

  double infinityNormDistance(const JointStateGroup *other) const;

  double distance(const JointStateGroup *other) const;

  std::pair<double, int> getMinDistanceToBounds() const;

  void interpolate(const JointStateGroup *to, const double t, JointStateGroup *dest) const;

  const std::vector<JointState*>& getJointRoots() const
  {
    return joint_roots_;
  }

  const std::vector<std::string>& getJointNames() const
  {
    return joint_model_group_->getJointModelNames();
  }

  const std::vector<JointState*>& getJointStateVector() const
  {
    return joint_state_vector_;
  }

  random_numbers::RandomNumberGenerator& getRandomNumberGenerator();

  bool getJacobian(const std::string &link_name, const Eigen::Vector3d &reference_point_position, Eigen::MatrixXd& jacobian,
                   bool use_quaterion_representation = false) const;

  double getDefaultIKTimeout() const
  {
    return joint_model_group_->getDefaultIKTimeout();
  }

  unsigned getDefaultIKAttempts() const
  {
    return joint_model_group_->getDefaultIKAttempts();
  }

  bool setFromIK(const geometry_msgs::Pose &pose, const std::string &tip, unsigned int attempts = 0, double timeout = 0.0,
                 const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const geometry_msgs::Pose &pose, unsigned int attempts = 0, double timeout = 0.0, const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const geometry_msgs::Pose &pose, unsigned int attempts = 0, double timeout = 0.0, const StateValidityCallbackFn &constraint =  StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const Eigen::Affine3d &pose, unsigned int attempts = 0, double timeout = 0.0,
                 const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const Eigen::Affine3d &pose_in, const std::string &tip_in, unsigned int attempts, double timeout, const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const Eigen::Affine3d &pose_in, unsigned int attempts, double timeout, const kinematics::KinematicsQueryOptions &options);

  bool setFromIK(const Eigen::Affine3d &pose, const std::string &tip, unsigned int attempts = 0, double timeout = 0.0,
                 const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const Eigen::Affine3d &pose, const std::string &tip,
                 const std::vector<double> &consistency_limits, unsigned int attempts = 0, double timeout = 0.0,
                 const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(const EigenSTL::vector_Affine3d &poses, const std::vector<std::string> &tips, unsigned int attempts = 0, double timeout = 0.0, const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromIK(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 StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  bool setFromDiffIK(const Eigen::VectorXd &twist, const std::string &tip, double dt, const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const SecondaryTaskFn &st = SecondaryTaskFn());

  bool setFromDiffIK(const geometry_msgs::Twist &twist, const std::string &tip, double dt, const StateValidityCallbackFn &constraint = StateValidityCallbackFn(), const SecondaryTaskFn &st = SecondaryTaskFn());

  void computeJointVelocity(Eigen::VectorXd &qdot, const Eigen::VectorXd &twist, const std::string &tip, const SecondaryTaskFn &st = SecondaryTaskFn()) const;

  bool integrateJointVelocity(const Eigen::VectorXd &qdot, double dt, const StateValidityCallbackFn &constraint = StateValidityCallbackFn());

  bool avoidJointLimitsSecondaryTask(const JointStateGroup *joint_state_group, Eigen::VectorXd &stvector,
                                     double activation_threshold, double gain) const;

  double computeCartesianPath(std::vector<boost::shared_ptr<RobotState> > &traj, const std::string &link_name, const Eigen::Vector3d &direction, bool global_reference_frame,
                              double distance, double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  double computeCartesianPath(std::vector<boost::shared_ptr<RobotState> > &traj, const std::string &link_name, const Eigen::Affine3d &target, bool global_reference_frame,
                              double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  double computeCartesianPath(std::vector<boost::shared_ptr<RobotState> > &traj, const std::string &link_name, const EigenSTL::vector_Affine3d &waypoints,
                              bool global_reference_frame, double max_step, double jump_threshold, const StateValidityCallbackFn &validCallback = StateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions());

  JointStateGroup& operator=(const JointStateGroup &other);

private:

  void copyFrom(const JointStateGroup &other_jsg);

  void ikCallbackFnAdapter(const StateValidityCallbackFn &constraint, const geometry_msgs::Pose &ik_pose,
                           const std::vector<double> &ik_sol, moveit_msgs::MoveItErrorCodes &error_code);

  RobotState *kinematic_state_;

  const robot_model::JointModelGroup     *joint_model_group_;

  std::vector<JointState*>                joint_state_vector_;

  std::map<std::string, JointState*>      joint_state_map_;

  std::vector<JointState*>                joint_roots_;

  std::vector<LinkState*>                 updated_links_;

  boost::scoped_ptr<random_numbers::RandomNumberGenerator> rng_;

};
}

#endif