mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Simplify code
We know that trajectory at least comprises the start state. Thus, we don't need the sanity checks.
This commit is contained in:
parent
71fab0fbce
commit
d6f68f9254
@ -102,15 +102,14 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
|
|||||||
is_valid);
|
is_valid);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (!trajectory.empty()) {
|
assert(!trajectory.empty()); // there should be at least the start state
|
||||||
result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg));
|
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
|
||||||
for (const auto& waypoint : trajectory)
|
for (const auto& waypoint : trajectory)
|
||||||
result->addSuffixWayPoint(waypoint, 0.0);
|
result->addSuffixWayPoint(waypoint, 0.0);
|
||||||
|
|
||||||
trajectory_processing::IterativeParabolicTimeParameterization timing;
|
trajectory_processing::IterativeParabolicTimeParameterization timing;
|
||||||
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
|
||||||
props.get<double>("max_acceleration_scaling_factor"));
|
props.get<double>("max_acceleration_scaling_factor"));
|
||||||
}
|
|
||||||
|
|
||||||
return achieved_fraction >= props.get<double>("min_fraction");
|
return achieved_fraction >= props.get<double>("min_fraction");
|
||||||
}
|
}
|
||||||
|
|||||||
@ -302,13 +302,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||||
|
|
||||||
double distance = 0.0;
|
double distance = 0.0;
|
||||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
if (use_rotation_distance) {
|
||||||
if (use_rotation_distance) {
|
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
distance = rotation.angle();
|
||||||
distance = rotation.angle();
|
} else
|
||||||
} else
|
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
|
||||||
}
|
|
||||||
|
|
||||||
// min_distance reached?
|
// min_distance reached?
|
||||||
if (min_distance > 0.0) {
|
if (min_distance > 0.0) {
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user