TimedCartesianPlanner.cpp

//
// Created by sebastian on 31.03.20.
//
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>

/**
 * allows time/velocity-constraint planning for cartesian paths
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "move_group_interface_tutorial");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // wait for robot init of robot_state_initializer
    std::cout << ">>>>>>>>>>>>>>>>> SLEEPING <<<<<<<<<<<<<<<< " << std::endl;
    ros::Duration(5.0).sleep();
    std::cout << ">>>>>>>>>>>>>>>>> WAKING UP <<<<<<<<<<<<<<<< " << std::endl;
    node_handle.setParam("tud_planner_ready", true);

    // Visualization Setup
    namespace rvt = rviz_visual_tools;
    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
    visual_tools.deleteAllMarkers();
    visual_tools.loadRemoteControl();

    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
    text_pose.translation().z() = 1.75;
    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);

    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
    visual_tools.trigger();

    moveit::planning_interface::MoveGroupInterface group("panda_arm");

    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
    moveit_msgs::DisplayTrajectory display_trajectory;

    moveit::planning_interface::MoveGroupInterface::Plan plan;
    group.setStartStateToCurrentState();

    std::vector<geometry_msgs::Pose> waypoints;

    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;

    target_pose_1.position.z = start_pose.position.z - 0.2;
    target_pose_1.position.y = start_pose.position.y + 0.2;
    target_pose_1.position.x = start_pose.position.x + 0.2;
    waypoints.push_back(target_pose_1);

    geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
    target_pose_2.position.z = start_pose.position.z + 0.2;
    target_pose_1.position.y = start_pose.position.y - 0.2;
    target_pose_2.position.x = start_pose.position.x - 0.2;
    waypoints.push_back(target_pose_2);

    moveit_msgs::RobotTrajectory trajectory_msg;
    group.setPlanningTime(10.0);

    double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);

    // The trajectory needs to be modified so it will include velocities as well.
    // First to create a RobotTrajectory object
    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");

    // Second get a RobotTrajectory from trajectory
    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);

    // Third create a IterativeParabolicTimeParameterization object
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    // Fourth compute computeTimeStamps
    bool success = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
    rt.getRobotTrajectoryMsg(trajectory_msg);

    //std::cout << ">>>>>>>>>>>>>>>>> data before: " << trajectory_msg << std::endl;

    // adjust velocities, accelerations and time_from_start
    for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
    {
        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
        {
            if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
                //trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) = 0;
            }
        }

        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
        {
            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
                    sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
        }
    }

    //std::cout << ">>>>>>>>>>>>>>>>> data after: " << trajectory_msg << std::endl;
    plan.trajectory_ = trajectory_msg;
    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);

    group.execute(plan);

    ros::shutdown();
    return 0;
}