diff --git a/core/include/moveit/task_constructor/solvers/joint_interpolation.h b/core/include/moveit/task_constructor/solvers/joint_interpolation.h new file mode 100644 index 00000000..64a5ca36 --- /dev/null +++ b/core/include/moveit/task_constructor/solvers/joint_interpolation.h @@ -0,0 +1,74 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: plan using MoveIt's PlanningPipeline +*/ + +#pragma once + +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +MOVEIT_CLASS_FORWARD(JointInterpolationPlanner) + +/** Use MoveIt's PlanningPipeline to plan a trajectory between to scenes */ +class JointInterpolationPlanner : public PlannerInterface { +public: + JointInterpolationPlanner(); + + void setGroup(const std::string &group) { setProperty("group", group); } + void setTimeout(double timeout) { setProperty("timeout", timeout); } + + void init(const moveit::core::RobotModelConstPtr& robot_model) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override; + + bool plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target, + const core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override; +}; + +} } } diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index be4153cc..cbe3d2dc 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -14,6 +14,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/solvers/planner_interface.h ${PROJECT_INCLUDE}/solvers/cartesian_path.h + ${PROJECT_INCLUDE}/solvers/joint_interpolation.h ${PROJECT_INCLUDE}/solvers/pipeline_planner.h container.cpp @@ -27,6 +28,7 @@ add_library(${PROJECT_NAME} solvers/cartesian_path.cpp solvers/pipeline_planner.cpp + solvers/joint_interpolation.cpp ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) target_include_directories(${PROJECT_NAME} diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp new file mode 100644 index 00000000..682af2eb --- /dev/null +++ b/core/src/solvers/joint_interpolation.cpp @@ -0,0 +1,113 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Bielefeld University + * 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 Bielefeld University 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. + *********************************************************************/ + +/* Authors: Robert Haschke + Desc: Plan using simple interpolation in joint-space + validity checking +*/ + +#include +#include +#include + +namespace moveit { namespace task_constructor { namespace solvers { + +JointInterpolationPlanner::JointInterpolationPlanner() +{ + auto& p = properties(); + p.declare("max_step", 0.1, "max joint step"); + p.declare("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor"); + p.declare("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor"); +} + +void JointInterpolationPlanner::init(const core::RobotModelConstPtr &robot_model) +{ +} + +bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const planning_scene::PlanningSceneConstPtr to, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) +{ + const auto& props = properties(); + + // Get maximum joint distance + double d = 0.0; + const moveit::core::RobotState& from_state = from->getCurrentState(); + const moveit::core::RobotState& to_state = to->getCurrentState(); + for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels()) + d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm)); + if (d < 1e-6) + return true; // return empty result trajectory: no motion needed + + result = std::make_shared(from->getRobotModel(), jmg); + + // add first point + result->addSuffixWayPoint(from->getCurrentState(), 0.0); + + moveit::core::RobotState waypoint(from_state); + double delta = props.get("max_step") / d; + for (double t = delta; t < 1.0; t += delta) { + from_state.interpolate(to_state, t, waypoint); + result->addSuffixWayPoint(waypoint, t); + + if (from->isStateColliding(waypoint, jmg->getName())) + return false; + } + + // add goal point + result->addSuffixWayPoint(to->getCurrentState(), 1.0); + + // add timing, TODO: use a generic method to add timing via plugins + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, + props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); + + return true; +} + +bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr from, + const moveit::core::LinkModel &link, + const Eigen::Affine3d& target_eigen, + const moveit::core::JointModelGroup *jmg, + double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) +{ + throw std::runtime_error("Not yet implemented"); +} + +} } }