TrajectoryTool.cpp

//
// Created by sebastian on 06.07.21.
//

#include "TrajectoryTool.h"

#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/robot_state/conversions.h>
#include <moveit/robot_model_loader/robot_model_loader.h>

moveit::planning_interface::MoveGroupInterface::Plan
TrajectoryTool::invert_plan(moveit::planning_interface::MoveGroupInterface::Plan &original_plan,
            moveit::planning_interface::MoveGroupInterface &move_group, std::string group){

    robot_model::RobotModelPtr _robot_model;
    robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
    _robot_model = robot_model_loader.getModel();

    moveit::planning_interface::MoveGroupInterface::Plan inverted_plan;

    inverted_plan.trajectory_.joint_trajectory.header.stamp = ros::Time::now();
    inverted_plan.trajectory_.joint_trajectory.header.frame_id = original_plan.trajectory_.joint_trajectory.header.frame_id;
    inverted_plan.trajectory_.joint_trajectory.joint_names = original_plan.trajectory_.joint_trajectory.joint_names;

    int j = original_plan.trajectory_.joint_trajectory.points.size() - 1;

    trajectory_processing::IterativeParabolicTimeParameterization time_param;

    robot_trajectory::RobotTrajectory inverted_trajectory(_robot_model, group);

    robot_state::RobotState robot_state(_robot_model);

    // new start point (current state)
    moveit::core::RobotStatePtr current_robot_state = move_group.getCurrentState(1.0);
    inverted_trajectory.insertWayPoint(0, current_robot_state, 0.1);

    for(size_t i = 0; i < original_plan.trajectory_.joint_trajectory.points.size() && j >= 0; i++){
        moveit::core::jointTrajPointToRobotState(original_plan.trajectory_.joint_trajectory, j, robot_state);
        inverted_trajectory.insertWayPoint(i+1, robot_state, 0.1);
        j--;
    }

    if(!time_param.computeTimeStamps(inverted_trajectory)){
        ROS_ERROR("Time parametrization failed.");
    }

    inverted_trajectory.getRobotTrajectoryMsg(inverted_plan.trajectory_);
    return inverted_plan;
}