mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
actually implement path constraints for CartesianPath planner
... validating constraints
This commit is contained in:
parent
d7d80c3499
commit
2ca68e6645
@ -61,7 +61,7 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
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_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);
|
||||||
@ -71,7 +71,8 @@ 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,
|
||||||
@ -80,17 +81,21 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr from,
|
|||||||
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_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;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user