Planning for a dual arm robot in MoveIt

asked 2021-01-23 15:49:55 -0600

Henning Luo
58 ●3 ●6 ●10

updated 2021-01-28 04:15:45 -0600

Hello,

I have been trying to plan for a dual arm robot ABB YuMi in MoveIt. The package now has a both_arm group and I can plan for this group by 'setPoseTarget()' for two end effectors and 'plan()' for the group. It works well. However, when I try to use 'computeCartesianPath', I realise this function does not allow specifying end effectors. Is there a way to compute trajectories with predefined waypoints for a two arm group?

One way that I could think of was to compute trajectories for separate arm groups and combine the trajectories before execution. However, in this way, collision between two arms would not be taken into consideration, which could be a potential risk.

Any idea is appreciated! Thanks in advance for your help.

Edit: Here is how I combined the trajectories.

edit retag flag offensive close merge delete

add a comment

2

answered 2021-01-26 22:11:14 -0600

fvd
2180 ●15 ●98 ●56

updated 2021-01-27 07:03:10 -0600

I've defined my SRDF like this:

<group name="a_bot">
    <joint name="a_bot_shoulder_pan_joint" />
    <joint name="a_bot_shoulder_lift_joint" />
    <joint name="a_bot_elbow_joint" />
    <joint name="a_bot_wrist_1_joint" />
    <joint name="a_bot_wrist_2_joint" />
    <joint name="a_bot_wrist_3_joint" />
    <chain base_link="a_bot_base_link" tip_link="a_bot_gripper_tip_link" />
</group>
<group name="b_bot">
    <joint name="b_bot_shoulder_pan_joint" />
    <joint name="b_bot_shoulder_lift_joint" />
    <joint name="b_bot_elbow_joint" />
    <joint name="b_bot_wrist_1_joint" />
    <joint name="b_bot_wrist_2_joint" />
    <joint name="b_bot_wrist_3_joint" />
    <chain base_link="b_bot_base_link" tip_link="b_bot_robotiq_85_tip_link" />
</group>
<group name="both_bots">
    <group name="a_bot" />
    <group name="b_bot" />
</group>

This preserves the end-effector information, so you can set multiple goals like this:

group.setPoseTarget(home1, ee_link_a);  
group.setPoseTarget(targetpose, ee_link_b);
success_plan = group.plan(myplan);

But this is for "free" planning, and you asked about cartesian planning with waypoints. I don't think that the current implementation supports this, and what you described (generating a cartesian path for each arm separately, then combining the trajectories) is the most appropriate workaround at the moment.

I don't think that there is a convenience function to check for self-collisions in a given trajectory, but you can make one yourself. Step through each waypoint of your trajectory, set the RobotState of a PlanningScene object to the state at that waypoint using setCurrentState, then check collisions with isStateValid or isStateColliding.

This does not guarantee safety since the states between waypoints are not checked, but with enough padding and a small step size it should be fine.

edit flag offensive delete link

Comments

Thanks fvd! That is exactly what I had been using! I was able to plan the trajectory by setting setPoseTarget, it worked perfectly. My problem is that I want to specify waypoints for the end effectors, this is usually done by computeCartesianPath. However, this function do not allow specifying end effectors. What I have been doing now is to use computeCartesianPath for each arm groups, merge the trajectories together and execute it with both_arms group. However, by executing them simultaneously, there is the potential risk that two trajectories might collide with each other.

Henning Luo ( 2021-01-27 05:31:17 -0600 )edit

I see, I skipped over that part of your original question. I edited my answer.

fvd ( 2021-01-27 07:04:42 -0600 )edit

Thank you so much for your answer! I will try to do collision detection as you mentioned!

Henning Luo ( 2021-01-27 14:10:48 -0600 )edit

It could be helpful if you shared your code for this. See also this discussion about a new multi-arm MoveIt tutorial.

fvd ( 2021-01-27 21:20:20 -0600 )edit

Yes of course, I will share it in my OP.

Henning Luo ( 2021-01-28 03:39:15 -0600 )edit

Can someone please elaborate on how to join trajectories because I joined the joint traj points as vectors but it doesn't seem to execute.

Omar Islam ( 2021-09-04 08:51:53 -0600 )edit

Please post a separate question about your problem, making sure to include the details.

fvd ( 2021-09-05 11:12:16 -0600 )edit

add a comment

2

answered 2023-03-23 08:53:49 -0600

