mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge pull request #28 from ros-planning/pr-path-constraints
implement path constraints
This commit is contained in:
commit
205beea503
@ -55,15 +55,17 @@ public:
|
|||||||
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const planning_scene::PlanningSceneConstPtr to,
|
const planning_scene::PlanningSceneConstPtr to,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
|
|
||||||
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const moveit::core::LinkModel &link,
|
const moveit::core::LinkModel &link,
|
||||||
const Eigen::Affine3d& target,
|
const Eigen::Affine3d& target,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
|
||||||
};
|
};
|
||||||
|
|
||||||
} } }
|
} } }
|
||||||
|
|||||||
@ -62,14 +62,16 @@ public:
|
|||||||
const planning_scene::PlanningSceneConstPtr to,
|
const planning_scene::PlanningSceneConstPtr to,
|
||||||
const core::JointModelGroup *jmg,
|
const core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
|
||||||
|
|
||||||
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const moveit::core::LinkModel &link,
|
const moveit::core::LinkModel &link,
|
||||||
const Eigen::Affine3d& target,
|
const Eigen::Affine3d& target,
|
||||||
const core::JointModelGroup *jmg,
|
const core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
planning_pipeline::PlanningPipelinePtr planner_;
|
planning_pipeline::PlanningPipelinePtr planner_;
|
||||||
|
|||||||
@ -39,6 +39,7 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
#include <moveit/macros/class_forward.h>
|
#include <moveit/macros/class_forward.h>
|
||||||
|
#include <moveit_msgs/Constraints.h>
|
||||||
#include <moveit/task_constructor/properties.h>
|
#include <moveit/task_constructor/properties.h>
|
||||||
#include <Eigen/Geometry>
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
@ -77,16 +78,18 @@ public:
|
|||||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const planning_scene::PlanningSceneConstPtr to,
|
const planning_scene::PlanningSceneConstPtr to,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) = 0;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||||
|
|
||||||
/// plan trajectory from current robot state to Cartesian target
|
/// plan trajectory from current robot state to Cartesian target
|
||||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const moveit::core::LinkModel &link,
|
const moveit::core::LinkModel &link,
|
||||||
const Eigen::Affine3d& target,
|
const Eigen::Affine3d& target,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result) = 0;
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);
|
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);
|
||||||
|
|||||||
@ -41,12 +41,22 @@
|
|||||||
#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>
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class Connect : public Connecting {
|
class Connect : public Connecting {
|
||||||
public:
|
public:
|
||||||
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
|
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
|
||||||
|
|
||||||
|
void setTimeout(const ros::Duration& timeout){
|
||||||
|
setProperty("timeout", timeout.toSec());
|
||||||
|
}
|
||||||
|
|
||||||
|
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
||||||
|
setProperty("path_constraints", std::move(path_constraints));
|
||||||
|
}
|
||||||
|
|
||||||
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||||
|
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
@ -59,8 +59,9 @@ void CartesianPath::init(const core::RobotModelConstPtr &robot_model)
|
|||||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const planning_scene::PlanningSceneConstPtr to,
|
const planning_scene::PlanningSceneConstPtr to,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result)
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints)
|
||||||
{
|
{
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
const std::string& link_name = solvers::getEndEffectorLink(jmg);
|
const std::string& link_name = solvers::getEndEffectorLink(jmg);
|
||||||
@ -70,25 +71,31 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
}
|
}
|
||||||
|
|
||||||
// reach pose of forward kinematics
|
// reach pose of forward kinematics
|
||||||
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result);
|
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link),
|
||||||
|
jmg, timeout, result, path_constraints);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||||
const moveit::core::LinkModel &link,
|
const moveit::core::LinkModel &link,
|
||||||
const Eigen::Affine3d &target,
|
const Eigen::Affine3d &target,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr &result)
|
robot_trajectory::RobotTrajectoryPtr &result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints)
|
||||||
{
|
{
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
|
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
|
||||||
|
|
||||||
auto isValid = [&sandbox_scene](moveit::core::RobotState* state,
|
kinematic_constraints::KinematicConstraintSet kcs(sandbox_scene->getRobotModel());
|
||||||
|
kcs.add(path_constraints, sandbox_scene->getTransforms());
|
||||||
|
|
||||||
|
auto isValid = [&sandbox_scene, &kcs](moveit::core::RobotState* state,
|
||||||
const moveit::core::JointModelGroup* jmg,
|
const moveit::core::JointModelGroup* jmg,
|
||||||
const double* joint_positions) {
|
const double* joint_positions) {
|
||||||
state->setJointGroupPositions(jmg, joint_positions);
|
state->setJointGroupPositions(jmg, joint_positions);
|
||||||
state->update();
|
state->update();
|
||||||
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName());
|
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName())
|
||||||
|
&& kcs.decide(*state).satisfied;
|
||||||
};
|
};
|
||||||
|
|
||||||
std::vector<moveit::core::RobotStatePtr> trajectory;
|
std::vector<moveit::core::RobotStatePtr> trajectory;
|
||||||
|
|||||||
@ -85,7 +85,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
const planning_scene::PlanningSceneConstPtr to,
|
const planning_scene::PlanningSceneConstPtr to,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result)
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints)
|
||||||
{
|
{
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
moveit_msgs::MotionPlanRequest req;
|
moveit_msgs::MotionPlanRequest req;
|
||||||
@ -95,6 +96,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||||
to->getCurrentState(), jmg,
|
to->getCurrentState(), jmg,
|
||||||
props.get<double>("goal_joint_tolerance"));
|
props.get<double>("goal_joint_tolerance"));
|
||||||
|
req.path_constraints = path_constraints;
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner_->generatePlan(from, req, res))
|
if(!planner_->generatePlan(from, req, res))
|
||||||
@ -109,8 +111,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
const Eigen::Affine3d& target_eigen,
|
const Eigen::Affine3d& target_eigen,
|
||||||
const moveit::core::JointModelGroup *jmg,
|
const moveit::core::JointModelGroup *jmg,
|
||||||
double timeout,
|
double timeout,
|
||||||
robot_trajectory::RobotTrajectoryPtr& result)
|
robot_trajectory::RobotTrajectoryPtr& result,
|
||||||
|
const moveit_msgs::Constraints& path_constraints)
|
||||||
{
|
{
|
||||||
const auto& props = properties();
|
const auto& props = properties();
|
||||||
moveit_msgs::MotionPlanRequest req;
|
moveit_msgs::MotionPlanRequest req;
|
||||||
@ -125,6 +127,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
link.getName(), target,
|
link.getName(), target,
|
||||||
props.get<double>("goal_position_tolerance"),
|
props.get<double>("goal_position_tolerance"),
|
||||||
props.get<double>("goal_orientation_tolerance"));
|
props.get<double>("goal_orientation_tolerance"));
|
||||||
|
req.path_constraints = path_constraints;
|
||||||
|
|
||||||
::planning_interface::MotionPlanResponse res;
|
::planning_interface::MotionPlanResponse res;
|
||||||
if(!planner_->generatePlan(from, req, res))
|
if(!planner_->generatePlan(from, req, res))
|
||||||
|
|||||||
@ -48,6 +48,8 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
|
|||||||
auto& p = properties();
|
auto& p = properties();
|
||||||
p.declare<double>("timeout", 10.0, "planning timeout");
|
p.declare<double>("timeout", 10.0, "planning timeout");
|
||||||
p.declare<std::string>("group", "name of planning group");
|
p.declare<std::string>("group", "name of planning group");
|
||||||
|
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
|
||||||
|
"constraints to maintain during trajectory");
|
||||||
}
|
}
|
||||||
|
|
||||||
void Connect::init(const core::RobotModelConstPtr& robot_model)
|
void Connect::init(const core::RobotModelConstPtr& robot_model)
|
||||||
@ -63,7 +65,8 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group);
|
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group);
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory;
|
robot_trajectory::RobotTrajectoryPtr trajectory;
|
||||||
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory))
|
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory,
|
||||||
|
props.get<moveit_msgs::Constraints>("path_constraints")))
|
||||||
return false;
|
return false;
|
||||||
|
|
||||||
connect(from, to, trajectory);
|
connect(from, to, trajectory);
|
||||||
|
|||||||
@ -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