ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Getting final robot state from a moveit plan

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

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);

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;
    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:
    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

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

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

Question Tools



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

Seen: 566 times

Last updated: Sep 06 '19