8. MoveIt trajectory planning

This lesson takes the MoveIT simulation as an example. If you need to set the synchronization between the real machine and the simulation, please refer to the lesson [02, MoveIt Precautions and Controlling the Real Machine]. ! ! ! be careful! ! !

The effect demonstration is a virtual machine, and other masters are running (related to the performance of the master, depending on the actual situation).

8.1. Start

Start the MoveIT

roslaunch arm_moveit_demo x3plus_moveit_demo.launch sim:=true 

Start trajectory planning node

xxxxxxxxxx
rosrun arm_moveit_demo 06_multi_track_motion

Effect drawing

To view the track, you need to add the [MarkerArray] plugin and select the [/rviz_visual_tools] topic.

Given three reachable target points of the robotic arm, MoveIT will plan three feasible trajectories according to the target points, and then combine the three trajectories into a continuous trajectory.

8.2. Source code analysis

Set three reachable target points (any target point can be reached, it must be reachable)

xxxxxxxxxx
    vector<vector<double>> poses{
            {1.34,  -1.0,  -0.61, 0.2,   0},
            {0,     0,     0,     0,     0},
            {-1.16, -0.97, -0.81, -0.79, 3.14}
    };
    for (int i = 0; i < poses.size(); ++i) {
        multi_trajectory(yahboomcar, poses.at(i), trajectory);
    }

Plan each trajectory

xxxxxxxxxx
void multi_trajectory(
        moveit::planning_interface::MoveGroupInterface &yahboomcar,
        const vector<double> &pose, moveit_msgs::RobotTrajectory &trajectory) {
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    const robot_state::JointModelGroup *joint_model_group;
    //Get the robot's starting position
    moveit::core::RobotStatePtr start_state(yahboomcar.getCurrentState());
    joint_model_group = start_state->getJointModelGroup(yahboomcar.getName());
    yahboomcar.setJointValueTarget(pose);
    yahboomcar.plan(plan);
    start_state->setJointGroupPositions(joint_model_group, pose);
    yahboomcar.setStartState(*start_state);
    trajectory.joint_trajectory.joint_names = plan.trajectory_.joint_trajectory.joint_names;
    for (size_t j = 0; j < plan.trajectory_.joint_trajectory.points.size(); j++) {
        trajectory.joint_trajectory.points.push_back(plan.trajectory_.joint_trajectory.points[j]);
    }
}

Trajectory merge

xxxxxxxxxx
    moveit::planning_interface::MoveGroupInterface::Plan joinedPlan;
    robot_trajectory::RobotTrajectory rt(yahboomcar.getCurrentState()->getRobotModel(), "arm_group");
    rt.setRobotTrajectoryMsg(*yahboomcar.getCurrentState(), trajectory);
    trajectory_processing::IterativeParabolicTimeParameterization iptp;
    iptp.computeTimeStamps(rt, 1, 1);
    rt.getRobotTrajectoryMsg(trajectory);
    joinedPlan.trajectory_ = trajectory;

Track display

xxxxxxxxxx
    moveit_visual_tools::MoveItVisualTools tool(yahboomcar.getPlanningFrame());
    tool.deleteAllMarkers();
    /* 
     ...
    */
    //Show trace
    tool.publishTrajectoryLine(joinedPlan.trajectory_, yahboomcar.getCurrentState()->getJointModelGroup("arm_group"));
    tool.trigger();

Perform trajectory planning

xxxxxxxxxx
    if (!yahboomcar.execute(joinedPlan)) {
        ROS_ERROR("Failed to execute plan");
        return false;
    }