<|start_filename|>moveit_ros/benchmarks/src/simple_benchmarks/CombinePredefinedPosesBenchmark.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik LLC
 *  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 the PickNik LLC 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: <NAME> */
/* Description: A simple benchmark that plans trajectories for all combinations of specified predefined poses */

// MoveIt Benchmark
#include <moveit/benchmarks/BenchmarkOptions.h>
#include <moveit/benchmarks/BenchmarkExecutor.h>

// MoveIt
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/robot_state/conversions.h>

namespace moveit_ros_benchmarks
{
constexpr char LOGNAME[] = "combine_predefined_poses_benchmark";
class CombinePredefinedPosesBenchmark : public BenchmarkExecutor
{
public:
  bool loadBenchmarkQueryData(const BenchmarkOptions& opts, moveit_msgs::PlanningScene& scene_msg,
                              std::vector<StartState>& start_states, std::vector<PathConstraints>& path_constraints,
                              std::vector<PathConstraints>& goal_constraints,
                              std::vector<TrajectoryConstraints>& traj_constraints,
                              std::vector<BenchmarkRequest>& queries) override
  {
    // Load planning scene
    if (!psm_)
      psm_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
    if (!psm_->newPlanningSceneMessage(scene_msg))
    {
      ROS_ERROR_NAMED(LOGNAME, "Failed to load planning scene");
      return false;
    }

    // Load robot model
    if (!psm_->getRobotModel())
    {
      ROS_ERROR_NAMED(LOGNAME, "Failed to load robot model");
      return false;
    }

    // Select planning group to use for predefined poses
    std::string predefined_poses_group = opts.getPredefinedPosesGroup();
    if (predefined_poses_group.empty())
    {
      ROS_WARN_NAMED(LOGNAME, "Parameter predefined_poses_group is not set, using default planning group instead");
      predefined_poses_group = opts.getGroupName();
    }
    const auto& joint_model_group = psm_->getRobotModel()->getJointModelGroup(predefined_poses_group);
    if (!joint_model_group)
    {
      ROS_ERROR_STREAM_NAMED(LOGNAME, "Robot model has no joint model group named '" << predefined_poses_group << "'");
      return false;
    }

    // Iterate over all predefined poses and use each as start and goal states
    moveit::core::RobotState robot_state(psm_->getRobotModel());
    start_states.clear();
    goal_constraints.clear();
    for (const auto& pose_id : opts.getPredefinedPoses())
    {
      if (!robot_state.setToDefaultValues(joint_model_group, pose_id))
      {
        ROS_WARN_STREAM_NAMED(LOGNAME, "Failed to set robot state to named target '" << pose_id << "'");
        continue;
      }
      // Create start state
      start_states.emplace_back();
      start_states.back().name = pose_id;
      moveit::core::robotStateToRobotStateMsg(robot_state, start_states.back().state);

      // Create goal constraints
      goal_constraints.emplace_back();
      goal_constraints.back().name = pose_id;
      goal_constraints.back().constraints.push_back(
          kinematic_constraints::constructGoalConstraints(robot_state, joint_model_group));
    }
    if (start_states.empty() || goal_constraints.empty())
    {
      ROS_ERROR_NAMED(LOGNAME, "Failed to init start and goal states from predefined_poses");
      return false;
    }

    // We don't use path/trajectory constraints or custom queries
    path_constraints.clear();
    traj_constraints.clear();
    queries.clear();
    return true;
  }

private:
  planning_scene_monitor::PlanningSceneMonitorPtr psm_;
};
}  // namespace moveit_ros_benchmarks

int main(int argc, char** argv)
{
  ros::init(argc, argv, "moveit_run_benchmark");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // Read benchmark options from param server
  moveit_ros_benchmarks::BenchmarkOptions opts(ros::this_node::getName());
  // Setup benchmark server
  moveit_ros_benchmarks::CombinePredefinedPosesBenchmark server;

  std::vector<std::string> planning_pipelines;
  opts.getPlanningPipelineNames(planning_pipelines);
  server.initialize(planning_pipelines);

  // Running benchmarks
  if (!server.runBenchmarks(opts))
    ROS_ERROR("Failed to run all benchmarks");
}


<|start_filename|>moveit_core/planning_scene/test/test_collision_objects.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, Universitaet Hamburg
 *  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 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: Michael 'v4hn' Goerner */

#include <gtest/gtest.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/utils/message_checks.h>
#include <moveit/utils/robot_model_test_utils.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <sstream>
#include <string>
#include <boost/filesystem/path.hpp>
#include <ros/package.h>
#include <tf2_eigen/tf2_eigen.h>

namespace
{
void makeSphere(moveit_msgs::CollisionObject& co)
{
  // set valid primitive
  shape_msgs::SolidPrimitive primitive;
  primitive.type = shape_msgs::SolidPrimitive::SPHERE;
  primitive.dimensions.push_back(/* SPHERE_RADIUS */ 1.0);
  co.primitives.push_back(primitive);
  geometry_msgs::Pose pose;
  pose.orientation.w = 1.0;
  co.primitive_poses.push_back(pose);
}
}  // namespace

TEST(PlanningScene, fillInObjectPoseFromPrimitive)
{
  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
  planning_scene::PlanningScene scene(robot_model);

  moveit_msgs::CollisionObject co;
  co.header.frame_id = robot_model->getModelFrame();
  co.id = "object_no_pose";
  co.operation = moveit_msgs::CollisionObject::ADD;
  makeSphere(co);
  scene.processCollisionObjectMsg(co);

  Eigen::Isometry3d primitive_pose;
  tf2::fromMsg(co.primitive_poses.at(0), primitive_pose);
  ASSERT_TRUE(scene.knowsFrameTransform(co.id)) << "failed to add object";
  EXPECT_TRUE(scene.getFrameTransform(co.id).isApprox(primitive_pose))
      << "scene did not use only primitive pose as object pose";
}

TEST(PlanningScene, fillInPrimitivePose)
{
  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
  planning_scene::PlanningScene scene(robot_model);

  moveit_msgs::CollisionObject co;
  co.header.frame_id = robot_model->getModelFrame();
  co.id = "object_no_primitive_pose";
  co.operation = moveit_msgs::CollisionObject::ADD;
  makeSphere(co);
  co.pose = co.primitive_poses.at(0);
  co.primitive_poses.resize(0);
  scene.processCollisionObjectMsg(co);

  Eigen::Isometry3d object_pose;
  tf2::fromMsg(co.pose, object_pose);
  ASSERT_TRUE(scene.knowsFrameTransform(co.id)) << "failed to add object";
  EXPECT_TRUE(scene.getFrameTransform(co.id).isApprox(object_pose))
      << "scene did not implicitly fill in identity pose for only primitive";
}

TEST(PlanningScene, rememberMetadataWhenAttached)
{
  moveit::core::RobotModelPtr robot_model(moveit::core::RobotModelBuilder("empty_robot", "base_link").build());
  planning_scene::PlanningScene scene(robot_model);

  // prepare planning scene message to add a colored object
  moveit_msgs::PlanningScene scene_msg;
  scene_msg.robot_model_name = robot_model->getName();
  scene_msg.is_diff = true;
  scene_msg.robot_state.is_diff = true;

  moveit_msgs::CollisionObject co;
  co.header.frame_id = robot_model->getModelFrame();
  co.id = "blue_sphere";
  co.operation = moveit_msgs::CollisionObject::ADD;
  co.pose.orientation.w = 1.0;
  makeSphere(co);

  // meta-data 1: object type
  co.type.key = "blue_sphere_type";
  co.type.db = "{'type':'CustomDB'}";
  scene_msg.world.collision_objects.push_back(co);

  // meta-data 2: object color
  moveit_msgs::ObjectColor color;
  color.id = co.id;
  color.color.b = 1.0;
  color.color.a = 1.0;
  scene_msg.object_colors.push_back(color);

  EXPECT_FALSE(scene.hasObjectColor(co.id)) << "scene knows color before adding it(?)";
  EXPECT_FALSE(scene.hasObjectType(co.id)) << "scene knows type before adding it(?)";

  // add object to scene
  scene.usePlanningSceneMsg(scene_msg);

  EXPECT_TRUE(scene.hasObjectColor(co.id)) << "scene failed to add object color";
  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene added wrong object color";
  EXPECT_TRUE(scene.hasObjectType(co.id)) << "scene failed to add object type";
  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene added wrong object type";

  // attach object
  moveit_msgs::AttachedCollisionObject aco;
  aco.object.operation = moveit_msgs::CollisionObject::ADD;
  aco.object.id = co.id;
  aco.link_name = robot_model->getModelFrame();
  scene.processAttachedCollisionObjectMsg(aco);

  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot object color after it got attached";
  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot object type after it got attached";

  // trying to remove object from the scene while it is attached is expected to fail
  co.operation = moveit_msgs::CollisionObject::REMOVE;
  EXPECT_FALSE(scene.processCollisionObjectMsg(co))
      << "scene removed attached object from collision world (although it's not there)";

  // detach again right away
  aco.object.operation = moveit_msgs::CollisionObject::REMOVE;
  scene.processAttachedCollisionObjectMsg(aco);

  EXPECT_EQ(scene.getObjectColor(co.id), color.color) << "scene forgot specified color after attach/detach";
  EXPECT_EQ(scene.getObjectType(co.id), co.type) << "scene forgot specified type after attach/detach";
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


<|start_filename|>moveit_core/robot_model/src/pyrobot_model.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <moveit/robot_model/robot_model.h>
#include <urdf_model/model.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>

namespace py = pybind11;
using namespace robot_model;

void def_robot_model_bindings(py::module& m)
{
  m.doc() = "Definition of a kinematic model. Not thread safe, however multiple instances can be created.";
  py::class_<RobotModel, RobotModelPtr>(m, "RobotModel")
      .def(py::init<const urdf::ModelInterfaceSharedPtr&, const srdf::ModelConstSharedPtr&>(), py::arg("urdf_model"),
           py::arg("srdf_model"))
      .def("getName", &RobotModel::getName)
      .def("getLinkModelNames", &RobotModel::getLinkModelNames)
      .def("getJointModelNames", &RobotModel::getJointModelNames)
      //
      ;

  py::class_<JointModelGroup, JointModelGroupPtr>(m, "JointModelGroup")
      .def("getLinkModelNames", &JointModelGroup::getLinkModelNames)
      .def("getJointModelNames", &JointModelGroup::getJointModelNames)
      //
      ;
}


<|start_filename|>moveit_core/robot_model/src/floating_joint_model.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, <NAME>
 *  Copyright (c) 2008-2013, <NAME>, 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 the Willow Garage 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: <NAME> */

#include <moveit/robot_model/floating_joint_model.h>
#include <geometric_shapes/check_isometry.h>
#include <boost/math/constants/constants.hpp>
#include <ros/console.h>
#include <limits>
#include <cmath>

namespace moveit
{
namespace core
{
namespace
{
constexpr size_t STATE_SPACE_DIMENSION = 7;

}  // namespace

FloatingJointModel::FloatingJointModel(const std::string& name) : JointModel(name), angular_distance_weight_(1.0)
{
  type_ = FLOATING;
  local_variable_names_.push_back("trans_x");
  local_variable_names_.push_back("trans_y");
  local_variable_names_.push_back("trans_z");
  local_variable_names_.push_back("rot_x");
  local_variable_names_.push_back("rot_y");
  local_variable_names_.push_back("rot_z");
  local_variable_names_.push_back("rot_w");
  for (size_t i = 0; i < STATE_SPACE_DIMENSION; ++i)
  {
    variable_names_.push_back(name_ + "/" + local_variable_names_[i]);
    variable_index_map_[variable_names_.back()] = i;
  }

  variable_bounds_.resize(7);

  variable_bounds_[0].position_bounded_ = true;
  variable_bounds_[1].position_bounded_ = true;
  variable_bounds_[2].position_bounded_ = true;
  variable_bounds_[3].position_bounded_ = true;
  variable_bounds_[4].position_bounded_ = true;
  variable_bounds_[5].position_bounded_ = true;
  variable_bounds_[6].position_bounded_ = true;

  variable_bounds_[0].min_position_ = -std::numeric_limits<double>::infinity();
  variable_bounds_[0].max_position_ = std::numeric_limits<double>::infinity();
  variable_bounds_[1].min_position_ = -std::numeric_limits<double>::infinity();
  variable_bounds_[1].max_position_ = std::numeric_limits<double>::infinity();
  variable_bounds_[2].min_position_ = -std::numeric_limits<double>::infinity();
  variable_bounds_[2].max_position_ = std::numeric_limits<double>::infinity();
  variable_bounds_[3].min_position_ = -1.0;
  variable_bounds_[3].max_position_ = 1.0;
  variable_bounds_[4].min_position_ = -1.0;
  variable_bounds_[4].max_position_ = 1.0;
  variable_bounds_[5].min_position_ = -1.0;
  variable_bounds_[5].max_position_ = 1.0;
  variable_bounds_[6].min_position_ = -1.0;
  variable_bounds_[6].max_position_ = 1.0;

  computeVariableBoundsMsg();
}

double FloatingJointModel::getMaximumExtent(const Bounds& other_bounds) const
{
  double dx = other_bounds[0].max_position_ - other_bounds[0].min_position_;
  double dy = other_bounds[1].max_position_ - other_bounds[1].min_position_;
  double dz = other_bounds[2].max_position_ - other_bounds[2].min_position_;
  return sqrt(dx * dx + dy * dy + dz * dz) + boost::math::constants::pi<double>() * 0.5 * angular_distance_weight_;
}

double FloatingJointModel::distance(const double* values1, const double* values2) const
{
  return distanceTranslation(values1, values2) + angular_distance_weight_ * distanceRotation(values1, values2);
}

double FloatingJointModel::distanceTranslation(const double* values1, const double* values2) const
{
  double dx = values1[0] - values2[0];
  double dy = values1[1] - values2[1];
  double dz = values1[2] - values2[2];
  return sqrt(dx * dx + dy * dy + dz * dz);
}

double FloatingJointModel::distanceRotation(const double* values1, const double* values2) const
{
  double dq =
      fabs(values1[3] * values2[3] + values1[4] * values2[4] + values1[5] * values2[5] + values1[6] * values2[6]);
  if (dq + std::numeric_limits<double>::epsilon() >= 1.0)
    return 0.0;
  else
    return acos(dq);
}

void FloatingJointModel::interpolate(const double* from, const double* to, const double t, double* state) const
{
  // interpolate position
  state[0] = from[0] + (to[0] - from[0]) * t;
  state[1] = from[1] + (to[1] - from[1]) * t;
  state[2] = from[2] + (to[2] - from[2]) * t;

  double dq = fabs(from[3] * to[3] + from[4] * to[4] + from[5] * to[5] + from[6] * to[6]);
  double theta = (dq + std::numeric_limits<double>::epsilon() >= 1.0) ? 0.0 : acos(dq);
  if (theta > std::numeric_limits<double>::epsilon())
  {
    double d = 1.0 / sin(theta);
    double s0 = sin((1.0 - t) * theta);
    double s1 = sin(t * theta);
    if (dq < 0)  // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
      s1 = -s1;
    state[3] = (from[3] * s0 + to[3] * s1) * d;
    state[4] = (from[4] * s0 + to[4] * s1) * d;
    state[5] = (from[5] * s0 + to[5] * s1) * d;
    state[6] = (from[6] * s0 + to[6] * s1) * d;
  }
  else
  {
    state[3] = from[3];
    state[4] = from[4];
    state[5] = from[5];
    state[6] = from[6];
  }
}

bool FloatingJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const
{
  if (values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin)
    return false;
  if (values[1] < bounds[1].min_position_ - margin || values[1] > bounds[1].max_position_ + margin)
    return false;
  if (values[2] < bounds[2].min_position_ - margin || values[2] > bounds[2].max_position_ + margin)
    return false;
  double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
  return fabs(norm_sqr - 1.0) <= std::numeric_limits<float>::epsilon() * 10.0;
}

bool FloatingJointModel::normalizeRotation(double* values) const
{
  // normalize the quaternion if we need to
  double norm_sqr = values[3] * values[3] + values[4] * values[4] + values[5] * values[5] + values[6] * values[6];
  if (fabs(norm_sqr - 1.0) > std::numeric_limits<double>::epsilon() * 100.0)
  {
    double norm = sqrt(norm_sqr);
    if (norm < std::numeric_limits<double>::epsilon() * 100.0)
    {
      ROS_WARN_NAMED("robot_model", "Quaternion is zero in RobotState representation. Setting to identity");
      values[3] = 0.0;
      values[4] = 0.0;
      values[5] = 0.0;
      values[6] = 1.0;
    }
    else
    {
      values[3] /= norm;
      values[4] /= norm;
      values[5] /= norm;
      values[6] /= norm;
    }
    return true;
  }
  else
    return false;
}

unsigned int FloatingJointModel::getStateSpaceDimension() const
{
  return STATE_SPACE_DIMENSION;
}

bool FloatingJointModel::enforcePositionBounds(double* values, const Bounds& bounds) const
{
  bool result = normalizeRotation(values);
  for (unsigned int i = 0; i < 3; ++i)
  {
    if (values[i] < bounds[i].min_position_)
    {
      values[i] = bounds[i].min_position_;
      result = true;
    }
    else if (values[i] > bounds[i].max_position_)
    {
      values[i] = bounds[i].max_position_;
      result = true;
    }
  }
  return result;
}

void FloatingJointModel::computeTransform(const double* joint_values, Eigen::Isometry3d& transf) const
{
  transf = Eigen::Isometry3d(
      Eigen::Translation3d(joint_values[0], joint_values[1], joint_values[2]) *
      Eigen::Quaterniond(joint_values[6], joint_values[3], joint_values[4], joint_values[5]).normalized());
}

void FloatingJointModel::computeVariablePositions(const Eigen::Isometry3d& transf, double* joint_values) const
{
  joint_values[0] = transf.translation().x();
  joint_values[1] = transf.translation().y();
  joint_values[2] = transf.translation().z();
  ASSERT_ISOMETRY(transf)  // unsanitized input, could contain non-isometry
  Eigen::Quaterniond q(transf.linear());
  joint_values[3] = q.x();
  joint_values[4] = q.y();
  joint_values[5] = q.z();
  joint_values[6] = q.w();
}

void FloatingJointModel::getVariableDefaultPositions(double* values, const Bounds& bounds) const
{
  for (unsigned int i = 0; i < 3; ++i)
  {
    // if zero is a valid value
    if (bounds[i].min_position_ <= 0.0 && bounds[i].max_position_ >= 0.0)
      values[i] = 0.0;
    else
      values[i] = (bounds[i].min_position_ + bounds[i].max_position_) / 2.0;
  }

  values[3] = 0.0;
  values[4] = 0.0;
  values[5] = 0.0;
  values[6] = 1.0;
}

void FloatingJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values,
                                                    const Bounds& bounds) const
{
  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
    values[0] = 0.0;
  else
    values[0] = rng.uniformReal(bounds[0].min_position_, bounds[0].max_position_);
  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
    values[1] = 0.0;
  else
    values[1] = rng.uniformReal(bounds[1].min_position_, bounds[1].max_position_);
  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
    values[2] = 0.0;
  else
    values[2] = rng.uniformReal(bounds[2].min_position_, bounds[2].max_position_);

  double q[4];
  rng.quaternion(q);
  values[3] = q[0];
  values[4] = q[1];
  values[5] = q[2];
  values[6] = q[3];
}

void FloatingJointModel::getVariableRandomPositionsNearBy(random_numbers::RandomNumberGenerator& rng, double* values,
                                                          const Bounds& bounds, const double* seed,
                                                          const double distance) const
{
  if (bounds[0].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[0].min_position_ <= -std::numeric_limits<double>::infinity())
    values[0] = 0.0;
  else
    values[0] = rng.uniformReal(std::max(bounds[0].min_position_, seed[0] - distance),
                                std::min(bounds[0].max_position_, seed[0] + distance));
  if (bounds[1].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[1].min_position_ <= -std::numeric_limits<double>::infinity())
    values[1] = 0.0;
  else
    values[1] = rng.uniformReal(std::max(bounds[1].min_position_, seed[1] - distance),
                                std::min(bounds[1].max_position_, seed[1] + distance));
  if (bounds[2].max_position_ >= std::numeric_limits<double>::infinity() ||
      bounds[2].min_position_ <= -std::numeric_limits<double>::infinity())
    values[2] = 0.0;
  else
    values[2] = rng.uniformReal(std::max(bounds[2].min_position_, seed[2] - distance),
                                std::min(bounds[2].max_position_, seed[2] + distance));

  double da = angular_distance_weight_ * distance;
  if (da >= .25 * boost::math::constants::pi<double>())
  {
    double q[4];
    rng.quaternion(q);
    values[3] = q[0];
    values[4] = q[1];
    values[5] = q[2];
    values[6] = q[3];
  }
  else
  {
    // taken from OMPL
    // sample angle & axis
    double ax = rng.gaussian01();
    double ay = rng.gaussian01();
    double az = rng.gaussian01();
    double angle = 2.0 * pow(rng.uniform01(), 1.0 / 3.0) * da;
    // convert to quaternion
    double q[4];
    double norm = sqrt(ax * ax + ay * ay + az * az);
    if (norm < 1e-6)
    {
      q[0] = q[1] = q[2] = 0.0;
      q[3] = 1.0;
    }
    else
    {
      double s = sin(angle / 2.0);
      q[0] = s * ax / norm;
      q[1] = s * ay / norm;
      q[2] = s * az / norm;
      q[3] = cos(angle / 2.0);
    }
    // multiply quaternions: near * q
    values[3] = seed[6] * q[0] + seed[3] * q[3] + seed[4] * q[2] - seed[5] * q[1];
    values[4] = seed[6] * q[1] + seed[4] * q[3] + seed[5] * q[0] - seed[3] * q[2];
    values[5] = seed[6] * q[2] + seed[5] * q[3] + seed[3] * q[1] - seed[4] * q[0];
    values[6] = seed[6] * q[3] - seed[3] * q[0] - seed[4] * q[1] - seed[5] * q[2];
  }
}

}  // end of namespace core
}  // end of namespace moveit


