I want to use moveit to control real robot so I changed 'fake excution' in 'demo.launch' to be false. In my disired out put ,there are there trajectories including pick the cup, lift the cup ,and rotate the cup.I can get way points for every trajectory in my action client terminal.However,it seems that after this change, the start state of robot in rviz will always be the original state.I tried to use setstartstate() to set the startstate as the final pose of last state,but the terminal which ran demo.launch showing warning of "Invalid Trajectory: start point deviates from current robot state more than 0.01 joint 'ljoint1': expected: 0, current: 1.84494".

I know I can pushback the waypoints of trajectory for the second motion and third motion to the first one and combined them as a hole 'plan',but I want to pause for a while after each motion.So this way will not work.

Now,I just want to find a way to make my robot excute 3 motions and pause a while between every 2 motion. Besides, my action client will still get waypoint for all the trajectory.

The following are my entire code:

void robot_motion(moveit::planning_interface::MoveGroupInterface& arm)
{

    geometry_msgs::Pose target_pose_pick;
    target_pose_pick.orientation.x = 0;
    target_pose_pick.orientation.y = -0.819;
    target_pose_pick.orientation.z = 0;
    target_pose_pick.orientation.w = 0.574;

    target_pose_pick.position.x = 0.1;
    target_pose_pick.position.y = -0.3;
    target_pose_pick.position.z = 0.9;

    // 设置机器臂当前的状态作为运动初始状态
    arm.setStartStateToCurrentState();
    arm.setPoseTarget(target_pose_pick);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan_pick;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(plan_pick);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan_pick);
    sleep(5);

    //end of pick

    //start of lift
    robot_state::RobotState start_state_lift(*arm.getCurrentState());
    const robot_state::JointModelGroup *joint_model_group =start_state_lift.getJointModelGroup(arm.getName());  
    start_state_lift.setFromIK(joint_model_group,target_pose_pick);
    //set the initial position of motion of rotating
    // 设置机器人终端的目标位置
    geometry_msgs::Pose target_pose_lift;
    target_pose_lift.orientation.x = 0;
    target_pose_lift.orientation.y = -0.819;
    target_pose_lift.orientation.z = 0;
    target_pose_lift.orientation.w = 0.574;
    //change the height of the end-effector
    target_pose_lift.position.x = 0.1;
    target_pose_lift.position.y = -0.3;
    target_pose_lift.position.z = 1.5;

    arm.setStartState(start_state_lift);
    arm.setPoseTarget(target_pose_lift);
    moveit::planning_interface::MoveGroupInterface::Plan plan_lift;
    success = arm.plan(plan_lift);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan_lift);
    sleep(5);
   //end of lift

  //start of rotate
    robot_state::RobotState start_state_rotate(*arm.getCurrentState()); 
    start_state_rotate.setFromIK(joint_model_group,target_pose_lift);

    geometry_msgs::Pose target_pose_rotate;
    //rotate around y axis by -150 degree
    target_pose_rotate.orientation.x = 0;
    target_pose_rotate.orientation.y = -0.966;
    target_pose_rotate.orientation.z = 0;
    target_pose_rotate.orientation.w = 0.259;

    target_pose_rotate.position.x = 0.1;
    target_pose_rotate.position.y = -0.3;
    target_pose_rotate.position.z = 1.5;

    arm.setStartState(start_state_rotate);
    arm.setPoseTarget(target_pose_rotate);

    // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动
    moveit::planning_interface::MoveGroupInterface::Plan plan_rotate;
    success = arm.plan(plan_rotate);

    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    //让机械臂按照规划的轨迹开始运动。
    if(success)
      arm.execute(plan_rotate);
    sleep(5);
}

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

    moveit::planning_interface::MoveGroupInterface arm("left_manipulator");

    //获取终端link的名称
    std::string end_effector_link = arm.getEndEffectorLink();

    //设置目标位置所使用的参考坐标系
    std::string reference_frame = "base_link";
    arm.setPoseReferenceFrame(reference_frame);

    //当运动规划失败后，允许重新规划
    arm.allowReplanning(true);

    //设置位置(单位：米)和姿态（单位：弧度）的允许误差
    arm.setGoalPositionTolerance(0.001);
    arm.setGoalOrientationTolerance(0.01);

    //设置允许的最大速度和加速度
    arm.setMaxAccelerationScalingFactor(0.2);
    arm.setMaxVelocityScalingFactor(0.2);

    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("left_original");
    arm.move();
    sleep(1);

    robot_motion(arm);


    // 控制机械臂先回到初始化位置
    arm.setNamedTarget("left_original");
    arm.move();
    sleep(5);

    ros::shutdown(); 

    return 0;
}