mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix joint_interpolation
- return a trajectory in any case (even if there is no motion needed) - check feasability of goal pose
This commit is contained in:
parent
9ee653453d
commit
8debe68f99
@ -64,16 +64,16 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
|||||||
const moveit::core::RobotState& to_state = to->getCurrentState();
|
const moveit::core::RobotState& to_state = to->getCurrentState();
|
||||||
for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels())
|
for (const moveit::core::JointModel* jm : from_state.getRobotModel()->getActiveJointModels())
|
||||||
d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm));
|
d = std::max(d, jm->getDistanceFactor() * from_state.distance(to_state, jm));
|
||||||
if (d < 1e-6)
|
|
||||||
return true; // return empty result trajectory: no motion needed
|
|
||||||
|
|
||||||
result = std::make_shared<robot_trajectory::RobotTrajectory>(from->getRobotModel(), jmg);
|
result = std::make_shared<robot_trajectory::RobotTrajectory>(from->getRobotModel(), jmg);
|
||||||
|
|
||||||
// add first point
|
// add first point
|
||||||
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
|
||||||
|
if (from->isStateColliding(from_state, jmg->getName()))
|
||||||
|
return false;
|
||||||
|
|
||||||
moveit::core::RobotState waypoint(from_state);
|
moveit::core::RobotState waypoint(from_state);
|
||||||
double delta = props.get<double>("max_step") / d;
|
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
|
||||||
for (double t = delta; t < 1.0; t += delta) {
|
for (double t = delta; t < 1.0; t += delta) {
|
||||||
from_state.interpolate(to_state, t, waypoint);
|
from_state.interpolate(to_state, t, waypoint);
|
||||||
result->addSuffixWayPoint(waypoint, t);
|
result->addSuffixWayPoint(waypoint, t);
|
||||||
@ -83,7 +83,9 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
|
|||||||
}
|
}
|
||||||
|
|
||||||
// add goal point
|
// add goal point
|
||||||
result->addSuffixWayPoint(to->getCurrentState(), 1.0);
|
result->addSuffixWayPoint(to_state, 1.0);
|
||||||
|
if (from->isStateColliding(to_state, jmg->getName()))
|
||||||
|
return false;
|
||||||
|
|
||||||
// add timing, TODO: use a generic method to add timing via plugins
|
// add timing, TODO: use a generic method to add timing via plugins
|
||||||
trajectory_processing::IterativeParabolicTimeParameterization timing;
|
trajectory_processing::IterativeParabolicTimeParameterization timing;
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user