<|start_filename|>moveit_core/macros/include/moveit/macros/declare_ptr.h<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2016, Delft Robotics B.V.
 *  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 the copyright holder 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.
 *********************************************************************/

#pragma once

#include <memory>

/**
 * \def MOVEIT_DELCARE_PTR
 * Macro that given a Name and a Type declares the following types:
 * - ${Name}Ptr            = shared_ptr<${Type}>
 * - ${Name}ConstPtr       = shared_ptr<const ${Type}>
 * - ${Name}WeakPtr        = weak_ptr<${Type}>
 * - ${Name}ConstWeakPtr   = weak_ptr<const ${Type}>
 * - ${Name}UniquePtr      = unique_ptr<${Type}>
 * - ${Name}ConstUniquePtr = unique_ptr<const ${Type}>
 *
 * For best portability the exact type of shared_ptr declared by the macro
 * should be considered to be an implementation detail, liable to change in
 * future releases.
 */

#define MOVEIT_DECLARE_PTR(Name, Type)                                                                                 \
  typedef std::shared_ptr<Type> Name##Ptr;                                                                             \
  typedef std::shared_ptr<const Type> Name##ConstPtr;                                                                  \
  typedef std::weak_ptr<Type> Name##WeakPtr;                                                                           \
  typedef std::weak_ptr<const Type> Name##ConstWeakPtr;                                                                \
  typedef std::unique_ptr<Type> Name##UniquePtr;                                                                       \
  typedef std::unique_ptr<const Type> Name##ConstUniquePtr

/**
 * \def MOVEIT_DELCARE_PTR_MEMBER
 * The macro defines the same typedefs as MOVEIT_DECLARE_PTR, but shortens the new names to their suffix.
 *
 * This can be used to create `Classname::Ptr` style names, but in most situations in MoveIt's codebase,
 * MOVEIT_CLASS_FORWARD and MOVEIT_DECLARE_PTR should be preferred.
 */

#define MOVEIT_DECLARE_PTR_MEMBER(Type)                                                                                \
  typedef std::shared_ptr<Type> Ptr;                                                                                   \
  typedef std::shared_ptr<const Type> ConstPtr;                                                                        \
  typedef std::weak_ptr<Type> WeakPtr;                                                                                 \
  typedef std::weak_ptr<const Type> ConstWeakPtr;                                                                      \
  typedef std::unique_ptr<Type> UniquePtr;                                                                             \
  typedef std::unique_ptr<const Type> ConstUniquePtr


<|start_filename|>moveit_core/collision_detection/src/pycollision_detection.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/eigen.h>
#include <moveit/collision_detection/world.h>
#include <moveit/collision_detection/collision_matrix.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>

namespace py = pybind11;

using namespace collision_detection;

void def_collision_detection_bindings(py::module& m)
{
  m.doc() = "contains collision detection, the world, and allowed collision matrices";
  py::enum_<BodyType>(m, "BodyType")
      .value("ROBOT_ATTACHED", BodyType::ROBOT_ATTACHED)
      .value("ROBOT_LINK", BodyType::ROBOT_LINK)
      .value("WORLD_OBJECT", BodyType::WORLD_OBJECT)
      .export_values();
  py::class_<Contact>(m, "Contact")
      .def(py::init<>())
      .def_readwrite("body_name_1", &Contact::body_name_1)
      .def_readwrite("body_name_2", &Contact::body_name_2)
      .def_readwrite("body_type_1", &Contact::body_type_1)
      .def_readwrite("body_type_2", &Contact::body_type_2)
      .def_readwrite("depth", &Contact::depth)
      .def_property_readonly("nearest_points",
                             [](const Contact& contact) {
                               std::vector<Eigen::Vector3d> v{ contact.nearest_points[0], contact.nearest_points[1] };
                               return v;
                             })
      .def_readwrite("normal", &Contact::normal)
      .def_readwrite("percent_interpolation", &Contact::percent_interpolation)
      .def_readwrite("pos", &Contact::pos)
      //
      ;
  py::class_<CollisionRequest>(m, "CollisionRequest")
      .def(py::init<>())
      .def_readwrite("contacts", &CollisionRequest::contacts)
      .def_readwrite("cost", &CollisionRequest::cost)
      .def_readwrite("distance", &CollisionRequest::distance)
      .def_readwrite("group_name", &CollisionRequest::group_name)
      .def_readwrite("is_done", &CollisionRequest::is_done)
      .def_readwrite("max_contacts", &CollisionRequest::max_contacts)
      .def_readwrite("max_contacts_per_pair", &CollisionRequest::max_contacts_per_pair)
      .def_readwrite("max_cost_sources", &CollisionRequest::max_cost_sources)
      .def_readwrite("verbose", &CollisionRequest::verbose)
      //
      ;
  py::class_<CollisionResult>(m, "CollisionResult")
      .def(py::init<>())
      .def_readwrite("collision", &CollisionResult::collision)
      .def_readwrite("contact_count", &CollisionResult::contact_count)
      .def_readwrite("contacts", &CollisionResult::contacts)
      .def_readwrite("cost_sources", &CollisionResult::cost_sources)
      .def_readwrite("distance", &CollisionResult::distance)
      .def("clear", &CollisionResult::clear)
      //
      ;
  py::class_<AllowedCollisionMatrix>(m, "AllowedCollisionMatrix")
      .def(py::init<>())
      .def("setEntry",
           py::overload_cast<const std::string&, const std::string&, bool>(&AllowedCollisionMatrix::setEntry))
      //
      ;
  py::class_<World, WorldPtr>(m, "World").def(py::init<>());
}


<|start_filename|>moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, <NAME>, 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 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: <NAME> */

#include "moveit/py_bindings_tools/roscpp_initializer.h"
#include "moveit/py_bindings_tools/py_conversions.h"
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <memory>

static std::vector<std::string>& ROScppArgs()
{
  static std::vector<std::string> args;
  return args;
}

static std::string& ROScppNodeName()
{
  static std::string node_name("moveit_python_wrappers");
  return node_name;
}

void moveit::py_bindings_tools::roscpp_set_arguments(const std::string& node_name, boost::python::list& argv)
{
  ROScppNodeName() = node_name;
  ROScppArgs() = stringFromList(argv);
}

