diff --git a/src/util/TrajectoryUtil.cpp b/src/util/TrajectoryUtil.cpp index 1e4dbbdc66ce7b21e51622369edb07b373eab536..00b520c0ce7e8289fee228936b45f6889039f6bb 100644 --- a/src/util/TrajectoryUtil.cpp +++ b/src/util/TrajectoryUtil.cpp @@ -34,15 +34,10 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm"); rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); trajectory_processing::IterativeParabolicTimeParameterization iptp{}; - - iptp.computeTimeStamps(rt); - if (iptp.computeTimeStamps(rt, maxVelocityFactor)) { ROS_INFO("Computed time stamps."); rt.getRobotTrajectoryMsg(trajectory_msg); plan.trajectory_ = trajectory_msg; - - ROS_ERROR_STREAM("After second time parameterization, the trajectory takes " << rt.getDuration() << "s"); } else { ROS_ERROR("Computation of cartesian path has failed."); return false; @@ -50,6 +45,7 @@ bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInte } else if (pathType == FLUID_PATH) { if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) { ROS_ERROR_STREAM("Computation of fluid path has failed. Path was motion to:\n" << targetPose); + return false; } } else { ROS_ERROR("Invalid type of path provided.");