TrajectoryUtil.cpp

//
// Created by sebastian on 08.04.20.
//

#include "TrajectoryUtil.h"

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) {
  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;
  }

  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.");
      ROS_ERROR_STREAM("Path was motion to:\n" << targetPose);
    }
  } else {
    ROS_ERROR("Invalid type of path provided.");
    return false;
  }
  return true;
}

void TrajectoryUtil::applyMotionSpeedFactor(moveit_msgs::RobotTrajectory &trajectory, double velocity) {
  for (auto &point : 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));
    }
  }
}