cartesian_interpolator.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.
 *  Copyright (c) 2019, PickNik 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, Mike Lautman */
 
#pragma once
 
#include <moveit/robot_state/robot_state.h>
 
namespace moveit
{
namespace core
{
struct CartesianPrecision
{
  double translational = 0.001;  //< max deviation in translation (meters)
  double rotational = 0.01;      //< max deviation in rotation (radians)
  double max_resolution = 1e-5;  //< max resolution for waypoints (fraction of total path)
};
 
struct JumpThreshold
{
  JumpThreshold() = default;  // default is equivalent to disabled().
 
  static JumpThreshold disabled();
 
  static JumpThreshold relative(double relative_factor);
 
  static JumpThreshold absolute(double revolute, double prismatic);
 
  double relative_factor = 0.0;
  double revolute = 0.0;   // Radians
  double prismatic = 0.0;  // Meters
 
  // Deprecated constructors. Construct using the builder methods above.
  [[deprecated("Use JumpThreshold::relative() instead.")]] JumpThreshold(double relative_factor);
  [[deprecated("Use JumpThreshold::absolute() instead.")]] JumpThreshold(double revolute, double prismatic);
};
 
struct MaxEEFStep
{
  MaxEEFStep(double translation, double rotation) : translation(translation), rotation(rotation)
  {
  }
 
  MaxEEFStep(double step_size) : translation(step_size), rotation(3.5 * step_size)  // 0.035 rad = 2 deg
  {
  }
 
  double translation;  // Meters
  double rotation;     // Radians
};
 
class CartesianInterpolator
{
  // TODO(mlautman): Eventually, this planner should be moved out of robot_state
 
public:
  struct Percentage
  {
    // value must be in [0,1]
    Percentage(double value) : value(value)
    {
      if (value < 0.0 || value > 1.0)
        throw std::runtime_error("Percentage values must be between 0 and 1, inclusive");
    }
    operator double()
    {
      return value;
    }
    double operator*()
    {
      return value;
    }
    Percentage operator*(const Percentage& p)
    {
      Percentage res(value * p.value);
      return res;
    }
    double value;
  };
 
  struct Distance
  {
    Distance(double meters) : meters(meters)
    {
    }
    operator double()
    {
      return meters;
    }
    double operator*()
    {
      return meters;
    }
    Distance operator*(const Percentage& p)
    {
      Distance res(meters * p.value);
      return res;
    }
    double meters;
  };
 
  static Distance computeCartesianPath(
      const RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
      const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame,
      const MaxEEFStep& max_step, const CartesianPrecision& precision,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  static Distance computeCartesianPath(
      const RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
      const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
      const MaxEEFStep& max_step, const CartesianPrecision& precision,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn())
  {
    return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
                                precision, validCallback, options, cost_function);
  }
 
  static Percentage computeCartesianPath(
      const RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj,
      const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
      const CartesianPrecision& precision, const GroupStateValidityCallbackFn& validCallback,
      const kinematics::KinematicsQueryOptions& options,
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
      const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
 
  static Percentage computeCartesianPath(
      const RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
      const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
      const MaxEEFStep& max_step, const CartesianPrecision& precision,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
      const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
 
  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath(
      RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& path,
      const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame,
      const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());
 
  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Distance computeCartesianPath(
      RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& path,
      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(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn())
  {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
    return computeCartesianPath(start_state, group, path, link, distance * direction, global_reference_frame, max_step,
                                jump_threshold, validCallback, options, cost_function);
#pragma GCC diagnostic pop
  }
 
  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath(
      RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& path,
      const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
      const JumpThreshold& jump_threshold,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
      const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
 
  [[deprecated("Replace JumpThreshold with CartesianPrecision")]] static Percentage computeCartesianPath(
      RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& path,
      const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
      const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
      const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
      const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
      const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
 
  static Percentage checkJointSpaceJump(const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& path,
                                        const JumpThreshold& jump_threshold);
};
 
std::optional<int> hasJointSpaceJump(const std::vector<moveit::core::RobotStatePtr>& waypoints,
                                     const moveit::core::JointModelGroup& group,
                                     const moveit::core::JumpThreshold& jump_threshold);
 
}  // end of namespace core
}  // end of namespace moveit