namespace
{
struct InitProxy
{
  InitProxy()
  {
    const std::vector<std::string>& args = ROScppArgs();
    int fake_argc = args.size();
    char** fake_argv = new char*[args.size()];
    for (std::size_t i = 0; i < args.size(); ++i)
      fake_argv[i] = strdup(args[i].c_str());

    ros::init(fake_argc, fake_argv, ROScppNodeName(),
              ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
    for (int i = 0; i < fake_argc; ++i)
      delete[] fake_argv[i];
    delete[] fake_argv;
  }

  ~InitProxy()
  {
    if (ros::isInitialized() && !ros::isShuttingDown())
      ros::shutdown();
  }
};
}  // namespace

static void roscpp_init_or_stop(bool init)
{
  // ensure we do not accidentally initialize ROS multiple times per process
  static boost::mutex lock;
  boost::mutex::scoped_lock slock(lock);

  // once per process, we start a spinner
  static bool once = true;
  static std::unique_ptr<InitProxy> proxy;
  static std::unique_ptr<ros::AsyncSpinner> spinner;

  // initialize only once
  if (once && init)
  {
    once = false;

    // if ROS (cpp) is not initialized, we initialize it
    if (!ros::isInitialized())
    {
      proxy = std::make_unique<InitProxy>();
      spinner = std::make_unique<ros::AsyncSpinner>(1);
      spinner->start();
    }
  }

  // shutdown if needed
  if (!init)
  {
    once = false;
    proxy.reset();
    spinner.reset();
  }
}

void moveit::py_bindings_tools::roscpp_init()
{
  roscpp_init_or_stop(true);
}

void moveit::py_bindings_tools::roscpp_init(const std::string& node_name, boost::python::list& argv)
{
  roscpp_set_arguments(node_name, argv);
  roscpp_init();
}

void moveit::py_bindings_tools::roscpp_init(boost::python::list& argv)
{
  ROScppArgs() = stringFromList(argv);
  roscpp_init();
}

void moveit::py_bindings_tools::roscpp_shutdown()
{
  roscpp_init_or_stop(false);
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer()
{
  roscpp_init();
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(boost::python::list& argv)
{
  roscpp_init(argv);
}

moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(const std::string& node_name, boost::python::list& argv)
{
  roscpp_init(node_name, argv);
}


<|start_filename|>moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  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 Pilz GmbH & Co. KG 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.
 *********************************************************************/

#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h"

#include <algorithm>
#include <memory>
#include <math.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit/planning_interface/planning_interface.h>

bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blend(
    const planning_scene::PlanningSceneConstPtr& planning_scene,
    const pilz_industrial_motion_planner::TrajectoryBlendRequest& req,
    pilz_industrial_motion_planner::TrajectoryBlendResponse& res)
{
  ROS_INFO("Start trajectory blending using transition window.");

  double sampling_time = 0.;
  if (!validateRequest(req, sampling_time, res.error_code))
  {
    ROS_ERROR("Trajectory blend request is not valid.");
    return false;
  }

  // search for intersection points of the two trajectories with the blending
  // sphere
  // intersection points belongs to blend trajectory after blending
  std::size_t first_intersection_index;
  std::size_t second_intersection_index;
  if (!searchIntersectionPoints(req, first_intersection_index, second_intersection_index))
  {
    ROS_ERROR("Blend radius to large.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // Select blending period and adjust the start and end point of the blend
  // phase
  std::size_t blend_align_index;
  determineTrajectoryAlignment(req, first_intersection_index, second_intersection_index, blend_align_index);

  // blend the trajectories in Cartesian space
  pilz_industrial_motion_planner::CartesianTrajectory blend_trajectory_cartesian;
  blendTrajectoryCartesian(req, first_intersection_index, second_intersection_index, blend_align_index, sampling_time,
                           blend_trajectory_cartesian);

  // generate the blending trajectory in joint space
  std::map<std::string, double> initial_joint_position, initial_joint_velocity;
  for (const std::string& joint_name :
       req.first_trajectory->getFirstWayPointPtr()->getJointModelGroup(req.group_name)->getActiveJointModelNames())
  {
    initial_joint_position[joint_name] =
        req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariablePosition(joint_name);
    initial_joint_velocity[joint_name] =
        req.first_trajectory->getWayPoint(first_intersection_index - 1).getVariableVelocity(joint_name);
  }
  trajectory_msgs::JointTrajectory blend_joint_trajectory;
  moveit_msgs::MoveItErrorCodes error_code;

  if (!generateJointTrajectory(planning_scene, limits_.getJointLimitContainer(), blend_trajectory_cartesian,
                               req.group_name, req.link_name, initial_joint_position, initial_joint_velocity,
                               blend_joint_trajectory, error_code, true))
  {
    // LCOV_EXCL_START
    ROS_INFO("Failed to generate joint trajectory for blending trajectory.");
    res.error_code.val = error_code.val;
    return false;
    // LCOV_EXCL_STOP
  }

  res.first_trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(req.first_trajectory->getRobotModel(),
                                                                             req.first_trajectory->getGroup());
  res.blend_trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(req.first_trajectory->getRobotModel(),
                                                                             req.first_trajectory->getGroup());
  res.second_trajectory = std::make_shared<robot_trajectory::RobotTrajectory>(req.first_trajectory->getRobotModel(),
                                                                              req.first_trajectory->getGroup());

  // set the three trajectories after blending in response
  // erase the points [first_intersection_index, back()] from the first
  // trajectory
  for (size_t i = 0; i < first_intersection_index; ++i)
  {
    res.first_trajectory->insertWayPoint(i, req.first_trajectory->getWayPoint(i),
                                         req.first_trajectory->getWayPointDurationFromPrevious(i));
  }

  // append the blend trajectory
  res.blend_trajectory->setRobotTrajectoryMsg(req.first_trajectory->getFirstWayPoint(), blend_joint_trajectory);
  // copy the points [second_intersection_index, len] from the second trajectory
  for (size_t i = second_intersection_index + 1; i < req.second_trajectory->getWayPointCount(); ++i)
  {
    res.second_trajectory->insertWayPoint(i - (second_intersection_index + 1), req.second_trajectory->getWayPoint(i),
                                          req.second_trajectory->getWayPointDurationFromPrevious(i));
  }

  // adjust the time from start
  res.second_trajectory->setWayPointDurationFromPrevious(0, sampling_time);

  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
  return true;
}

bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::validateRequest(
    const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, double& sampling_time,
    moveit_msgs::MoveItErrorCodes& error_code) const
{
  ROS_DEBUG("Validate the trajectory blend request.");

  // check planning group
  if (!req.first_trajectory->getRobotModel()->hasJointModelGroup(req.group_name))
  {
    ROS_ERROR_STREAM("Unknown planning group: " << req.group_name);
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
    return false;
  }

  // check link exists
  if (!req.first_trajectory->getRobotModel()->hasLinkModel(req.link_name) &&
      !req.first_trajectory->getLastWayPoint().hasAttachedBody(req.link_name))
  {
    ROS_ERROR_STREAM("Unknown link name: " << req.link_name);
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
    return false;
  }

  if (req.blend_radius <= 0)
  {
    ROS_ERROR("Blending radius must be positive");
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // end position of the first trajectory and start position of second
  // trajectory must be the same
  if (!pilz_industrial_motion_planner::isRobotStateEqual(
          req.first_trajectory->getLastWayPoint(), req.second_trajectory->getFirstWayPoint(), req.group_name, EPSILON))
  {
    ROS_ERROR_STREAM("During blending the last point of the preceding and the first point of the succeding trajectory");
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // same uniform sampling time
  if (!pilz_industrial_motion_planner::determineAndCheckSamplingTime(req.first_trajectory, req.second_trajectory,
                                                                     EPSILON, sampling_time))
  {
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  // end position of the first trajectory and start position of second
  // trajectory must have zero
  // velocities/accelerations
  if (!pilz_industrial_motion_planner::isRobotStateStationary(req.first_trajectory->getLastWayPoint(), req.group_name,
                                                              EPSILON) ||
      !pilz_industrial_motion_planner::isRobotStateStationary(req.second_trajectory->getFirstWayPoint(), req.group_name,
                                                              EPSILON))
  {
    ROS_ERROR("Intersection point of the blending trajectories has non-zero "
              "velocities/accelerations.");
    error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN;
    return false;
  }

  return true;
}

void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::blendTrajectoryCartesian(
    const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, const std::size_t first_interse_index,
    const std::size_t second_interse_index, const std::size_t blend_align_index, double sampling_time,
    pilz_industrial_motion_planner::CartesianTrajectory& trajectory) const
{
  // other fields of the trajectory
  trajectory.group_name = req.group_name;
  trajectory.link_name = req.link_name;

  // Pose on first trajectory
  Eigen::Isometry3d blend_sample_pose1 =
      req.first_trajectory->getWayPoint(first_interse_index).getFrameTransform(req.link_name);

  // Pose on second trajectory
  Eigen::Isometry3d blend_sample_pose2 =
      req.second_trajectory->getWayPoint(second_interse_index).getFrameTransform(req.link_name);

  // blend the trajectory
  double blend_sample_num = second_interse_index + blend_align_index - first_interse_index + 1;
  pilz_industrial_motion_planner::CartesianTrajectoryPoint waypoint;
  blend_sample_pose2 = req.second_trajectory->getFirstWayPoint().getFrameTransform(req.link_name);

  // Pose on blending trajectory
  Eigen::Isometry3d blend_sample_pose;
  for (std::size_t i = 0; i < blend_sample_num; ++i)
  {
    // if the first trajectory does not reach the last sample, update
    if ((first_interse_index + i) < req.first_trajectory->getWayPointCount())
    {
      blend_sample_pose1 = req.first_trajectory->getWayPoint(first_interse_index + i).getFrameTransform(req.link_name);
    }

    // if after the alignment, the second trajectory starts, update
    if ((first_interse_index + i) > blend_align_index)
    {
      blend_sample_pose2 = req.second_trajectory->getWayPoint(first_interse_index + i - blend_align_index)
                               .getFrameTransform(req.link_name);
    }

    double s = (i + 1) / blend_sample_num;
    double alpha = 6 * std::pow(s, 5) - 15 * std::pow(s, 4) + 10 * std::pow(s, 3);

    // blend the translation
    blend_sample_pose.translation() = blend_sample_pose1.translation() +
                                      alpha * (blend_sample_pose2.translation() - blend_sample_pose1.translation());

    // blend the orientation
    Eigen::Quaterniond start_quat(blend_sample_pose1.rotation());
    Eigen::Quaterniond end_quat(blend_sample_pose2.rotation());
    blend_sample_pose.linear() = start_quat.slerp(alpha, end_quat).toRotationMatrix();

    // push to the trajectory
    waypoint.pose = tf2::toMsg(blend_sample_pose);
    waypoint.time_from_start = ros::Duration((i + 1.0) * sampling_time);
    trajectory.points.push_back(waypoint);
  }
}

bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIntersectionPoints(
    const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t& first_interse_index,
    std::size_t& second_interse_index) const
{
  ROS_INFO("Search for start and end point of blending trajectory.");

  // compute the position of the center of the blend sphere
  // (last point of the first trajectory, first point of the second trajectory)
  Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name);

  // Searh for intersection points according to distance
  if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory,
                                     true, first_interse_index))
  {
    ROS_ERROR_STREAM("Intersection point of first trajectory not found.");
    return false;
  }
  ROS_INFO_STREAM("Intersection point of first trajectory found, index: " << first_interse_index);

  if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.second_trajectory,
                                     false, second_interse_index))
  {
    ROS_ERROR_STREAM("Intersection point of second trajectory not found.");
    return false;
  }

  ROS_INFO_STREAM("Intersection point of second trajectory found, index: " << second_interse_index);
  return true;
}

void pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::determineTrajectoryAlignment(
    const pilz_industrial_motion_planner::TrajectoryBlendRequest& req, std::size_t first_interse_index,
    std::size_t second_interse_index, std::size_t& blend_align_index) const
{
  size_t way_point_count_1 = req.first_trajectory->getWayPointCount() - first_interse_index;
  size_t way_point_count_2 = second_interse_index + 1;

  if (way_point_count_1 > way_point_count_2)
  {
    blend_align_index = req.first_trajectory->getWayPointCount() - second_interse_index - 1;
  }
  else
  {
    blend_align_index = first_interse_index;
  }
}


<|start_filename|>moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  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 Pilz GmbH & Co. KG 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.
 *********************************************************************/

#include "pilz_industrial_motion_planner/trajectory_generator_ptp.h"
#include "moveit/robot_state/conversions.h"
#include "ros/ros.h"

#include <iostream>
#include <sstream>

#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

namespace pilz_industrial_motion_planner
{
TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const robot_model::RobotModelConstPtr& robot_model,
                                               const LimitsContainer& planner_limits, const std::string& group_name)
  : TrajectoryGenerator::TrajectoryGenerator(robot_model, planner_limits)
{
  if (!planner_limits_.hasJointLimits())
  {
    throw TrajectoryGeneratorInvalidLimitsException("joint limit not set");
  }

  joint_limits_ = planner_limits_.getJointLimitContainer();

  // collect most strict joint limits for each group in robot model
  const auto* jmg = robot_model->getJointModelGroup(group_name);
  if (!jmg)
    throw TrajectoryGeneratorInvalidLimitsException("invalid group: " + group_name);

  const auto& active_joints = jmg->getActiveJointModelNames();

  // no active joints
  if (!active_joints.empty())
  {
    most_strict_limit_ = joint_limits_.getCommonLimit(active_joints);

    if (!most_strict_limit_.has_velocity_limits)
      throw TrajectoryGeneratorInvalidLimitsException("velocity limit not set for group " + group_name);
    if (!most_strict_limit_.has_acceleration_limits)
      throw TrajectoryGeneratorInvalidLimitsException("acceleration limit not set for group " + group_name);
    if (!most_strict_limit_.has_deceleration_limits)
      throw TrajectoryGeneratorInvalidLimitsException("deceleration limit not set for group " + group_name);
  }

  ROS_INFO("Initialized Point-to-Point Trajectory Generator.");
}

void TrajectoryGeneratorPTP::planPTP(const std::map<std::string, double>& start_pos,
                                     const std::map<std::string, double>& goal_pos,
                                     trajectory_msgs::JointTrajectory& joint_trajectory,
                                     const double& velocity_scaling_factor, const double& acceleration_scaling_factor,
                                     const double& sampling_time)
{
  // initialize joint names
  for (const auto& item : goal_pos)
  {
    joint_trajectory.joint_names.push_back(item.first);
  }

  // check if goal already reached
  bool goal_reached = true;
  for (auto const& goal : goal_pos)
  {
    if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT)
    {
      goal_reached = false;
      break;
    }
  }
  if (goal_reached)
  {
    ROS_INFO_STREAM("Goal already reached, set one goal point explicitly.");
    if (joint_trajectory.points.empty())
    {
      trajectory_msgs::JointTrajectoryPoint point;
      point.time_from_start = ros::Duration(sampling_time);
      for (const std::string& joint_name : joint_trajectory.joint_names)
      {
        point.positions.push_back(start_pos.at(joint_name));
        point.velocities.push_back(0);
        point.accelerations.push_back(0);
      }
      joint_trajectory.points.push_back(point);
    }
    return;
  }

  // compute the fastest trajectory and choose the slowest joint as leading axis
  std::string leading_axis = joint_trajectory.joint_names.front();
  double max_duration = -1.0;

  std::map<std::string, VelocityProfileATrap> velocity_profile;
  for (const auto& joint_name : joint_trajectory.joint_names)
  {
    // create vecocity profile if necessary
    velocity_profile.insert(std::make_pair(
        joint_name, VelocityProfileATrap(velocity_scaling_factor * most_strict_limit_.max_velocity,
                                         acceleration_scaling_factor * most_strict_limit_.max_acceleration,
                                         acceleration_scaling_factor * most_strict_limit_.max_deceleration)));

    velocity_profile.at(joint_name).SetProfile(start_pos.at(joint_name), goal_pos.at(joint_name));
    if (velocity_profile.at(joint_name).Duration() > max_duration)
    {
      max_duration = velocity_profile.at(joint_name).Duration();
      leading_axis = joint_name;
    }
  }

  // Full Synchronization
  // This should only work if all axes have same max_vel, max_acc, max_dec
  // values
  // reset the velocity profile for other joints
  double acc_time = velocity_profile.at(leading_axis).firstPhaseDuration();
  double const_time = velocity_profile.at(leading_axis).secondPhaseDuration();
  double dec_time = velocity_profile.at(leading_axis).thirdPhaseDuration();

  for (const auto& joint_name : joint_trajectory.joint_names)
  {
    if (joint_name != leading_axis)
    {
      // make full synchronization
      // causes the program to terminate if acc_time<=0 or dec_time<=0 (should
      // be prevented by goal_reached block above)
      // by using the most strict limit, the following should always return true
      if (!velocity_profile.at(joint_name)
               .setProfileAllDurations(start_pos.at(joint_name), goal_pos.at(joint_name), acc_time, const_time,
                                       dec_time))
      // LCOV_EXCL_START
      {
        std::stringstream error_str;
        error_str << "TrajectoryGeneratorPTP::planPTP(): Can not synchronize "
                     "velocity profile of axis "
                  << joint_name << " with leading axis " << leading_axis;
        throw PtpVelocityProfileSyncFailed(error_str.str());
      }
      // LCOV_EXCL_STOP
    }
  }

  // first generate the time samples
  std::vector<double> time_samples;
  for (double t_sample = 0.0; t_sample < max_duration; t_sample += sampling_time)
  {
    time_samples.push_back(t_sample);
  }
  // add last time
  time_samples.push_back(max_duration);

  // construct joint trajectory point
  for (double time_stamp : time_samples)
  {
    trajectory_msgs::JointTrajectoryPoint point;
    point.time_from_start = ros::Duration(time_stamp);
    for (std::string& joint_name : joint_trajectory.joint_names)
    {
      point.positions.push_back(velocity_profile.at(joint_name).Pos(time_stamp));
      point.velocities.push_back(velocity_profile.at(joint_name).Vel(time_stamp));
      point.accelerations.push_back(velocity_profile.at(joint_name).Acc(time_stamp));
    }
    joint_trajectory.points.push_back(point);
  }

  // Set last point velocity and acceleration to zero
  std::fill(joint_trajectory.points.back().velocities.begin(), joint_trajectory.points.back().velocities.end(), 0.0);
  std::fill(joint_trajectory.points.back().accelerations.begin(), joint_trajectory.points.back().accelerations.end(),
            0.0);
}

void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::PlanningSceneConstPtr& scene,
                                                   const planning_interface::MotionPlanRequest& req,
                                                   MotionPlanInfo& info) const
{
  info.group_name = req.group_name;

  // extract start state information
  info.start_joint_position.clear();
  for (std::size_t i = 0; i < req.start_state.joint_state.name.size(); ++i)
  {
    info.start_joint_position[req.start_state.joint_state.name[i]] = req.start_state.joint_state.position[i];
  }

  // extract goal
  info.goal_joint_position.clear();
  if (!req.goal_constraints.at(0).joint_constraints.empty())
  {
    for (const auto& joint_constraint : req.goal_constraints.at(0).joint_constraints)
    {
      info.goal_joint_position[joint_constraint.joint_name] = joint_constraint.position;
    }
  }
  // solve the ik
  else
  {
    Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
    if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
                       goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
    {
      throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
    }
  }
}

void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/,
                                  const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info,
                                  const double& sampling_time, trajectory_msgs::JointTrajectory& joint_trajectory)
{
  // plan the ptp trajectory
  planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory,
          req.max_velocity_scaling_factor, req.max_acceleration_scaling_factor, sampling_time);
}

}  // namespace pilz_industrial_motion_planner


