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/DisplayTrajectory.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>

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

const double default_velocity = 1.0;

bool isInitialized = false;

void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::MoveGroupInterface* group)
{
    geometry_msgs::Pose target_pose_1 = group->getCurrentPose().pose;
    geometry_msgs::Pose target_pose_2 = group->getCurrentPose().pose;
    geometry_msgs::Pose target_pose_3 = group->getCurrentPose().pose;
    geometry_msgs::Pose target_pose_4 = group->getCurrentPose().pose;

    target_pose_1.position.z = 0.6;//0.583;
    target_pose_1.position.y = -0.6;//63;
    target_pose_1.position.x = 0;//-0.007;
    raw_trajectory.push_back(target_pose_1);

    target_pose_2.position.z = 0.6;//0.691;
    target_pose_2.position.y = -0.032;
    target_pose_2.position.x = -0.607;
    raw_trajectory.push_back(target_pose_2);

    target_pose_3.position.z = 0.6;
    target_pose_3.position.y = 0.6;//0.509;
    target_pose_3.position.x = 0;//0.039;
    raw_trajectory.push_back(target_pose_3);

    target_pose_4.position.z = group->getCurrentPose().pose.position.z;
    target_pose_4.position.y = group->getCurrentPose().pose.position.y;
    target_pose_4.position.x = group->getCurrentPose().pose.position.x;
    raw_trajectory.push_back(target_pose_4);
}

void moveRobotToInitialState(ros::NodeHandle node_handle)
{
    ROS_INFO("Moving to initial pose of trajectory.");
    moveit::planning_interface::MoveGroupInterface group("panda_arm");

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

    std::vector<geometry_msgs::Pose> trajectory_to_init_pose;
    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;

    target_pose_1.position.z = group.getCurrentPose().pose.position.z - 0.4;
    target_pose_1.position.y = group.getCurrentPose().pose.position.y;
    target_pose_1.position.x = group.getCurrentPose().pose.position.x + 0.5;
    trajectory_to_init_pose.push_back(target_pose_1);
    group.setPlanningTime(10.0);

    moveit_msgs::RobotTrajectory trajectory;
    double fraction = group.computeCartesianPath(trajectory_to_init_pose, 0.01, 0.0, trajectory);

    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    cartesian_plan.trajectory_ = trajectory;
    group.execute(cartesian_plan);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "timed_cartesian_planner");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // wait for robot init of robot_state_initializer
    ros::Duration(5.0).sleep();
    ROS_INFO(">>>>>>>>>>>>>>>>> START UP FINISHED <<<<<<<<<<<<<<<< ");
    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);
    visual_tools.trigger();

    // Initialize start state of robot and target trajectory.
    moveit::planning_interface::MoveGroupInterface group("panda_arm");
    moveRobotToInitialState(node_handle);
    initRawTrajectory(node_handle, &group);

    for(int i = 0; i < raw_trajectory.size(); i++)
    {
        double velocity = 0.0;

        if(!node_handle.getParam("robot_speed_factor", velocity))
        {
            velocity = default_velocity;
        }

        std::cout << ">>>>>>>>>>>>>>>>> starting planing step " << i << " (speed: " << velocity << ")"  << std::endl;

        std::vector<geometry_msgs::Pose> single_traj;
        single_traj.push_back(raw_trajectory.at(i));

        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::planning_interface::MoveGroupInterface::Plan plan;
        group.setStartStateToCurrentState();

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

        double fraction = group.computeCartesianPath(single_traj,0.01,0.0,trajectory_msg, false);
        robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
        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);
        plan.trajectory_ = trajectory_msg;
        ROS_INFO("Visualizing cartesian plan (%.2f%% acheived)",fraction * 100.0);

        //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++)
        {
            double new_tfs = 1 / velocity;
            trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(new_tfs);
            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) *= velocity;
                }
            }

            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;

        group.execute(plan);

        if(i == (raw_trajectory.size() -1))
        {
            i = -1;
        }
    }

    ros::shutdown();
    return 0;
}