mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add path constraints to planner API
and use it in the Connect class. The cartesian planner ignores the constraints for now.
This commit is contained in:
parent
b15c5adbd7
commit
8af44cefc3
@ -55,15 +55,17 @@ public:
|
||||
bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const planning_scene::PlanningSceneConstPtr to,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
||||
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 moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
||||
double timeout,
|
||||
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 core::JointModelGroup *jmg,
|
||||
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,
|
||||
const moveit::core::LinkModel &link,
|
||||
const Eigen::Affine3d& target,
|
||||
const core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) override;
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints= moveit_msgs::Constraints()) override;
|
||||
|
||||
protected:
|
||||
planning_pipeline::PlanningPipelinePtr planner_;
|
||||
|
||||
@ -39,6 +39,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <moveit/macros/class_forward.h>
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
#include <moveit/task_constructor/properties.h>
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
@ -77,16 +78,18 @@ public:
|
||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const planning_scene::PlanningSceneConstPtr to,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) = 0;
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||
|
||||
/// plan trajectory from current robot state to Cartesian target
|
||||
virtual bool plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const moveit::core::LinkModel &link,
|
||||
const Eigen::Affine3d& target,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result) = 0;
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
|
||||
};
|
||||
|
||||
std::string getEndEffectorLink(const moveit::core::JointModelGroup *jmg);
|
||||
|
||||
@ -41,12 +41,18 @@
|
||||
#include <moveit/task_constructor/stage.h>
|
||||
#include <moveit/task_constructor/solvers/planner_interface.h>
|
||||
|
||||
#include <moveit_msgs/Constraints.h>
|
||||
|
||||
namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class Connect : public Connecting {
|
||||
public:
|
||||
Connect(std::string name, const solvers::PlannerInterfacePtr &planner);
|
||||
|
||||
void setPathConstraints(moveit_msgs::Constraints path_constraints){
|
||||
setProperty("path_constraints", std::move(path_constraints));
|
||||
}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||
|
||||
|
||||
@ -59,8 +59,9 @@ void CartesianPath::init(const core::RobotModelConstPtr &robot_model)
|
||||
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const planning_scene::PlanningSceneConstPtr to,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result)
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& /*path_constraints*/)
|
||||
{
|
||||
const moveit::core::LinkModel* link;
|
||||
const std::string& link_name = solvers::getEndEffectorLink(jmg);
|
||||
@ -77,8 +78,9 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const moveit::core::LinkModel &link,
|
||||
const Eigen::Affine3d &target,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr &result)
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr &result,
|
||||
const moveit_msgs::Constraints& /*path_constraints*/)
|
||||
{
|
||||
const auto& props = properties();
|
||||
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
|
||||
|
||||
@ -85,7 +85,8 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
const planning_scene::PlanningSceneConstPtr to,
|
||||
const moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result)
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints)
|
||||
{
|
||||
const auto& props = properties();
|
||||
moveit_msgs::MotionPlanRequest req;
|
||||
@ -95,6 +96,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
|
||||
to->getCurrentState(), jmg,
|
||||
props.get<double>("goal_joint_tolerance"));
|
||||
req.path_constraints = path_constraints;
|
||||
|
||||
::planning_interface::MotionPlanResponse 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 moveit::core::JointModelGroup *jmg,
|
||||
double timeout,
|
||||
robot_trajectory::RobotTrajectoryPtr& result)
|
||||
|
||||
robot_trajectory::RobotTrajectoryPtr& result,
|
||||
const moveit_msgs::Constraints& path_constraints)
|
||||
{
|
||||
const auto& props = properties();
|
||||
moveit_msgs::MotionPlanRequest req;
|
||||
@ -125,6 +127,7 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr from,
|
||||
link.getName(), target,
|
||||
props.get<double>("goal_position_tolerance"),
|
||||
props.get<double>("goal_orientation_tolerance"));
|
||||
req.path_constraints = path_constraints;
|
||||
|
||||
::planning_interface::MotionPlanResponse res;
|
||||
if(!planner_->generatePlan(from, req, res))
|
||||
|
||||
@ -48,6 +48,8 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
|
||||
auto& p = properties();
|
||||
p.declare<double>("timeout", 10.0, "planning timeout");
|
||||
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)
|
||||
@ -63,7 +65,8 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
||||
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group);
|
||||
|
||||
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;
|
||||
|
||||
connect(from, to, trajectory);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user