<|start_filename|>moveit_planners/pilz_industrial_motion_planner/test/unittest_trajectory_blender_transition_window.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  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 Pilz GmbH & Co. KG 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.
 *********************************************************************/

#include <memory>

#include <gtest/gtest.h>

#include <moveit/kinematic_constraints/utils.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_state/conversions.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <pilz_industrial_motion_planner_testutils/command_types_typedef.h>
#include <pilz_industrial_motion_planner_testutils/sequence.h>
#include <pilz_industrial_motion_planner_testutils/xml_testdata_loader.h>

#include "pilz_industrial_motion_planner/joint_limits_aggregator.h"
#include "pilz_industrial_motion_planner/trajectory_blend_request.h"
#include "pilz_industrial_motion_planner/trajectory_blend_response.h"
#include "pilz_industrial_motion_planner/trajectory_blender_transition_window.h"
#include "pilz_industrial_motion_planner/trajectory_generator_lin.h"
#include "test_utils.h"

const std::string PARAM_MODEL_NO_GRIPPER_NAME{ "robot_description" };
const std::string PARAM_MODEL_WITH_GRIPPER_NAME{ "robot_description_pg70" };

// parameters from parameter server
const std::string PARAM_PLANNING_GROUP_NAME("planning_group");
const std::string PARAM_TARGET_LINK_NAME("target_link");
const std::string CARTESIAN_VELOCITY_TOLERANCE("cartesian_velocity_tolerance");
const std::string CARTESIAN_ANGULAR_VELOCITY_TOLERANCE("cartesian_angular_velocity_tolerance");
const std::string JOINT_VELOCITY_TOLERANCE("joint_velocity_tolerance");
const std::string JOINT_ACCELERATION_TOLERANCE("joint_acceleration_tolerance");
const std::string OTHER_TOLERANCE("other_tolerance");
const std::string SAMPLING_TIME("sampling_time");
const std::string TEST_DATA_FILE_NAME("testdata_file_name");

using namespace pilz_industrial_motion_planner;
using namespace pilz_industrial_motion_planner_testutils;

class TrajectoryBlenderTransitionWindowTest : public testing::TestWithParam<std::string>
{
protected:
  /**
   * @brief Create test scenario for trajectory blender
   *
   */
  void SetUp() override;

  /**
   * @brief Generate lin trajectories for blend sequences
   */
  std::vector<planning_interface::MotionPlanResponse> generateLinTrajs(const Sequence& seq, size_t num_cmds);

protected:
  // ros stuff
  ros::NodeHandle ph_{ "~" };
  robot_model::RobotModelConstPtr robot_model_{ robot_model_loader::RobotModelLoader(GetParam()).getModel() };
  planning_scene::PlanningSceneConstPtr planning_scene_{ new planning_scene::PlanningScene(robot_model_) };

  std::unique_ptr<TrajectoryGenerator> lin_generator_;
  std::unique_ptr<TrajectoryBlenderTransitionWindow> blender_;

  // test parameters from parameter server
  std::string planning_group_, target_link_;
  double cartesian_velocity_tolerance_, cartesian_angular_velocity_tolerance_, joint_velocity_tolerance_,
      joint_acceleration_tolerance_, sampling_time_;
  LimitsContainer planner_limits_;

  std::string test_data_file_name_;
  XmlTestDataLoaderUPtr data_loader_;
};

void TrajectoryBlenderTransitionWindowTest::SetUp()
{
  // get parameters
  ASSERT_TRUE(ph_.getParam(PARAM_PLANNING_GROUP_NAME, planning_group_));
  ASSERT_TRUE(ph_.getParam(PARAM_TARGET_LINK_NAME, target_link_));
  ASSERT_TRUE(ph_.getParam(CARTESIAN_VELOCITY_TOLERANCE, cartesian_velocity_tolerance_));
  ASSERT_TRUE(ph_.getParam(CARTESIAN_ANGULAR_VELOCITY_TOLERANCE, cartesian_angular_velocity_tolerance_));
  ASSERT_TRUE(ph_.getParam(JOINT_VELOCITY_TOLERANCE, joint_velocity_tolerance_));
  ASSERT_TRUE(ph_.getParam(JOINT_ACCELERATION_TOLERANCE, joint_acceleration_tolerance_));
  ASSERT_TRUE(ph_.getParam(SAMPLING_TIME, sampling_time_));
  ASSERT_TRUE(ph_.getParam(TEST_DATA_FILE_NAME, test_data_file_name_));

  // load the test data provider
  data_loader_ = std::make_unique<XmlTestdataLoader>(test_data_file_name_, robot_model_);
  ASSERT_NE(nullptr, data_loader_) << "Failed to load test data by provider.";

  // check robot model
  testutils::checkRobotModel(robot_model_, planning_group_, target_link_);

  // create the limits container
  pilz_industrial_motion_planner::JointLimitsContainer joint_limits =
      pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(ph_,
                                                                                 robot_model_->getActiveJointModels());
  CartesianLimit cart_limits;
  cart_limits.setMaxRotationalVelocity(1 * M_PI);
  cart_limits.setMaxTranslationalAcceleration(2);
  cart_limits.setMaxTranslationalDeceleration(2);
  cart_limits.setMaxTranslationalVelocity(1);
  planner_limits_.setJointLimits(joint_limits);
  planner_limits_.setCartesianLimits(cart_limits);

  // initialize trajectory generators and blender
  lin_generator_ = std::make_unique<TrajectoryGeneratorLIN>(robot_model_, planner_limits_, planning_group_);
  ASSERT_NE(nullptr, lin_generator_) << "failed to create LIN trajectory generator";
  blender_ = std::make_unique<TrajectoryBlenderTransitionWindow>(planner_limits_);
  ASSERT_NE(nullptr, blender_) << "failed to create trajectory blender";
}

std::vector<planning_interface::MotionPlanResponse>
TrajectoryBlenderTransitionWindowTest::generateLinTrajs(const Sequence& seq, size_t num_cmds)
{
  std::vector<planning_interface::MotionPlanResponse> responses(num_cmds);

  for (size_t index = 0; index < num_cmds; ++index)
  {
    planning_interface::MotionPlanRequest req{ seq.getCmd<LinCart>(index).toRequest() };
    // Set start state of request to end state of previous trajectory (except
    // for first)
    if (index > 0)
    {
      moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state);
    }
    // generate trajectory
    planning_interface::MotionPlanResponse resp;
    if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_))
    {
      std::runtime_error("Failed to generate trajectory.");
    }
    responses.at(index) = resp;
  }
  return responses;
}

// Instantiate the test cases for robot model with and without gripper
INSTANTIATE_TEST_SUITE_P(InstantiationName, TrajectoryBlenderTransitionWindowTest,
                         ::testing::Values(PARAM_MODEL_NO_GRIPPER_NAME, PARAM_MODEL_WITH_GRIPPER_NAME));

/**
 * @brief  Tests the blending of two trajectories with an invalid group name.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories.
 *    2. Try to generate blending trajectory with invalid group name.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidGroupName)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = "invalid_group_name";
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories with an invalid target link.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories.
 *    2. Try to generate blending trajectory with invalid target link.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testInvalidTargetLink)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = "invalid_target_link";
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories with a negative blending
 * radius.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories.
 *    2. Try to generate blending trajectory with negative blending radius.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testNegativeRadius)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  blend_req.blend_radius = -0.1;
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories with zero blending radius.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories.
 *    2. Try to generate blending trajectory with zero blending radius.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testZeroRadius)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  blend_req.blend_radius = 0.;
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories with differenent sampling
 * times.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories with different sampling times.
 *    2. Try to generate blending trajectory.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testDifferentSamplingTimes)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  // perform lin trajectory generation and modify sampling time
  std::size_t num_cmds{ 2 };
  std::vector<planning_interface::MotionPlanResponse> responses(num_cmds);

  for (size_t index = 0; index < num_cmds; ++index)
  {
    planning_interface::MotionPlanRequest req{ seq.getCmd<LinCart>(index).toRequest() };
    // Set start state of request to end state of previous trajectory (except
    // for first)
    if (index > 0)
    {
      moveit::core::robotStateToRobotStateMsg(responses[index - 1].trajectory_->getLastWayPoint(), req.start_state);
      sampling_time_ *= 2;
    }
    // generate trajectory
    planning_interface::MotionPlanResponse resp;
    if (!lin_generator_->generate(planning_scene_, req, resp, sampling_time_))
    {
      std::runtime_error("Failed to generate trajectory.");
    }
    responses.at(index) = resp;
  }

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = responses[0].trajectory_;
  blend_req.second_trajectory = responses[1].trajectory_;
  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories with one trajectory
 * having non-uniform sampling time (apart from the last sample,
 * which is ignored).
 *
 * Test Sequence:
 *    1. Generate two linear trajectories and corrupt uniformity of sampling
 * time.
 *    2. Try to generate blending trajectory.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testNonUniformSamplingTime)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  // Modify first time interval
  EXPECT_GT(res[0].trajectory_->getWayPointCount(), 2u);
  res[0].trajectory_->setWayPointDurationFromPrevious(1, 2 * sampling_time_);

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;
  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two trajectories which do not intersect.
 *
 * Test Sequence:
 *    1. Generate two trajectories from valid test data set.
 *    2. Replace the second trajectory by the first one.
 *    2. Try to generate blending trajectory.
 *
 * Expected Results:
 *    1. Two trajectories generated.
 *    2. Two trajectories that do not intersect.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testNotIntersectingTrajectories)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 1) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  // replace the second trajectory to make the two trajectories timely not
  // intersect
  blend_req.second_trajectory = res.at(0).trajectory_;
  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two cartesian trajectories with the
 * shared point (last point of first, first point of second trajectory)
 * having a non-zero velocity
 *
 * Test Sequence:
 *    1. Generate two trajectories from the test data set.
 *    2. Generate blending trajectory modify the shared point to have velocity.
 * Expected Results:
 *    1. Two trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testNonStationaryPoint)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.blend_radius = seq.getBlendRadius(0);

  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  // Modify last waypoint of first trajectory and first point of second
  // trajectory
  blend_req.first_trajectory->getLastWayPointPtr()->setVariableVelocity(0, 1.0);
  blend_req.second_trajectory->getFirstWayPointPtr()->setVariableVelocity(0, 1.0);

  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief Tests the blending of two cartesian trajectories where the first
 * trajectory is completely within the sphere defined by the blend radius
 *
 * Test Sequence:
 *    1. Generate two trajectories from the test data set.
 *    2. Generate blending trajectory with a blend_radius larger
 *        than the smaller trajectory.
 *
 * Expected Results:
 *    1. Two trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj1InsideBlendRadius)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  double lin1_distance;
  lin1_distance = (res[0].trajectory_->getFirstWayPoint().getFrameTransform(target_link_).translation() -
                   res[0].trajectory_->getLastWayPoint().getFrameTransform(target_link_).translation())
                      .norm();

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.blend_radius = 1.1 * lin1_distance;

  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief Tests the blending of two cartesian trajectories where the second
 * trajectory is completely within the sphere defined by the blend radius
 *
 * Test Sequence:
 *    1. Generate two trajectories from the test data set.
 *    2. Generate blending trajectory with a blend_radius larger
 *        than the smaller trajectory.
 *
 * Expected Results:
 *    1. Two trajectories generated.
 *    2. Blending trajectory cannot be generated.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testTraj2InsideBlendRadius)
{
  Sequence seq{ data_loader_->getSequence("NoIntersectionTraj2") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.blend_radius = seq.getBlendRadius(0);

  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  EXPECT_FALSE(blender_->blend(planning_scene_, blend_req, blend_res));
}

/**
 * @brief  Tests the blending of two cartesian linear trajectories using robot
 * model
 *
 * Test Sequence:
 *    1. Generate two linear trajectories from the test data set.
 *    2. Generate blending trajectory.
 *    3. Check blending trajectory:
 *      - for position, velocity, and acceleration bounds,
 *      - for continuity in joint space,
 *      - for continuity in cartesian space.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory generated.
 *    3. No bound is violated, the trajectories are continuous
 *        in joint and cartesian space.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testLinLinBlending)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.blend_radius = seq.getBlendRadius(0);

  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;

  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));

  EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                          joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
                                          cartesian_angular_velocity_tolerance_));
}

/**
 * @brief  Tests the blending of two cartesian linear trajectories which have
 * an overlap in the blending sphere using robot model. To be precise,
 * the trajectories exactly lie on top of each other.
 *
 * Test Sequence:
 *    1. Generate two linear trajectories from the test data set. Set goal of
 * second traj to start of first traj.
 *    2. Generate blending trajectory.
 *    3. Check blending trajectory:
 *      - for position, velocity, and acceleration bounds,
 *      - for continuity in joint space,
 *      - for continuity in cartesian space.
 *
 * Expected Results:
 *    1. Two linear trajectories generated.
 *    2. Blending trajectory generated.
 *    3. No bound is violated, the trajectories are continuous
 *        in joint and cartesian space.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testOverlappingBlendTrajectories)
{
  Sequence seq{ data_loader_->getSequence("SimpleSequence") };
  // Set goal of second traj to start of first traj.
  seq.getCmd<LinCart>(1).setGoalConfiguration(seq.getCmd<LinCart>(0).getStartConfiguration());

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  pilz_industrial_motion_planner::TrajectoryBlendRequest blend_req;
  pilz_industrial_motion_planner::TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.first_trajectory = res.at(0).trajectory_;
  blend_req.second_trajectory = res.at(1).trajectory_;
  blend_req.blend_radius = seq.getBlendRadius(0);
  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));

  EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                          joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
                                          cartesian_angular_velocity_tolerance_));
}

/**
 * @brief Tests the blending of two cartesian trajectories which differ
 * from a straight line.
 *
 * Test Sequence:
 *    1. Generate two trajectories from the test data set.
 *    2. Add scaled sine function to cartesian trajectories, such that
 *        start and end state remain unchanged; generate resulting
 *        joint trajectories using a time scaling in order to preserve
 *        joint velocity limits.
 *    3. Generate blending trajectory.
 *    4. Check blending trajectory:
 *      - for position, velocity, and acceleration bounds,
 *      - for continuity in joint space,
 *      - for continuity in cartesian space.
 *
 * Expected Results:
 *    1. Two trajectories generated.
 *    2. Modified joint trajectories generated.
 *    3. Blending trajectory generated.
 *    4. No bound is violated, the trajectories are continuous
 *        in joint and cartesian space.
 */
