// // Created by sebastian on 08.04.20. // #include "TrajectoryUtil.h" const std::string TrajectoryUtil::CARTESIAN_PATH = "cartesian_path"; const std::string TrajectoryUtil::FLUID_PATH = "fluid_path"; bool TrajectoryUtil::computePathToPose(moveit::planning_interface::MoveGroupInterface &group, moveit::planning_interface::MoveGroupInterface::Plan &plan, geometry_msgs::Pose targetPose, const std::string& pathType, double maxVelocityFactor, double maxAccelerationFactor, double motionSpeedFactor) { if (maxVelocityFactor <= 0 || maxVelocityFactor > 1.0) { ROS_ERROR_STREAM("Invalid maximum velocity scaling factor " << maxVelocityFactor << ". Must be in (0,1]."); return false; } if (maxAccelerationFactor <= 0 || maxAccelerationFactor > 1.0) { ROS_ERROR_STREAM("Invalid maximum acceleration scaling factor " << maxAccelerationFactor << ". Must be in (0,1]."); return false; } if (motionSpeedFactor <= 0 || motionSpeedFactor > 1.0) { ROS_ERROR_STREAM("Invalid motion speed factor " << motionSpeedFactor << ". Must be in (0,1]."); return false; } group.setStartStateToCurrentState(); group.setPlanningTime(10.0); group.setPoseTarget(targetPose); group.setMaxVelocityScalingFactor(maxVelocityFactor); group.setMaxAccelerationScalingFactor(maxAccelerationFactor); if (pathType == CARTESIAN_PATH) { std::vector<geometry_msgs::Pose> single_traj{targetPose}; moveit_msgs::RobotTrajectory trajectory_msg; double fraction = group.computeCartesianPath(single_traj, 0.01, 0.0, trajectory_msg, true); ROS_INFO("Planned cartesian plan (%.2f%% achieved)", fraction * 100.0); robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm"); rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); trajectory_processing::IterativeParabolicTimeParameterization iptp{}; ROS_ERROR_STREAM("Before time parameterization, the trajectory takes " << rt.getDuration() << "s"); if (iptp.computeTimeStamps(rt)) { ROS_ERROR_STREAM("After first time parameterization, the trajectory takes " << rt.getDuration() << "s"); } 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; } } else if (pathType == FLUID_PATH) { if (group.plan(plan) != moveit::planning_interface::MoveItErrorCode::SUCCESS) { ROS_ERROR("Computation of fluid path has failed."); } } else { ROS_ERROR("Invalid type of path provided."); return false; } manipulateVelocity(plan, motionSpeedFactor); } bool TrajectoryUtil::initWaypoints(std::vector<geometry_msgs::Pose> &waypoints, ros::NodeHandle &node_handle, geometry_msgs::Pose &initialPose) { std::vector<double> x_values; std::vector<double> y_values; std::vector<double> z_values; // check if no trajectory is configured if (!node_handle.getParam("trajectory_pos_x", x_values) || !node_handle.getParam("trajectory_pos_y", y_values) || !node_handle.getParam("trajectory_pos_z", z_values)) { return false; } else { for (int i = 0; i < x_values.size(); i++) { geometry_msgs::Pose pose = initialPose; pose.position.x = x_values.at(i); pose.position.y = y_values.at(i); pose.position.z = z_values.at(i); waypoints.push_back(pose); } node_handle.setParam("new_trajectory_available", false); return true; } } void TrajectoryUtil::manipulateVelocity(moveit::planning_interface::MoveGroupInterface::Plan &plan, double velocity) { ROS_INFO_STREAM("slowing down plan with " << plan.trajectory_.joint_trajectory.points.size() << "joints"); for (auto &point : plan.trajectory_.joint_trajectory.points) { point.time_from_start *= 1 / velocity; for (double &v : point.velocities) { v *= velocity; } for (double &acceleration : point.accelerations) { acceleration = sqrt(std::abs(acceleration)); } } }