Ask Your Question
0

Getting final robot state from a moveit plan

asked 2019-09-06 06:41:28 -0500

kky gravatar image

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;
    }
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2019-09-06 09:28:56 -0500

kky gravatar image

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);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-09-06 06:41:28 -0500

Seen: 13 times

Last updated: Sep 06