From d0c8f7f819f60a6c569ae57bd4d6ea6a13356652 Mon Sep 17 00:00:00 2001 From: llach Date: Thu, 5 Apr 2018 14:55:15 +0200 Subject: [PATCH] MoveRelative: added relative joint space goals to --- .../task_constructor/stages/move_relative.h | 3 ++ core/src/stages/move_relative.cpp | 39 +++++++++++++++++-- 2 files changed, 38 insertions(+), 4 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index 22e34260..14748322 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -63,6 +63,8 @@ public: void setMaxDistance(double distance); void setMinMaxDistance(double min_distance, double max_distance); + void setRelativeJointSpaceGoal(const std::map& goal); + void setPathConstraints(moveit_msgs::Constraints path_constraints){ setProperty("path_constraints", std::move(path_constraints)); } @@ -78,6 +80,7 @@ protected: protected: solvers::PlannerInterfacePtr planner_; + bool joint_space_goal_ = false; }; } } } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 2c788418..dc2b3340 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -57,11 +57,16 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf p.declare("twist", "Cartesian twist transform"); p.declare("direction", "Cartesian translation direction"); + p.declare>("relative_joint_space_goal", "Relative joint space goal"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); } +void MoveRelative::setRelativeJointSpaceGoal(const std::map& goal){ + setProperty("relative_joint_space_goal", goal); +} + void MoveRelative::setGroup(const std::string& group){ setProperty("group", group); } @@ -114,7 +119,7 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning } // only allow single target - size_t count_goals = props.countDefined({"twist", "direction"}); + size_t count_goals = props.countDefined({"twist", "direction", "relative_joint_space_goal"}); if (count_goals != 1) { if (count_goals == 0) ROS_WARN("MoveRelative: no goal defined"); else ROS_WARN("MoveRelative: cannot plan to multiple goals"); @@ -207,9 +212,35 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning target_eigen.translation() += linear; } - robot_trajectory::RobotTrajectoryPtr robot_trajectory; - bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, - props.get("path_constraints")); + goal = props.get("relative_joint_space_goal"); + if (!goal.empty()) { + + const auto &start_state = scene->getCurrentState(); + auto& current_state = scene->getCurrentStateNonConst(); + joint_space_goal_ = true; + + const std::map& relative_joint_position_map = boost::any_cast>(goal); + + for (auto j : relative_joint_position_map) { + double new_pos = (start_state.getVariablePosition(j.first) + j.second); + current_state.setVariablePosition(j.first, new_pos); + + auto jm = scene->getRobotModel()->getJointModel(j.first); + current_state.enforceBounds(jm); + current_state.update(true); + } + } + + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = false; + + if (joint_space_goal_) { + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, + props.get("path_constraints")); + } else { + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, + props.get("path_constraints")); + } // min_distance reached? if (min_distance > 0.0) {