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

Planning for a dual arm robot in MoveIt

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

Henning Luo gravatar image

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

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

5 Answers

Sort by » oldest newest most voted
2

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

fvd gravatar image

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

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 more

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 gravatar image Henning Luo  ( 2021-01-27 05:31:17 -0500 )edit

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

fvd gravatar image fvd  ( 2021-01-27 07:04:42 -0500 )edit

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

Henning Luo gravatar image Henning Luo  ( 2021-01-27 14:10:48 -0500 )edit

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

fvd gravatar image fvd  ( 2021-01-27 21:20:20 -0500 )edit

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

Henning Luo gravatar image Henning Luo  ( 2021-01-28 03:39:15 -0500 )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 gravatar image Omar Islam  ( 2021-09-04 08:51:53 -0500 )edit

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

fvd gravatar image fvd  ( 2021-09-05 11:12:16 -0500 )edit
2

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

pablomalvido gravatar image

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 more
0

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

ravnicas gravatar image

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

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 more

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 gravatar image Henning Luo  ( 2021-01-25 09:04:43 -0500 )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 gravatar image ravnicas  ( 2021-01-25 09:42:54 -0500 )edit
0

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

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 more

Comments

isn't that code broken?

Pablo Iñigo Blasco gravatar image Pablo Iñigo Blasco  ( 2022-06-09 14:29:28 -0500 )edit
0

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

Henning Luo gravatar image

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 more

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 gravatar image fvd  ( 2021-01-27 19:23:33 -0500 )edit

Question Tools

5 followers

Stats

Asked: 2021-01-23 15:49:55 -0500

Seen: 2,251 times

Last updated: Mar 11 '22