ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Your title asks how to set the last state of a trajectory as the start state of a new plan, which you probably already figured out (no guarantee on the syntax):
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
move_group_interface.plan(my_plan)
robot_state::RobotState last_joint_state = my_plan.trajectory_.getLastWayPoint();
move_group_interface.
But in your question you ask how to set the start state of a robot motion. Your approach looks fine at first sight. I would check if setFromIK
is failing for start_state_lift
, and if the joint state you receive from that call is actually the same as the state of your robot after you executed the motion. You can use moveit_visual_tools to visualize the state.