TEST_P(TrajectoryBlenderTransitionWindowTest, testNonLinearBlending)
{
  const double sine_scaling_factor{ 0.01 };
  const double time_scaling_factor{ 10 };

  Sequence seq{ data_loader_->getSequence("SimpleSequence") };

  std::vector<planning_interface::MotionPlanResponse> res{ generateLinTrajs(seq, 2) };

  // prepare looping over trajectories
  std::vector<robot_trajectory::RobotTrajectoryPtr> sine_trajs(2);

  for (size_t traj_index = 0; traj_index < 2; ++traj_index)
  {
    auto lin_traj{ res.at(traj_index).trajectory_ };

    CartesianTrajectory cart_traj;
    trajectory_msgs::JointTrajectory joint_traj;
    const double duration{ lin_traj->getWayPointDurationFromStart(lin_traj->getWayPointCount()) };
    // time from start zero does not work
    const double time_from_start_offset{ time_scaling_factor * lin_traj->getWayPointDurations().back() };

    // generate modified cartesian trajectory
    for (size_t i = 0; i < lin_traj->getWayPointCount(); ++i)
    {
      // transform time to interval [0, 4*pi]
      const double sine_arg{ 4 * M_PI * lin_traj->getWayPointDurationFromStart(i) / duration };

      // get pose
      CartesianTrajectoryPoint waypoint;
      const Eigen::Isometry3d eigen_pose{ lin_traj->getWayPointPtr(i)->getFrameTransform(target_link_) };
      geometry_msgs::Pose waypoint_pose = tf2::toMsg(eigen_pose);

      // add scaled sine function
      waypoint_pose.position.x += sine_scaling_factor * sin(sine_arg);
      waypoint_pose.position.y += sine_scaling_factor * sin(sine_arg);
      waypoint_pose.position.z += sine_scaling_factor * sin(sine_arg);

      // add to trajectory
      waypoint.pose = waypoint_pose;
      waypoint.time_from_start =
          ros::Duration(time_from_start_offset + time_scaling_factor * lin_traj->getWayPointDurationFromStart(i));
      cart_traj.points.push_back(waypoint);
    }

    // prepare ik
    std::map<std::string, double> initial_joint_position, initial_joint_velocity;
    for (const std::string& joint_name :
         lin_traj->getFirstWayPointPtr()->getJointModelGroup(planning_group_)->getActiveJointModelNames())
    {
      if (traj_index == 0)
      {
        initial_joint_position[joint_name] = lin_traj->getFirstWayPoint().getVariablePosition(joint_name);
        initial_joint_velocity[joint_name] = lin_traj->getFirstWayPoint().getVariableVelocity(joint_name);
      }
      else
      {
        initial_joint_position[joint_name] =
            sine_trajs[traj_index - 1]->getLastWayPoint().getVariablePosition(joint_name);
        initial_joint_velocity[joint_name] =
            sine_trajs[traj_index - 1]->getLastWayPoint().getVariableVelocity(joint_name);
      }
    }

    moveit_msgs::MoveItErrorCodes error_code;
    if (!generateJointTrajectory(planning_scene_, planner_limits_.getJointLimitContainer(), cart_traj, planning_group_,
                                 target_link_, initial_joint_position, initial_joint_velocity, joint_traj, error_code,
                                 true))
    {
      std::runtime_error("Failed to generate trajectory.");
    }

    joint_traj.points.back().velocities.assign(joint_traj.points.back().velocities.size(), 0.0);
    joint_traj.points.back().accelerations.assign(joint_traj.points.back().accelerations.size(), 0.0);

    // convert trajectory_msgs::JointTrajectory to
    // robot_trajectory::RobotTrajectory
    sine_trajs[traj_index] = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, planning_group_);
    sine_trajs.at(traj_index)->setRobotTrajectoryMsg(lin_traj->getFirstWayPoint(), joint_traj);
  }

  TrajectoryBlendRequest blend_req;
  TrajectoryBlendResponse blend_res;

  blend_req.group_name = planning_group_;
  blend_req.link_name = target_link_;
  blend_req.blend_radius = seq.getBlendRadius(0);

  blend_req.first_trajectory = sine_trajs.at(0);
  blend_req.second_trajectory = sine_trajs.at(1);

  EXPECT_TRUE(blender_->blend(planning_scene_, blend_req, blend_res));

  EXPECT_TRUE(testutils::checkBlendResult(blend_req, blend_res, planner_limits_, joint_velocity_tolerance_,
                                          joint_acceleration_tolerance_, cartesian_velocity_tolerance_,
                                          cartesian_angular_velocity_tolerance_));
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "unittest_trajectory_blender_transition_window");
  ros::NodeHandle nh;
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


<|start_filename|>moveit_planners/chomp/chomp_motion_planner/src/chomp_parameters.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, <NAME>, 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 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: <NAME> */

#include <chomp_motion_planner/chomp_parameters.h>

namespace chomp
{
ChompParameters::ChompParameters()
{
  planning_time_limit_ = 6.0;
  max_iterations_ = 50;
  max_iterations_after_collision_free_ = 5;
  smoothness_cost_weight_ = 0.1;
  obstacle_cost_weight_ = 1.0;
  learning_rate_ = 0.01;

  // add_randomness_ = false;
  smoothness_cost_velocity_ = 0.0;
  smoothness_cost_acceleration_ = 1.0;
  smoothness_cost_jerk_ = 0.0;
  // hmc_discretization_ = 0.01;
  // hmc_stochasticity_ = 0.01;
  // hmc_annealing_factor_ = 0.99;
  // use_hamiltonian_monte_carlo_ = false;
  ridge_factor_ = 0.0;
  use_pseudo_inverse_ = false;
  pseudo_inverse_ridge_factor_ = 1e-4;

  joint_update_limit_ = 0.1;
  min_clearance_ = 0.2;
  collision_threshold_ = 0.07;
  // random_jump_amount_ = 1.0;
  use_stochastic_descent_ = true;
  filter_mode_ = false;
  trajectory_initialization_method_ = std::string("quintic-spline");
  enable_failure_recovery_ = false;
  max_recovery_attempts_ = 5;
}

ChompParameters::~ChompParameters() = default;

void ChompParameters::setRecoveryParams(double learning_rate, double ridge_factor, int planning_time_limit,
                                        int max_iterations)
{
  this->learning_rate_ = learning_rate;
  this->ridge_factor_ = ridge_factor;
  this->planning_time_limit_ = planning_time_limit;
  this->max_iterations_ = max_iterations;
}

const std::vector<std::string> ChompParameters::VALID_INITIALIZATION_METHODS{ "quintic-spline", "linear", "cubic",
                                                                              "fillTrajectory" };

bool ChompParameters::setTrajectoryInitializationMethod(std::string method)
{
  if (std::find(VALID_INITIALIZATION_METHODS.cbegin(), VALID_INITIALIZATION_METHODS.cend(), method) !=
      VALID_INITIALIZATION_METHODS.end())
  {
    this->trajectory_initialization_method_ = std::move(method);
    return true;
  }
  return false;
}
}  // namespace chomp


<|start_filename|>moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/checks.h<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019 Pilz GmbH & Co. KG
 *  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 Pilz GmbH & Co. KG 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.
 *********************************************************************/

#ifndef CHECKS_H
#define CHECKS_H

#include <gtest/gtest.h>
#include <moveit/robot_state/robot_state.h>

namespace pilz_industrial_motion_planner_testutils
{
::testing::AssertionResult isAtExpectedPosition(const robot_state::RobotState& expected,
                                                const robot_state::RobotState& actual, const double epsilon,
                                                const std::string& group_name = "")
{
  if (group_name.empty() && expected.distance(actual) <= epsilon)
  {
    return ::testing::AssertionSuccess();
  }
  else if (!group_name.empty() && expected.distance(actual, actual.getJointModelGroup(group_name)) <= epsilon)
  {
    return ::testing::AssertionSuccess();
  }
  std::stringstream msg;
  msg << " expected: " << expected << " actual: " << actual;

  return ::testing::AssertionFailure() << msg.str();
}
}  // namespace pilz_industrial_motion_planner_testutils

#endif  // CENTERAUXILIARY_H


<|start_filename|>moveit_core/kinematic_constraints/src/pykinematic_constraint.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>

#include <moveit/kinematic_constraints/kinematic_constraint.h>
#include <moveit/kinematic_constraints/utils.h>

namespace py = pybind11;

using namespace kinematic_constraints;

void def_kinematic_constraints_bindings(py::module& m)
{
  m.doc() = "Class for joint, position, visibility, and other constraints";

  py::class_<ConstraintEvaluationResult, std::shared_ptr<ConstraintEvaluationResult>>(m, "ConstraintEvaluationResult")
      .def(py::init<>())
      .def_readwrite("satisfied", &ConstraintEvaluationResult::satisfied)
      .def_readwrite("distance", &ConstraintEvaluationResult::distance)
      //
      ;

  py::class_<KinematicConstraintSet, KinematicConstraintSetPtr>(m, "KinematicConstraintSet")
      .def(py::init<robot_model::RobotModelConstPtr>(), py::arg("robot_model"))
      .def("add", py::overload_cast<const moveit_msgs::Constraints&, const moveit::core::Transforms&>(
                      &KinematicConstraintSet::add))
      .def("decide",
           py::overload_cast<const moveit::core::RobotState&, bool>(&KinematicConstraintSet::decide, py::const_),
           py::arg("state"), py::arg("verbose") = false)
      //
      ;

  m.def("constructGoalConstraints",
        py::overload_cast<const std::string&, const geometry_msgs::PoseStamped&, double, double>(
            &constructGoalConstraints),
        py::arg("link_name"), py::arg("pose"), py::arg("tolerance_pos") = 1e-3, py::arg("tolerance_angle") = 1e-2)
      //
      ;
}


<|start_filename|>moveit_ros/move_group/src/move_group_capability.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, <NAME>, 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 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: <NAME> */

#include <moveit/moveit_cpp/moveit_cpp.h>
#include <moveit/move_group/move_group_capability.h>
#include <moveit/robot_state/conversions.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <sstream>
#include <string>

void move_group::MoveGroupCapability::setContext(const MoveGroupContextPtr& context)
{
  context_ = context;
}

void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
                                                   moveit_msgs::RobotState& first_state_msg,
                                                   std::vector<moveit_msgs::RobotTrajectory>& trajectory_msg) const
{
  if (!trajectory.empty())
  {
    bool first = true;
    trajectory_msg.resize(trajectory.size());
    for (std::size_t i = 0; i < trajectory.size(); ++i)
    {
      if (trajectory[i].trajectory_)
      {
        if (first && !trajectory[i].trajectory_->empty())
        {
          moveit::core::robotStateToRobotStateMsg(trajectory[i].trajectory_->getFirstWayPoint(), first_state_msg);
          first = false;
        }
        trajectory[i].trajectory_->getRobotTrajectoryMsg(trajectory_msg[i]);
      }
    }
  }
}

