Select Git revision
-
Johannes Mey authoredJohannes Mey authored
TrajectoryUtil.cpp 4.31 KiB
//
// 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);