Ask Your Question
0

move_group plan for 2 or more waypoints then execute

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

xibeisiber gravatar image

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

1 Answer

Sort by ยป oldest newest most voted
0

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

xibeisiber gravatar image

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 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: 2020-07-01 07:19:57 -0600

Seen: 49 times

Last updated: Jul 02 '20