implement path constraints for Move*

This commit is contained in:
v4hn 2018-02-26 15:13:30 +01:00 committed by Robert Haschke
parent 80ae01dcca
commit d7d80c3499
4 changed files with 22 additions and 3 deletions

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h>
#include <geometry_msgs/TwistStamped.h> #include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Vector3Stamped.h> #include <geometry_msgs/Vector3Stamped.h>
@ -62,6 +63,10 @@ public:
void setMaxDistance(double distance); void setMaxDistance(double distance);
void setMinMaxDistance(double min_distance, double max_distance); void setMinMaxDistance(double min_distance, double max_distance);
void setPathConstraints(moveit_msgs::Constraints path_constraints){
setProperty("path_constraints", std::move(path_constraints));
}
/// perform twist motion on specified link /// perform twist motion on specified link
void along(const geometry_msgs::TwistStamped& twist); void along(const geometry_msgs::TwistStamped& twist);
/// translate link along given direction /// translate link along given direction

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/stage.h> #include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/solvers/planner_interface.h> #include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/Constraints.h>
#include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h> #include <geometry_msgs/PointStamped.h>
@ -63,6 +64,10 @@ public:
/// move joint model group to given named pose /// move joint model group to given named pose
void setGoal(const std::string& joint_pose); void setGoal(const std::string& joint_pose);
void setPathConstraints(moveit_msgs::Constraints path_constraints){
setProperty("path_constraints", std::move(path_constraints));
}
protected: protected:
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene,
SubTrajectory &trajectory, Direction dir); SubTrajectory &trajectory, Direction dir);

View File

@ -57,6 +57,9 @@ MoveRelative::MoveRelative(std::string name, const solvers::PlannerInterfacePtr&
p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform"); p.declare<geometry_msgs::TwistStamped>("twist", "Cartesian twist transform");
p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction"); p.declare<geometry_msgs::Vector3Stamped>("direction", "Cartesian translation direction");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
} }
void MoveRelative::setGroup(const std::string& group){ void MoveRelative::setGroup(const std::string& group){
@ -204,7 +207,8 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning
} }
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); bool success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory,
props.get<moveit_msgs::Constraints>("path_constraints"));
// min_distance reached? // min_distance reached?
if (success && min_distance > 0.0) { if (success && min_distance > 0.0) {

View File

@ -55,6 +55,9 @@ MoveTo::MoveTo(std::string name, const solvers::PlannerInterfacePtr& planner)
p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose"); p.declare<geometry_msgs::PoseStamped>("pose", "Cartesian target pose");
p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point"); p.declare<geometry_msgs::PointStamped>("point", "Cartesian target point");
p.declare<std::string>("joint_pose", "named joint pose"); p.declare<std::string>("joint_pose", "named joint pose");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
} }
void MoveTo::setGroup(const std::string& group){ void MoveTo::setGroup(const std::string& group){
@ -108,6 +111,8 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
return false; return false;
} }
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr robot_trajectory; robot_trajectory::RobotTrajectoryPtr robot_trajectory;
bool success = false; bool success = false;
@ -118,7 +123,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str()); ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'", named_joint_pose.c_str(), group.c_str());
return false; return false;
} }
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory); success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
} else { } else {
// Cartesian targets require the link name // Cartesian targets require the link name
std::string link_name = props.get<std::string>("link"); std::string link_name = props.get<std::string>("link");
@ -167,7 +172,7 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP
tf::poseEigenToMsg(target_eigen, pose_msg.pose); tf::poseEigenToMsg(target_eigen, pose_msg.pose);
rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame");
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory); success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
} }
// store result // store result