pablomalvido
41 ●2 ●3

We have developed a solution for this issue, you can check it here: https://github.com/ros-planning/movei.... This function allows the definition of dual-arm trajectories according to 5 synchronization policies: both arms start and finish moving at the same time, both arms move independently (each one at its own target speed), both arms reach a set of waypoints at the same time (the transitions between waypoints are smooth and the arms don’t stop between waypoints), constant distance master-slave (i.e., just the trajectory of the master arm is specified, and the slave arm moves to keep always a constant relative pose with respect to the master arm), identical master-slave (i.e., just the trajectory of the master arm is specified, and the slave arm performs the same cartesian displacements and rotations as the master arm). The function checks possible collisions and retrieves a safe plan. With this function, the user has full control over the path and speed of the arms.

The performance of the five synchronization policies on a SDA10F dual-arm Yaskawa robot can be seen in this video (from minute 3:55): https://youtu.be/DJXKWWmBquM?t=235.

edit flag offensive delete link

add a comment

0

answered 2022-03-11 09:08:57 -0600

bi3ri

31 ●3 ●3 https://github.com/bi3ri

The Solution of Felix is planning directly for pose targets, which did not work for me using noetic.

You first need to find a fitting joint configuration for each robot using IK and then set joint targets.

bool rocket_found_ik = kinematic_state->setFromIK(rocket_joint_model_group, rocket_pose, timeout);
bool groot_found_ik = kinematic_state->setFromIK(groot_joint_model_group, groot_pose, timeout);

kinematic_state->copyJointGroupPositions(rocket_joint_model_group, rocket_joint_values);
kinematic_state->copyJointGroupPositions(groot_joint_model_group, 

// planning group with rocket and groot
rng_group_interface.setJointValueTarget(rocket_joint_names, 
rng_group_interface.setJointValueTarget(groot_joint_names, 

bool success = (rng_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

I create a demo project you can find here.

dual arm

edit flag offensive delete link

Comments

isn't that code broken?

Pablo Iñigo Blasco ( 2022-06-09 14:29:28 -0600 )edit

add a comment

0

answered 2021-01-25 03:50:56 -0600

ravnicas
11 ●8 ●12 ●12

updated 2021-01-25 03:51:53 -0600

The way i solved it for MoveIt2 and also MoveIt1 at the moment is defining several planning groups via the srdf. One for each robot, and one for both. The latter including the joints only, so it doesnt know about the individual EEFs. Then setting the eef of the robots individually, which i guess you would be doing anyway, getting the IK of the resulting eef position and passing the joints of the result of both individual groups to the compound group. The compound group should now be able to compute a collission free path for both robots simultaneosly (if it exists).

edit flag offensive delete link

Comments

Hi thanks for your reply. I already have have a both_arm group and groups for both arms. The both_arm group is already capable of computing collision free trajectories by setting targets for each eef separately. The problem that I am facing now is that I would like to declare some waypoints for each eef. Could you kindly specify clearly 'passing the joints of the result of both individual groups to the compound group'? My understanding is that if the trajectories are planned for each individual group separately and then combined together, there is a posibility that the arms will collide during execution. Thanks.

Henning Luo ( 2021-01-25 09:04:43 -0600 )edit

You can set an EEF of a planning group via .setJointValueTarget(geometry_msgs::msg::Pose pose_of_eef) and get the resulting joints via getJointValueTarget. I do this for my first and second robot, catenate both vectors and pass them via setJointValueTarget to the "both_robots" planning group.

ravnicas ( 2021-01-25 09:42:54 -0600 )edit

add a comment

0

answered 2021-01-27 16:52:22 -0600

Henning Luo
58 ●3 ●6 ●10

For people who have the same problem and hoping that moveit2 will have computeCartesianPath support dual arm planning, I have checked it definition in moveit2/moveit_core/robot_state/src/cartesian_interpolator.cpp, it is not different from moveit1.

edit flag offensive delete link

Comments

For the record, there was a Google Summer of Code project for improving Cartesian planning and pull requests for multi-EEF cartesian interpolation would be welcome.

fvd ( 2021-01-27 19:23:33 -0600 )edit

add a comment

Planning for a dual arm robot in MoveIt

5 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

Planning for a dual arm robot in MoveIt edit

5 Answers

Comments

Comments

Comments

Comments

Question Tools

Stats

Related questions

Planning for a dual arm robot in MoveIt