BasicCartesianPlanner.cpp

#include <moveit/move_group_interface/move_group_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>

/**
 * simple demo of a robotic motions based on cartesian paths
 */
int main(int argc, char **argv) {
    // init the node
    ros::init(argc, argv, "basic_cartesian_planner");
    ros::NodeHandle node_handle;
    ros::AsyncSpinner spinner(1);
    spinner.start();

    // setup the planning environment
    static const std::string PLANNING_GROUP = "panda_arm";
    moveit::planning_interface::MoveGroupInterface group(PLANNING_GROUP);

    ros::Duration(3.0).sleep();

    group.setStartStateToCurrentState();

    // You can plan a Cartesian path directly by specifying a list of waypoints
    // for the end-effector to go through. Note that we are starting
    // from the new start state above.
    std::vector<geometry_msgs::Pose> waypoints;

    // initial pose of the robot
    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;

    // waypoints of the target trajectory
    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;

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

    target_pose_2.position.z = 0.6;
    target_pose_2.position.y = -0.6;
    target_pose_2.position.x = 0;
    waypoints.push_back(target_pose_2);

    target_pose_3.position.z = 0.6;
    target_pose_3.position.y = -0.032;
    target_pose_3.position.x = -0.6;
    waypoints.push_back(target_pose_3);

    group.setPlanningTime(10.0);

    // create a plan object and compute the cartesian motion
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit_msgs::RobotTrajectory trajectory_msg;
    group.computeCartesianPath(waypoints, 0.01, 0.0, trajectory_msg, true);
    plan.trajectory_ = trajectory_msg;

    // uncomment to modify maximum velocity and acceleration of the motion to be planned
    /*
    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), PLANNING_GROUP);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;

    double max_velocity_scaling_factor  = 0.1;
    double max_acceleration_scaling_factor = 0.1;

    iptp.computeTimeStamps(rt, max_velocity_scaling_factor, max_acceleration_scaling_factor);
    rt.getRobotTrajectoryMsg(trajectory_msg);
    */

    // execute the planned motion
    group.execute(plan);

    ros::shutdown();
    return 0;
}