diff --git a/core/src/solvers/cartesian_path.cpp b/core/src/solvers/cartesian_path.cpp index a895fb66..e15306ab 100644 --- a/core/src/solvers/cartesian_path.cpp +++ b/core/src/solvers/cartesian_path.cpp @@ -102,15 +102,14 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons is_valid); #endif - if (!trajectory.empty()) { - result.reset(new robot_trajectory::RobotTrajectory(sandbox_scene->getRobotModel(), jmg)); - for (const auto& waypoint : trajectory) - result->addSuffixWayPoint(waypoint, 0.0); + assert(!trajectory.empty()); // there should be at least the start state + result = std::make_shared(sandbox_scene->getRobotModel(), jmg); + for (const auto& waypoint : trajectory) + result->addSuffixWayPoint(waypoint, 0.0); - trajectory_processing::IterativeParabolicTimeParameterization timing; - timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), - props.get("max_acceleration_scaling_factor")); - } + trajectory_processing::IterativeParabolicTimeParameterization timing; + timing.computeTimeStamps(*result, props.get("max_velocity_scaling_factor"), + props.get("max_acceleration_scaling_factor")); return achieved_fraction >= props.get("min_fraction"); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 7fb440c3..4271337d 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -302,13 +302,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link); double distance = 0.0; - if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { - if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); - distance = rotation.angle(); - } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); - } + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + distance = rotation.angle(); + } else + distance = (reached_pose.translation() - link_pose.translation()).norm(); // min_distance reached? if (min_distance > 0.0) {