mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
implement path constraints for Move*
This commit is contained in:
parent
80ae01dcca
commit
d7d80c3499
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user