void move_group::MoveGroupCapability::convertToMsg(const robot_trajectory::RobotTrajectoryPtr& trajectory,
                                                   moveit_msgs::RobotState& first_state_msg,
                                                   moveit_msgs::RobotTrajectory& trajectory_msg) const
{
  if (trajectory && !trajectory->empty())
  {
    moveit::core::robotStateToRobotStateMsg(trajectory->getFirstWayPoint(), first_state_msg);
    trajectory->getRobotTrajectoryMsg(trajectory_msg);
  }
}

void move_group::MoveGroupCapability::convertToMsg(const std::vector<plan_execution::ExecutableTrajectory>& trajectory,
                                                   moveit_msgs::RobotState& first_state_msg,
                                                   moveit_msgs::RobotTrajectory& trajectory_msg) const
{
  if (trajectory.size() > 1)
    ROS_ERROR_STREAM("Internal logic error: trajectory component ignored. !!! THIS IS A SERIOUS ERROR !!!");
  if (!trajectory.empty())
    convertToMsg(trajectory[0].trajectory_, first_state_msg, trajectory_msg);
}

planning_interface::MotionPlanRequest
move_group::MoveGroupCapability::clearRequestStartState(const planning_interface::MotionPlanRequest& request) const
{
  planning_interface::MotionPlanRequest r = request;
  r.start_state = moveit_msgs::RobotState();
  r.start_state.is_diff = true;
  ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
           "start state in the motion planning request");
  return r;
}

moveit_msgs::PlanningScene
move_group::MoveGroupCapability::clearSceneRobotState(const moveit_msgs::PlanningScene& scene) const
{
  moveit_msgs::PlanningScene r = scene;
  r.robot_state = moveit_msgs::RobotState();
  r.robot_state.is_diff = true;
  ROS_WARN("Execution of motions should always start at the robot's current state. Ignoring the state supplied as "
           "difference in the planning scene diff");
  return r;
}

std::string move_group::MoveGroupCapability::getActionResultString(const moveit_msgs::MoveItErrorCodes& error_code,
                                                                   bool planned_trajectory_empty, bool plan_only)
{
  if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
  {
    if (planned_trajectory_empty)
      return "Requested path and goal constraints are already met.";
    else
    {
      if (plan_only)
        return "Motion plan was computed succesfully.";
      else
        return "Solution was found and executed.";
    }
  }
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME)
    return "Must specify group in motion plan request";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PLANNING_FAILED ||
           error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_MOTION_PLAN)
  {
    if (planned_trajectory_empty)
      return "No motion plan found. No execution attempted.";
    else
      return "Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.";
  }
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA)
    return "Motion plan was found but it seems to be too costly and looking around did not help.";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::MOTION_PLAN_INVALIDATED_BY_ENVIRONMENT_CHANGE)
    return "Solution found but the environment changed during execution and the path was aborted";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::CONTROL_FAILED)
    return "Solution found but controller failed during execution";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::TIMED_OUT)
    return "Timeout reached";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
    return "Preempted";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS)
    return "Invalid goal constraints";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::INVALID_OBJECT_NAME)
    return "Invalid object name";
  else if (error_code.val == moveit_msgs::MoveItErrorCodes::FAILURE)
    return "Catastrophic failure";
  return "Unknown event";
}

std::string move_group::MoveGroupCapability::stateToStr(MoveGroupState state) const
{
  switch (state)
  {
    case IDLE:
      return "IDLE";
    case PLANNING:
      return "PLANNING";
    case MONITOR:
      return "MONITOR";
    case LOOK:
      return "LOOK";
    default:
      return "UNKNOWN";
  }
}

bool move_group::MoveGroupCapability::performTransform(geometry_msgs::PoseStamped& pose_msg,
                                                       const std::string& target_frame) const
{
  if (!context_ || !context_->planning_scene_monitor_->getTFClient())
    return false;
  if (pose_msg.header.frame_id == target_frame)
    return true;
  if (pose_msg.header.frame_id.empty())
  {
    pose_msg.header.frame_id = target_frame;
    return true;
  }

  try
  {
    geometry_msgs::TransformStamped common_tf = context_->planning_scene_monitor_->getTFClient()->lookupTransform(
        pose_msg.header.frame_id, target_frame, ros::Time(0.0));
    geometry_msgs::PoseStamped pose_msg_in(pose_msg);
    pose_msg_in.header.stamp = common_tf.header.stamp;
    context_->planning_scene_monitor_->getTFClient()->transform(pose_msg_in, pose_msg, target_frame);
  }
  catch (tf2::TransformException& ex)
  {
    ROS_ERROR("TF Problem: %s", ex.what());
    return false;
  }
  return true;
}

planning_pipeline::PlanningPipelinePtr
move_group::MoveGroupCapability::resolvePlanningPipeline(const std::string& pipeline_id) const
{
  if (pipeline_id.empty())
  {
    // Without specified planning pipeline we use the default
    return context_->planning_pipeline_;
  }
  else
  {
    // Attempt to get the planning pipeline for the specified identifier
    try
    {
      auto pipeline = context_->moveit_cpp_->getPlanningPipelines().at(pipeline_id);
      ROS_INFO_NAMED(getName(), "Using planning pipeline '%s'", pipeline_id.c_str());
      return pipeline;
    }
    catch (const std::out_of_range&)
    {
      ROS_WARN_NAMED(getName(), "Couldn't find requested planning pipeline '%s'", pipeline_id.c_str());
    }
  }

  return planning_pipeline::PlanningPipelinePtr();
}


<|start_filename|>moveit_core/transforms/src/pytransforms.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>

#include <moveit/transforms/transforms.h>

namespace py = pybind11;
using namespace moveit::core;

void def_transforms_bindings(py::module& m)
{
  m.doc() = "Provides an implementation of a snapshot of a transform tree that can be easily queried for transforming "
            "different quantities. Transforms are maintained as a list of transforms to a particular frame. All stored "
            "transforms are considered fixed.";
  py::class_<Transforms, TransformsPtr>(m, "Transforms")
      .def(py::init<const std::string&>())
      .def("canTransform", &Transforms::canTransform)
      .def("getTargetFrame", &Transforms::getTargetFrame)
      .def("getTransform", &Transforms::getTransform)
      .def("isFixedFrame", &Transforms::isFixedFrame)
      .def("getAllTransforms", &Transforms::getAllTransforms)
      .def("setTransform", py::overload_cast<const Eigen::Isometry3d&, const std::string&>(&Transforms::setTransform))
      .def("setTransform", py::overload_cast<const geometry_msgs::TransformStamped&>(&Transforms::setTransform))
      .def("setTransforms", &Transforms::setTransforms)
      .def("setAllTransforms", &Transforms::setAllTransforms)
      .def("transformVector3", &Transforms::transformVector3)
      .def("transformQuaternion", &Transforms::transformQuaternion)
      .def("transformRotationMatrix", &Transforms::transformRotationMatrix)
      .def("transformPose", &Transforms::transformPose)
      //
      ;
}


<|start_filename|>moveit_ros/warehouse/warehouse/Makefile<|end_filename|>
include $(shell rospack find mk)/cmake.mk


<|start_filename|>moveit_core/robot_state/test/test_cartesian_interpolator.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, PickNik LLC.
 *  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 the Willow Garage 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: <NAME> */

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/cartesian_interpolator.h>
#include <moveit/utils/robot_model_test_utils.h>

#include <urdf_parser/urdf_parser.h>
#include <gtest/gtest.h>

#include <memory>
#include <sstream>
#include <algorithm>
#include <ctype.h>

class OneRobot : public testing::Test
{
protected:
  void SetUp() override
  {
    // TODO(mlautman): Use new testing framework for loading models
    // https://ros-planning.github.io/moveit_tutorials/doc/tests/tests_tutorial.html
    static const std::string MODEL2 =
        "<?xml version=\"1.0\" ?>"
        "<robot name=\"one_robot\">"
        "<link name=\"base_link\">"
        "  <inertial>"
        "    <mass value=\"2.81\"/>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
        "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
        "  </inertial>"
        "  <collision name=\"my_collision\">"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "<joint name=\"panda_joint0\" type=\"continuous\">"
        "   <axis xyz=\"0 0 1\"/>"
        "   <parent link=\"base_link\"/>"
        "   <child link=\"link_a\"/>"
        "   <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
        "</joint>"
        "<link name=\"link_a\">"
        "  <inertial>"
        "    <mass value=\"1.0\"/>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
        "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
        "  </inertial>"
        "  <collision>"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "<joint name=\"joint_b\" type=\"fixed\">"
        "  <parent link=\"link_a\"/>"
        "  <child link=\"link_b\"/>"
        "  <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
        "</joint>"
        "<link name=\"link_b\">"
        "  <inertial>"
        "    <mass value=\"1.0\"/>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
        "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
        "  </inertial>"
        "  <collision>"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "  <joint name=\"panda_joint1\" type=\"prismatic\">"
        "    <axis xyz=\"1 0 0\"/>"
        "    <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
        "    <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" "
        "soft_upper_limit=\"0.089\"/>"
        "    <parent link=\"link_b\"/>"
        "    <child link=\"link_c\"/>"
        "    <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
        "  </joint>"
        "<link name=\"link_c\">"
        "  <inertial>"
        "    <mass value=\"1.0\"/>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
        "    <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
        "  </inertial>"
        "  <collision>"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "  <joint name=\"mim_f\" type=\"prismatic\">"
        "    <axis xyz=\"1 0 0\"/>"
        "    <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
        "    <parent link=\"link_c\"/>"
        "    <child link=\"link_d\"/>"
        "    <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
        "    <mimic joint=\"joint_f\" multiplier=\"1.5\" offset=\"0.1\"/>"
        "  </joint>"
        "  <joint name=\"joint_f\" type=\"prismatic\">"
        "    <axis xyz=\"1 0 0\"/>"
        "    <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.19\" velocity=\"0.2\"/>"
        "    <parent link=\"link_d\"/>"
        "    <child link=\"link_e\"/>"
        "    <origin rpy=\" 0.0 0.0 0.0 \" xyz=\"0.1 0.1 0 \"/>"
        "  </joint>"
        "<link name=\"link_d\">"
        "  <collision>"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "<link name=\"link_e\">"
        "  <collision>"
        "    <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </collision>"
        "  <visual>"
        "    <origin rpy=\"0 1 0\" xyz=\"0 0.1 0\"/>"
        "    <geometry>"
        "      <box size=\"1 2 1\" />"
        "    </geometry>"
        "  </visual>"
        "</link>"
        "</robot>";

    static const std::string SMODEL2 =
        "<?xml version=\"1.0\" ?>"
        "<robot name=\"one_robot\">"
        "<virtual_joint name=\"base_joint\" child_link=\"base_link\" parent_frame=\"odom_combined\" type=\"planar\"/>"
        "<group name=\"base_from_joints\">"
        "<joint name=\"base_joint\"/>"
        "<joint name=\"panda_joint0\"/>"
        "<joint name=\"panda_joint1\"/>"
        "</group>"
        "<group name=\"mim_joints\">"
        "<joint name=\"joint_f\"/>"
        "<joint name=\"mim_f\"/>"
        "</group>"
        "<group name=\"base_with_subgroups\">"
        "<group name=\"base_from_base_to_tip\"/>"
        "<joint name=\"panda_joint1\"/>"
        "</group>"
        "<group name=\"base_from_base_to_tip\">"
        "<chain base_link=\"base_link\" tip_link=\"link_b\"/>"
        "<joint name=\"base_joint\"/>"
        "</group>"
        "<group name=\"arm\">"
        "<chain base_link=\"base_link\" tip_link=\"link_e\"/>"
        "<joint name=\"base_joint\"/>"
        "</group>"
        "<group name=\"base_with_bad_subgroups\">"
        "<group name=\"error\"/>"
        "</group>"
        "</robot>";

    urdf::ModelInterfaceSharedPtr urdf_model = urdf::parseURDF(MODEL2);
    srdf::ModelSharedPtr srdf_model(new srdf::Model());
    srdf_model->initString(*urdf_model, SMODEL2);
    robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
  }

  void TearDown() override
  {
  }

protected:
  moveit::core::RobotModelConstPtr robot_model_;
};

std::size_t generateTestTraj(std::vector<std::shared_ptr<moveit::core::RobotState>>& traj,
                             const moveit::core::RobotModelConstPtr& robot_model_)
{
  traj.clear();

  std::shared_ptr<moveit::core::RobotState> robot_state(new moveit::core::RobotState(robot_model_));
  robot_state->setToDefaultValues();
  double ja, jc;

  // 3 waypoints with default joints
  for (std::size_t traj_ix = 0; traj_ix < 3; ++traj_ix)
  {
    traj.push_back(std::make_shared<moveit::core::RobotState>(*robot_state));
  }

  ja = robot_state->getVariablePosition("panda_joint0");
  jc = robot_state->getVariablePosition("panda_joint1");

  // 4th waypoint with a small jump of 0.01 in revolute joint and prismatic joint. This should not be considered a jump
  ja = ja - 0.01;
  robot_state->setVariablePosition("panda_joint0", ja);
  jc = jc - 0.01;
  robot_state->setVariablePosition("panda_joint1", jc);
  traj.push_back(std::make_shared<moveit::core::RobotState>(*robot_state));

  // 5th waypoint with a large jump of 1.01 in first revolute joint
  ja = ja + 1.01;
  robot_state->setVariablePosition("panda_joint0", ja);
  traj.push_back(std::make_shared<moveit::core::RobotState>(*robot_state));

  // 6th waypoint with a large jump of 1.01 in first prismatic joint
  jc = jc + 1.01;
  robot_state->setVariablePosition("panda_joint1", jc);
  traj.push_back(std::make_shared<moveit::core::RobotState>(*robot_state));

  // 7th waypoint with no jump
  traj.push_back(std::make_shared<moveit::core::RobotState>(*robot_state));

  return traj.size();
}

TEST_F(OneRobot, testGenerateTrajectory)
{
  std::vector<std::shared_ptr<moveit::core::RobotState>> traj;

  // The full trajectory should be of length 7
  const std::size_t expected_full_traj_len = 7;

  // Generate a test trajectory
  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);

  // Test the generateTestTraj still generates a trajectory of length 7
  EXPECT_EQ(full_traj_len, expected_full_traj_len);  // full traj should be 7 waypoints long
}

