Getting final robot state from a moveit plan

Question

I want to plan two consecutive motions on a move group. How can I do that without executing the first plan. Not that I cannot put the first goal as viapoint because I need to execute some other actions unrelated to the robot.

On the tutorials I also saw something like this:

   start_state.setFromIK(joint_model_group, start_pose2);
   move_group.setStartState(start_state);

But obviously this cannot guarantee continuous joint motion if there are mutiple IK solutions, I think.

My current code is this:

    moveit::planning_interface::MoveGroupInterface::Plan plan1;
    move_group.setPoseTarget(pose1);
    bool success = (move_group.plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (!success) {
        ROS_ERROR_STREAM("plan1 failed");
        return false;
    }

    moveit::planning_interface::MoveGroupInterface::Plan plan2;
    /*
    I want to get endState of plan1 here, something like:
    move_group.setStartState(plan1.getEndState());
    */
    move_group.setPoseTarget(pose2);
    bool success = (move_group.plan(plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (!success) {
        ROS_ERROR_STREAM("plan2 failed");
        return false;
    }

---

Originally posted by kky on ROS Answers with karma: 61 on 2019-09-06

Post score: 1

Answer 1

I found the solution after some while. It is not as clean as I would like it to be but here it is.

robot_state::RobotState start_state(*move_group.getCurrentState());

moveit::planning_interface::MoveGroupInterface::Plan plan2;
robot_state::RobotState state(start_state);
const std::vector<double> joints = plan1.trajectory_.joint_trajectory.points.back().positions;
state.setJointGroupPositions(move_group_name, joints);
move_group.setStartState(state);
move_group.setPoseTarget(pose2);

---

Originally posted by kky with karma: 61 on 2019-09-06

This answer was ACCEPTED on the original site

Post score: 4