Attention: Answers.ros.org is deprecated as of August the 11th, 2023. Please visit robotics.stackexchange.com to ask a new question. This site will remain online in read-only mode during the transition and into the foreseeable future. Selected questions and answers have been migrated, and redirects have been put in place to direct users to the corresponding questions on Robotics Stack Exchange. Additional details are available here.

move_group plan for 2 or more waypoints then execute

asked 2020-07-01 07:19:57 -0500

xibeisiber

137 ●28 ●44 ●43

Hi all, I want to move arm to pose1 and pose2, the straightforward way is

move_group.setStartState(*group.getCurrentState());
move_group.setPoseTarget(pose1);
move_group.plan(plan1);
move_group.execute(plan1);
move_group.setStartState(*group.getCurrentState());
move_group.setPoseTarget(pose2);
move_group.plan(plan2);
move_group.execute(plan2);

The problem is that if plan1 succeeds, and plan2 fails, plan1 is still executed. Is there a way to first check plan1 and plan2? If both succeed, then execute the plans.

I saw the argument of setPoseTargets() can be a list of poses, but the plan will only choose one randomly from the list as the goal...

edit retag flag offensive close merge delete

add a comment

1 Answer

Sort by » oldest newest most voted

answered 2020-07-02 02:09:20 -0500

xibeisiber

137 ●28 ●44 ●43

The following works. (reference)

#include<moveit/robot_trajectory/robot_trajectory.h>
#include<moveit/robot_state/robot_state.h>   

 group.setStartState(*group.getCurrentState());
 group.setPoseTarget(target_pose);
 success = group.plan(plan); 

 moveit::core::RobotModelConstPtr robot_model=group.getRobotModel();
 robot_trajectory::RobotTrajectory trajectory(robot_model, planning_group);
 trajectory.setRobotTrajectoryMsg(*(group.getCurrentState()),plan.trajectory_);

 group.setStartState(*trajectory.getLastWayPointPtr());
 group.setPoseTarget(target_pose2);
 success2 = group.plan(plan2); 

 if(success and success2){
    success = group.execute(plan);
    if(success)
     success2 = group.execute(plan2);
 }

edit flag offensive delete link

add a comment

Question Tools

2 followers

Stats

Asked: 2020-07-01 07:19:57 -0500

Seen: 214 times

Last updated: Jul 02 '20

move_group plan for 2 or more waypoints then execute edit

1 Answer

Question Tools

Stats

Related questions

move_group plan for 2 or more waypoints then execute