TEST_F(OneRobot, checkAbsoluteJointSpaceJump)
{
  const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm");
  std::vector<std::shared_ptr<moveit::core::RobotState>> traj;

  // A revolute joint jumps 1.01 at the 5th waypoint and a prismatic joint jumps 1.01 at the 6th waypoint
  const std::size_t expected_revolute_jump_traj_len = 4;
  const std::size_t expected_prismatic_jump_traj_len = 5;

  // Pre-compute expected results for tests
  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);
  const double expected_revolute_jump_fraction = (double)expected_revolute_jump_traj_len / (double)full_traj_len;
  const double expected_prismatic_jump_fraction = (double)expected_prismatic_jump_traj_len / (double)full_traj_len;

  // Container for results
  double fraction;

  // Direct call of absolute version
  fraction = moveit::core::CartesianInterpolator::checkAbsoluteJointSpaceJump(joint_model_group, traj, 1.0, 1.0);
  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());  // traj should be cut
  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);

  // Indirect call using checkJointSpaceJumps
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(1.0, 1.0));
  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());  // traj should be cut before the revolute jump
  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);

  // Test revolute joints
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(1.0, 0.0));
  EXPECT_EQ(expected_revolute_jump_traj_len, traj.size());  // traj should be cut before the revolute jump
  EXPECT_NEAR(expected_revolute_jump_fraction, fraction, 0.01);

  // Test prismatic joints
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(0.0, 1.0));
  EXPECT_EQ(expected_prismatic_jump_traj_len, traj.size());  // traj should be cut before the prismatic jump
  EXPECT_NEAR(expected_prismatic_jump_fraction, fraction, 0.01);

  // Ignore all absolute jumps
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(0.0, 0.0));
  EXPECT_EQ(full_traj_len, traj.size());  // traj should not be cut
  EXPECT_NEAR(1.0, fraction, 0.01);
}

TEST_F(OneRobot, checkRelativeJointSpaceJump)
{
  const moveit::core::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup("arm");
  std::vector<std::shared_ptr<moveit::core::RobotState>> traj;

  // The first large jump of 1.01 occurs at the 5th waypoint so the test should trim the trajectory to length 4
  const std::size_t expected_relative_jump_traj_len = 4;

  // Pre-compute expected results for tests
  std::size_t full_traj_len = generateTestTraj(traj, robot_model_);
  const double expected_relative_jump_fraction = (double)expected_relative_jump_traj_len / (double)full_traj_len;

  // Container for results
  double fraction;

  // Direct call of relative version: 1.01 > 2.97 * (0.01 * 2 + 1.01 * 2)/6.
  fraction = moveit::core::CartesianInterpolator::checkRelativeJointSpaceJump(joint_model_group, traj, 2.97);
  EXPECT_EQ(expected_relative_jump_traj_len, traj.size());  // traj should be cut before the first jump of 1.01
  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);

  // Indirect call of relative version using checkJointSpaceJumps
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(2.97));
  EXPECT_EQ(expected_relative_jump_traj_len, traj.size());  // traj should be cut before the first jump of 1.01
  EXPECT_NEAR(expected_relative_jump_fraction, fraction, 0.01);

  // Trajectory should not be cut: 1.01 < 2.98 * (0.01 * 2 + 1.01 * 2)/6.
  generateTestTraj(traj, robot_model_);
  fraction = moveit::core::CartesianInterpolator::checkJointSpaceJump(joint_model_group, traj,
                                                                      moveit::core::JumpThreshold(2.98));
  EXPECT_EQ(full_traj_len, traj.size());  // traj should not be cut
  EXPECT_NEAR(1.0, fraction, 0.01);
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


<|start_filename|>moveit_ros/moveit_servo/test/basic_servo_tests.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2020, PickNik LLC
 *  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 PickNik LLC 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: <NAME>, <NAME>
   Desc:   Basic functionality tests
*/

// System
#include <string>

// ROS
#include <ros/ros.h>

// Testing
#include <gtest/gtest.h>

// Servo
#include <moveit_servo/make_shared_from_pool.h>
#include <moveit_servo/servo.h>

static const std::string LOGNAME = "basic_servo_tests";

namespace moveit_servo
{
class ServoFixture : public ::testing::Test
{
public:
  void SetUp() override
  {
    // Wait for several key topics / parameters
    ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states");
    while (!nh_.hasParam("/robot_description") && ros::ok())
    {
      ros::Duration(0.1).sleep();
    }

    // Load the planning scene monitor
    planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
    planning_scene_monitor_->startSceneMonitor();
    planning_scene_monitor_->startStateMonitor();
    planning_scene_monitor_->startWorldGeometryMonitor(
        planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC,
        planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
        false /* skip octomap monitor */);

    // Create moveit_servo
    servo_ = std::make_shared<Servo>(nh_, planning_scene_monitor_);
  }
  void TearDown() override
  {
  }

protected:
  void enforceVelLimits(Eigen::ArrayXd& delta_theta)
  {
    servo_->servo_calcs_->enforceVelLimits(delta_theta);
  }
  ros::NodeHandle nh_{ "~" };
  planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;
  moveit_servo::ServoPtr servo_;
};  // class ServoFixture

TEST_F(ServoFixture, SendTwistStampedTest)
{
  servo_->start();

  auto parameters = servo_->getParameters();

  // count trajectory messages sent by servo
  size_t received_count = 0;
  boost::function<void(const trajectory_msgs::JointTrajectoryConstPtr&)> traj_callback =
      [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; };
  auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback);

  // Create publisher to send servo commands
  auto twist_stamped_pub = nh_.advertise<geometry_msgs::TwistStamped>(parameters.cartesian_command_in_topic, 1);

  constexpr double test_duration = 1.0;
  const double publish_period = parameters.publish_period;
  const size_t num_commands = static_cast<size_t>(test_duration / publish_period);

  // Set the rate differently from the publish period from the parameters to show that
  // the number of outputs is set by the number of commands sent and not the rate they are sent.
  ros::Rate publish_rate(2. / publish_period);

  // Send a few Cartesian velocity commands
  for (size_t i = 0; i < num_commands && ros::ok(); ++i)
  {
    auto msg = moveit::util::make_shared_from_pool<geometry_msgs::TwistStamped>();
    msg->header.stamp = ros::Time::now();
    msg->header.frame_id = "panda_link0";
    msg->twist.angular.y = 1.0;

    // Send the message
    twist_stamped_pub.publish(msg);
    publish_rate.sleep();
  }

  EXPECT_GT(received_count, num_commands - 20);
  EXPECT_GT(received_count, (unsigned)0);
  EXPECT_LT(received_count, num_commands + 20);
  servo_->setPaused(true);
}

TEST_F(ServoFixture, SendJointServoTest)
{
  servo_->start();

  auto parameters = servo_->getParameters();

  // count trajectory messages sent by servo
  size_t received_count = 0;
  boost::function<void(const trajectory_msgs::JointTrajectoryConstPtr&)> traj_callback =
      [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& /*msg*/) { ++received_count; };
  auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback);

  // Create publisher to send servo commands
  auto joint_servo_pub = nh_.advertise<control_msgs::JointJog>(parameters.joint_command_in_topic, 1);

  constexpr double test_duration = 1.0;
  const double publish_period = parameters.publish_period;
  const size_t num_commands = static_cast<size_t>(test_duration / publish_period);

  // Set the rate differently from the publish period from the parameters to show that
  // the number of outputs is set by the number of commands sent and not the rate they are sent.
  ros::Rate publish_rate(2. / publish_period);

  // Send a few joint velocity commands
  for (size_t i = 0; i < num_commands && ros::ok(); ++i)
  {
    auto msg = moveit::util::make_shared_from_pool<control_msgs::JointJog>();
    msg->header.stamp = ros::Time::now();
    msg->header.frame_id = "panda_link3";
    msg->velocities.push_back(0.1);

    // Send the message
    joint_servo_pub.publish(msg);
    publish_rate.sleep();
  }

  EXPECT_GT(received_count, num_commands - 20);
  EXPECT_GT(received_count, (unsigned)0);
  EXPECT_LT(received_count, num_commands + 20);
  servo_->setPaused(true);
}

// This a friend test of a private member function
TEST_F(ServoFixture, EnforceVelLimitsTest)
{
  auto parameters = servo_->getParameters();
  const double publish_period = parameters.publish_period;

  // Request joint angle changes that are too fast, given the control period in servo settings YAML file.
  Eigen::ArrayXd delta_theta(7);
  delta_theta[0] = 0;  // rad
  delta_theta[1] = 0.01;
  delta_theta[2] = 0.02;
  delta_theta[3] = 0.03;
  delta_theta[4] = 0.04;
  delta_theta[5] = 0.05;
  delta_theta[6] = 0.06;

  // Store the original joint commands for comparison before applying velocity scaling.
  Eigen::ArrayXd orig_delta_theta = delta_theta;
  enforceVelLimits(delta_theta);

  // From Panda arm MoveIt joint_limits.yaml. The largest velocity limits for a joint.
  const double panda_max_joint_vel = 2.610;  // rad/s
  const double velocity_scaling_factor = panda_max_joint_vel / (orig_delta_theta.maxCoeff() / publish_period);
  const double tolerance = 5e-3;
  for (int i = 0; i < 7; ++i)
  {
    EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance);
  }

  // Now, negative joint angle deltas. Some will result to velocities
  // greater than the arm joint velocity limits.
  delta_theta[0] = 0;  // rad
  delta_theta[1] = -0.01;
  delta_theta[2] = -0.02;
  delta_theta[3] = -0.03;
  delta_theta[4] = -0.04;
  delta_theta[5] = -0.05;
  delta_theta[6] = -0.06;

  // Store the original joint commands for comparison before applying velocity scaling.
  orig_delta_theta = delta_theta;
  enforceVelLimits(delta_theta);
  for (int i = 0; i < 7; ++i)
  {
    EXPECT_NEAR(orig_delta_theta(i) * velocity_scaling_factor, delta_theta(i), tolerance);
  }

  // Final test with joint angle deltas that will result in velocities
  // below the lowest Panda arm joint velocity limit.
  delta_theta[0] = 0;  // rad
  delta_theta[1] = -0.013;
  delta_theta[2] = 0.023;
  delta_theta[3] = -0.004;
  delta_theta[4] = 0.021;
  delta_theta[5] = 0.012;
  delta_theta[6] = 0.0075;

  // Store the original joint commands for comparison before applying velocity scaling.
  orig_delta_theta = delta_theta;
  enforceVelLimits(delta_theta);
  for (int i = 0; i < 7; ++i)
  {
    EXPECT_NEAR(orig_delta_theta(i), delta_theta(i), tolerance);
  }
}
}  // namespace moveit_servo

int main(int argc, char** argv)
{
  ros::init(argc, argv, LOGNAME);
  testing::InitGoogleTest(&argc, argv);

  ros::AsyncSpinner spinner(8);
  spinner.start();

  int result = RUN_ALL_TESTS();
  return result;
}


<|start_filename|>moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2018 Pilz GmbH & Co. KG
 *  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 Pilz GmbH & Co. KG 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.
 *********************************************************************/

#include "pilz_industrial_motion_planner/joint_limits_aggregator.h"
#include "pilz_industrial_motion_planner/joint_limits_interface_extension.h"

#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_model_loader/robot_model_loader.h>

#include <vector>

pilz_industrial_motion_planner::JointLimitsContainer
pilz_industrial_motion_planner::JointLimitsAggregator::getAggregatedLimits(
    const ros::NodeHandle& nh, const std::vector<const moveit::core::JointModel*>& joint_models)
{
  JointLimitsContainer container;

  ROS_INFO_STREAM("Reading limits from namespace " << nh.getNamespace());

  // Iterate over all joint models and generate the map
  for (auto joint_model : joint_models)
  {
    JointLimit joint_limit;

    // If there is something defined for the joint on the parameter server
    if (joint_limits_interface::getJointLimits(joint_model->getName(), nh, joint_limit))
    {
      if (joint_limit.has_position_limits)
      {
        checkPositionBoundsThrowing(joint_model, joint_limit);
      }
      else
      {
        updatePositionLimitFromJointModel(joint_model, joint_limit);
      }

      if (joint_limit.has_velocity_limits)
      {
        checkVelocityBoundsThrowing(joint_model, joint_limit);
      }
      else
      {
        updateVelocityLimitFromJointModel(joint_model, joint_limit);
      }
    }
    else
    {
      // If there is nothing defined for this joint on the parameter server just
      // update the values by the values of
      // the urdf

      updatePositionLimitFromJointModel(joint_model, joint_limit);
      updateVelocityLimitFromJointModel(joint_model, joint_limit);
    }

    // Update max_deceleration if no max_acceleration has been set
    if (joint_limit.has_acceleration_limits && !joint_limit.has_deceleration_limits)
    {
      joint_limit.max_deceleration = -joint_limit.max_acceleration;
      joint_limit.has_deceleration_limits = true;
    }

    // Insert the joint limit into the map
    container.addLimit(joint_model->getName(), joint_limit);
  }

  return container;
}

void pilz_industrial_motion_planner::JointLimitsAggregator::updatePositionLimitFromJointModel(
    const moveit::core::JointModel* joint_model, JointLimit& joint_limit)
{
  switch (joint_model->getVariableBounds().size())
  {
    // LCOV_EXCL_START
    case 0:
      ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName());
      break;
    // LCOV_EXCL_STOP
    case 1:
      joint_limit.has_position_limits = joint_model->getVariableBounds()[0].position_bounded_;
      joint_limit.min_position = joint_model->getVariableBounds()[0].min_position_;
      joint_limit.max_position = joint_model->getVariableBounds()[0].max_position_;
      break;
    // LCOV_EXCL_START
    default:
      ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
      joint_limit.has_position_limits = true;
      joint_limit.min_position = 0;
      joint_limit.max_position = 0;
      break;
      // LCOV_EXCL_STOP
  }

  ROS_DEBUG_STREAM("Limit(" << joint_model->getName() << " min:" << joint_limit.min_position
                            << " max:" << joint_limit.max_position);
}

void pilz_industrial_motion_planner::JointLimitsAggregator::updateVelocityLimitFromJointModel(
    const moveit::core::JointModel* joint_model, JointLimit& joint_limit)
{
  switch (joint_model->getVariableBounds().size())
  {
    // LCOV_EXCL_START
    case 0:
      ROS_WARN_STREAM("no bounds set for joint " << joint_model->getName());
      break;
    // LCOV_EXCL_STOP
    case 1:
      joint_limit.has_velocity_limits = joint_model->getVariableBounds()[0].velocity_bounded_;
      joint_limit.max_velocity = joint_model->getVariableBounds()[0].max_velocity_;
      break;
    // LCOV_EXCL_START
    default:
      ROS_WARN_STREAM("Multi-DOF-Joint '" << joint_model->getName() << "' not supported.");
      joint_limit.has_velocity_limits = true;
      joint_limit.max_velocity = 0;
      break;
      // LCOV_EXCL_STOP
  }
}

void pilz_industrial_motion_planner::JointLimitsAggregator::checkPositionBoundsThrowing(
    const moveit::core::JointModel* joint_model, const JointLimit& joint_limit)
{
  // Check min position
  if (!joint_model->satisfiesPositionBounds(&joint_limit.min_position))
  {
    throw AggregationBoundsViolationException("min_position of " + joint_model->getName() +
                                              " violates min limit from URDF");
  }

  // Check max position
  if (!joint_model->satisfiesPositionBounds(&joint_limit.max_position))
  {
    throw AggregationBoundsViolationException("max_position of " + joint_model->getName() +
                                              " violates max limit from URDF");
  }
}

void pilz_industrial_motion_planner::JointLimitsAggregator::checkVelocityBoundsThrowing(
    const moveit::core::JointModel* joint_model, const JointLimit& joint_limit)
{
  // Check min position
  if (!joint_model->satisfiesVelocityBounds(&joint_limit.max_velocity))
  {
    throw AggregationBoundsViolationException("max_velocity of " + joint_model->getName() +
                                              " violates velocity limit from URDF");
  }
}


<|start_filename|>.docker/release/Dockerfile<|end_filename|>
# Full debian-based install of MoveIt using apt-get

ARG ROS_DISTRO=noetic
FROM ros:${ROS_DISTRO}-ros-base
MAINTAINER <NAME> <EMAIL>

# Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
RUN apt-get update -q && \
    apt-get dist-upgrade -q -y && \
    apt-get install -y ros-${ROS_DISTRO}-moveit-* && \
    rm -rf /var/lib/apt/lists/*


<|start_filename|>moveit_core/collision_detection_bullet/src/bullet_integration/bullet_bvh_manager.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD-2-Clause)
 *
 * Copyright (c) 2017, Southwest Research Institute
 * 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.
 *
 * 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.
 *********************************************************************/

/* Authors: <NAME>, <NAME> */

#include <moveit/collision_detection_bullet/bullet_integration/bullet_bvh_manager.h>
#include <map>
#include <utility>

namespace collision_detection_bullet
{
BulletBVHManager::BulletBVHManager()
{
  dispatcher_ = std::make_unique<btCollisionDispatcher>(&coll_config_);

  dispatcher_->registerCollisionCreateFunc(BOX_SHAPE_PROXYTYPE, BOX_SHAPE_PROXYTYPE,
                                           coll_config_.getCollisionAlgorithmCreateFunc(CONVEX_SHAPE_PROXYTYPE,
                                                                                        CONVEX_SHAPE_PROXYTYPE));

  dispatcher_->setDispatcherFlags(dispatcher_->getDispatcherFlags() &
                                  ~btCollisionDispatcher::CD_USE_RELATIVE_CONTACT_BREAKING_THRESHOLD);

  broadphase_ = std::make_unique<btDbvtBroadphase>();

  broadphase_->getOverlappingPairCache()->setOverlapFilterCallback(&filter_callback_);

  contact_distance_ = BULLET_DEFAULT_CONTACT_DISTANCE;
}

BulletBVHManager::~BulletBVHManager()
{
  // clean up remaining objects
  for (const std::pair<const std::string, collision_detection_bullet::CollisionObjectWrapperPtr>& cow : link2cow_)
    removeCollisionObjectFromBroadphase(cow.second, broadphase_, dispatcher_);
}

bool BulletBVHManager::hasCollisionObject(const std::string& name) const
{
  return (link2cow_.find(name) != link2cow_.end());
}

bool BulletBVHManager::removeCollisionObject(const std::string& name)
{
  auto it = link2cow_.find(name);
  if (it != link2cow_.end())
  {
    CollisionObjectWrapperPtr& cow = it->second;
    removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_);
    link2cow_.erase(name);
    return true;
  }

  return false;
}

bool BulletBVHManager::enableCollisionObject(const std::string& name)
{
  auto it = link2cow_.find(name);
  if (it != link2cow_.end())
  {
    it->second->m_enabled = true;
    return true;
  }

  return false;
}

bool BulletBVHManager::disableCollisionObject(const std::string& name)
{
  auto it = link2cow_.find(name);
  if (it != link2cow_.end())
  {
    it->second->m_enabled = false;
    return true;
  }

  return false;
}

void BulletBVHManager::setCollisionObjectsTransform(const std::string& name, const Eigen::Isometry3d& pose)
{
  // TODO(j-petit): Find a way to remove this check. Need to store information in CollisionEnv transforms with geometry
  auto it = link2cow_.find(name);
  if (it != link2cow_.end())
  {
    CollisionObjectWrapperPtr& cow = it->second;
    btTransform tf = convertEigenToBt(pose);
    cow->setWorldTransform(tf);

    // Now update Broadphase AABB (See BulletWorld updateSingleAabb function)
    if (cow->getBroadphaseHandle())
      updateBroadphaseAABB(cow, broadphase_, dispatcher_);
  }
}

void BulletBVHManager::setActiveCollisionObjects(const std::vector<std::string>& names)
{
  active_ = names;

  // Now need to update the broadphase with correct aabb
  for (std::pair<const std::string, CollisionObjectWrapperPtr>& co : link2cow_)
  {
    CollisionObjectWrapperPtr& cow = co.second;
    updateCollisionObjectFilters(active_, *cow);

    // The broadphase tree structure has to be updated, therefore remove and add is necessary
    removeCollisionObjectFromBroadphase(cow, broadphase_, dispatcher_);
    addCollisionObjectToBroadphase(cow, broadphase_, dispatcher_);
  }
}

const std::vector<std::string>& BulletBVHManager::getActiveCollisionObjects() const
{
  return active_;
}

void BulletBVHManager::setContactDistanceThreshold(double contact_distance)
{
  contact_distance_ = contact_distance;

  for (std::pair<const std::string, CollisionObjectWrapperPtr>& co : link2cow_)
  {
    CollisionObjectWrapperPtr& cow = co.second;
    cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
    if (cow->getBroadphaseHandle())
      updateBroadphaseAABB(cow, broadphase_, dispatcher_);
  }
}

double BulletBVHManager::getContactDistanceThreshold() const
{
  return contact_distance_;
}

const std::map<std::string, CollisionObjectWrapperPtr>& BulletBVHManager::getCollisionObjects() const
{
  return link2cow_;
}

}  // namespace collision_detection_bullet


<|start_filename|>moveit_core/python/tools/include/moveit/python/pybind_rosmsg_typecasters.h<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of <NAME> may not 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: <NAME> */

#pragma once

#include <pybind11/pybind11.h>
#include <ros/duration.h>
#include <ros/serialization.h>

/** Provide pybind11 type converters for ROS types */

namespace moveit
{
namespace python
{
PYBIND11_EXPORT pybind11::object createMessage(const std::string& ros_msg_name);
PYBIND11_EXPORT bool convertible(const pybind11::handle& h, const char* ros_msg_name);

}  // namespace python
}  // namespace moveit

namespace pybind11
{
namespace detail
{
/// Convert ros::Duration / ros::WallDuration into a float
template <typename T>
struct DurationCaster
{
  // C++ -> Python
  static handle cast(T&& src, return_value_policy /* policy */, handle /* parent */)
  {
    return PyFloat_FromDouble(src.toSec());
  }

  // Python -> C++
  bool load(handle src, bool convert)
  {
    if (hasattr(src, "to_sec"))
    {
      value = T(src.attr("to_sec")().cast<double>());
    }
    else if (convert)
    {
      value = T(src.cast<double>());
    }
    else
      return false;
    return true;
  }

  PYBIND11_TYPE_CASTER(T, _("Duration"));
};

template <>
struct type_caster<ros::Duration> : DurationCaster<ros::Duration>
{
};

template <>
struct type_caster<ros::WallDuration> : DurationCaster<ros::WallDuration>
{
};

/// Convert ROS message types (C++ <-> python)
template <typename T>
struct type_caster<T, enable_if_t<ros::message_traits::IsMessage<T>::value>>
{
  // C++ -> Python
  static handle cast(const T& src, return_value_policy /* policy */, handle /* parent */)
  {
    // serialize src into (python) buffer
    std::size_t size = ros::serialization::serializationLength(src);
    object pbuffer = reinterpret_steal<object>(PyBytes_FromStringAndSize(nullptr, size));
    ros::serialization::OStream stream(reinterpret_cast<uint8_t*>(PyBytes_AsString(pbuffer.ptr())), size);
    ros::serialization::serialize(stream, src);
    // deserialize python type from buffer
    object msg = moveit::python::createMessage(ros::message_traits::DataType<T>::value());
    msg.attr("deserialize")(pbuffer);
    return msg.release();
  }

  // Python -> C++
  bool load(handle src, bool /*convert*/)
  {
    if (!moveit::python::convertible(src, ros::message_traits::DataType<T>::value()))
      return false;
    // serialize src into (python) buffer
    object pstream = module::import("io").attr("BytesIO")();
    src.attr("serialize")(pstream);
    object pbuffer = pstream.attr("getvalue")();
    // deserialize C++ type from buffer
    char* cbuffer = nullptr;
    Py_ssize_t size;
    PyBytes_AsStringAndSize(pbuffer.ptr(), &cbuffer, &size);
    ros::serialization::IStream cstream(const_cast<uint8_t*>(reinterpret_cast<const uint8_t*>(cbuffer)), size);
    ros::serialization::deserialize(cstream, value);
    return true;
  }

  PYBIND11_TYPE_CASTER(T, _<T>());
};
}  // namespace detail
}  // namespace pybind11


<|start_filename|>moveit_core/python/tools/src/pybind_rosmsg_typecasters.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of <NAME> may not 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: <NAME> */

#include <moveit/python/pybind_rosmsg_typecasters.h>

namespace py = pybind11;
namespace moveit
{
namespace python
{
py::object createMessage(const std::string& ros_msg_name)
{
  // find delimiting '/' in ros msg name
  std::size_t pos = ros_msg_name.find('/');
  // import module
  py::module m = py::module::import((ros_msg_name.substr(0, pos) + ".msg").c_str());
  // retrieve type instance
  py::object cls = m.attr(ros_msg_name.substr(pos + 1).c_str());
  // create message instance
  return cls();
}

bool convertible(const pybind11::handle& h, const char* ros_msg_name)
{
  try
  {
    PyObject* o = h.attr("_type").ptr();
    return py::cast<std::string>(o) == ros_msg_name;
  }
  catch (const std::exception& e)
  {
    return false;
  }
}
}  // namespace python
}  // namespace moveit


<|start_filename|>moveit_core/robot_state/src/pyrobot_state.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>
#include <moveit/python/pybind_rosmsg_typecasters.h>

#include <moveit/robot_state/robot_state.h>
#include <moveit/robot_state/conversions.h>

namespace py = pybind11;
using namespace robot_state;

void def_robot_state_bindings(py::module& m)
{
  m.doc() = "Representation of a robot's state. This includes position, velocity, acceleration and effort.";
  py::class_<RobotState, RobotStatePtr>(m, "RobotState")
      .def(py::init<const robot_model::RobotModelConstPtr&>(), py::arg("robot_model"))
      .def("setToRandomPositions", py::overload_cast<>(&RobotState::setToRandomPositions))
      .def("setToRandomPositions", py::overload_cast<const JointModelGroup*>(&RobotState::setToRandomPositions))
      .def("getJointModelGroup", &RobotState::getJointModelGroup, py::return_value_policy::reference)
      .def("getJointModel", &RobotState::getJointModel, py::return_value_policy::reference)
      .def("getLinkModel", &RobotState::getLinkModel, py::return_value_policy::reference)
      .def("getVariableNames", &RobotState::getVariableNames)
      .def("getVariablePositions", py::overload_cast<>(&RobotState::getVariablePositions, py::const_),
           py::return_value_policy::reference)
      .def("getVariableCount", &RobotState::getVariableCount)
      .def("hasVelocities", &RobotState::hasVelocities)
      .def("setJointGroupPositions",
           py::overload_cast<const std::string&, const std::vector<double>&>(&RobotState::setJointGroupPositions))
      .def("setJointGroupPositions",
           py::overload_cast<const JointModelGroup*, const std::vector<double>&>(&RobotState::setJointGroupPositions))
      .def("satisfiesBounds",
           py::overload_cast<const JointModelGroup*, double>(&RobotState::satisfiesBounds, py::const_),
           py::arg("joint_model_group"), py::arg("margin") = 0.0)
      .def("update", &RobotState::update, py::arg("force") = false)
      .def("printStateInfo",
           [](const RobotState& s) {
             std::stringstream ss;
             s.printStateInfo(ss);
             return ss.str();
           })
      .def("printStatePositions",
           [](const RobotState& s) {
             std::stringstream ss;
             s.printStatePositions(ss);
             return ss.str();
           })
      .def("__repr__",
           [](const RobotState& s) {
             std::stringstream ss;
             s.printStatePositions(ss);
             return ss.str();
           })
      //
      ;

  m.def("jointStateToRobotState", &jointStateToRobotState);
  m.def(
      "robotStateToRobotStateMsg",
      [](const RobotState& state, bool copy_attached_bodies) {
        moveit_msgs::RobotState state_msg;
        robotStateToRobotStateMsg(state, state_msg, copy_attached_bodies);
        return state_msg;
      },
      py::arg("state"), py::arg("copy_attached_bodies") = true);
}


<|start_filename|>moveit_core/python/pymoveit_core.cpp<|end_filename|>
/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2021, <NAME>
 *  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.
 *   * The name of Peter Mitrano may not 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: <NAME> */

#include <pybind11/pybind11.h>
#include <urdf_parser/urdf_parser.h>
#include <moveit/robot_model/robot_model.h>
#include <srdfdom/model.h>

namespace py = pybind11;

// Each of these functions defines the bindings for a namespace/subfolder within moveit_core
void def_collision_detection_bindings(py::module& contact);

void def_robot_model_bindings(py::module& m);

void def_robot_state_bindings(py::module& m);

void def_transforms_bindings(py::module& m);

void def_planning_scene_bindings(py::module& m);

void def_kinematic_constraints_bindings(py::module& m);

auto load_robot_model(const std::string& urdf_path, const std::string& srdf_path)
{
  auto urdf_model = urdf::parseURDFFile(urdf_path);
  auto srdf_model = std::make_shared<srdf::Model>();
  srdf_model->initFile(*urdf_model, srdf_path);
  auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
  return robot_model;
}

PYBIND11_MODULE(pymoveit_core, m)
{
  auto collision_detection_m = m.def_submodule("collision_detection");
  auto planning_scene_m = m.def_submodule("planning_scene");
  auto robot_model_m = m.def_submodule("robot_model");
  auto robot_state_m = m.def_submodule("robot_state");
  auto kinematic_constraints_m = m.def_submodule("kinematic_constraints");
  auto transforms_m = m.def_submodule("transforms");

  def_collision_detection_bindings(collision_detection_m);
  def_robot_model_bindings(robot_model_m);
  def_robot_state_bindings(robot_state_m);
  def_planning_scene_bindings(planning_scene_m);
  def_transforms_bindings(transforms_m);
  def_kinematic_constraints_bindings(kinematic_constraints_m);

  // convenience function
  m.def("load_robot_model", &load_robot_model);
}
