Moving multiple groups simultaneously

Sudarshan Srinivasan

unread,

Jan 15, 2014, 2:31:36 PM1/15/14

to moveit...@googlegroups.com

Hi,

We have been using Moveit! with a Meka robot which has two arms amongst other groups of joints. During my setup assistant, we set up each arm to be its own group: left_arm group and right_arm group. We wrote an action server for the move_group node to talk to. We also have a client which uses the MoveGroupCommander to send goal poses to Moveit! which is then served by the action server and executed on the robot. Things are working pretty well as long as we only want to move one group. We threaded our client: one thread for left_arm and one thread for right_arm and issued goal poses for each and called group.go() for each. We had one move_group node running and different number of action servers (one action server for each group of joints with a total of 7). However, both arms will move only sequentially even if the client sends the command in different threads. The sequence seems to be random based on which group gets queued first at move_group. This seems to indicate that the move_group node can only move one group at a time. Is this true? If it is, how would we be go about moving multiple groups of joints at the same time? Can this be done? Please let me know if you need more information.

Thanks.

Sudarshan Srinivasan

unread,

Jan 15, 2014, 2:42:43 PM1/15/14

to moveit...@googlegroups.com

Here is an example output:

[ INFO] [1389731883.680229577]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.

[ INFO] [1389731883.680387708]: Planning attempt 1 of at most 1

[ INFO] [1389731883.703451544]: Attempting to use default projection.

[ INFO] [1389731883.703793084]: Attempting to use default projection.

[ INFO] [1389731883.706768593]: Starting with 1 states

[ INFO] [1389731883.709395865]: Starting with 1 states

[ INFO] [1389731883.712720196]: Attempting to use default projection.

[ INFO] [1389731883.713038721]: Starting with 1 states

[ INFO] [1389731884.204941472]: Created 122 (60 start + 62 goal) states in 94 cells (48 start (48 on boundary) + 46 goal (46 on boundary))

[ INFO] [1389731884.337196281]: Created 505 (143 start + 362 goal) states in 354 cells (90 start (73 on boundary) + 264 goal (253 on boundary))

[ INFO] [1389731884.478011758]: Created 349 (175 start + 174 goal) states in 241 cells (121 start (113 on boundary) + 120 goal (119 on boundary))

[ INFO] [1389731884.580840701]: Solution found in 0.880095 seconds

[ INFO] [1389731884.609606373]: Path simplification took 0.028650 seconds

[ INFO] [1389731892.065572206]: Stopping execution due to preempt request

[ INFO] [1389731892.065653304]: MoveitSimpleControllerManager: Cancelling execution for meka_left_arm_controller

[ INFO] [1389731892.065735991]: Stopped trajectory execution.

.

[ INFO] [1389731900.350038704]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.

[ INFO] [1389731900.350163392]: Planning attempt 1 of at most 1

[ INFO] [1389731900.369970132]: Starting with 1 states

[ INFO] [1389731900.594655101]: Created 151 (74 start + 77 goal) states in 119 cells (62 start (61 on boundary) + 57 goal (52 on boundary))

[ INFO] [1389731900.594791085]: Solution found in 0.227647 seconds

[ INFO] [1389731900.626299997]: Path simplification took 0.031297 seconds

It seems as if the first request gets preempted even though its for a different group.

Philippe Capdepuy

unread,

Jan 15, 2014, 2:45:53 PM1/15/14

to Sudarshan Srinivasan, moveit...@googlegroups.com

Hi Sudarshan,

I just ran into the same problem yesterday, a workaround is to ask moveit for the trajectory and then transmit it directly to the action server (with some message repacking). But I'd like to hear about a more standard approach.

2014/1/15 Sudarshan Srinivasan <sudar...@gmail.com>

--

Regards,

Dr. Philippe Capdepuy

Research Engineer

Génération Robots / HumaRobotics

Tel : +33 5 56 39 37 05

www.humarobotics.com

www.generationrobots.com

Découvrez Baxter, votre nouveau compagnon

pour la recherche et l’éducation

Baxter Research Robot

image001.png

Sachin Chitta

unread,

Jan 15, 2014, 4:59:36 PM1/15/14

to Philippe Capdepuy, Sudarshan Srinivasan, moveit-users

It feels like you both are trying to move two arms in a coordinated fashion. When you do that, you have to plan for both arms simultaneously, otherwise they cannot take into account the motion of the other. I would do this by defining a "dual_arms" group that contains the joints for both group. When you do it this way, MoveIt! will generate one trajectory for both arms but the trajectory execution manager should be able to break the trajectory apart into the right set of trajectories for the two parts. Your controllers should be looking at the timestamps on the individual trajectories and coordinating them together (since they should both have the same start time stamp). Let us know if this works.

Sachin

image001.png

Sam Pfeiffer

unread,

Jan 16, 2014, 10:43:31 AM1/16/14

to moveit...@googlegroups.com, Philippe Capdepuy, Sudarshan Srinivasan

I can confirm that I have a group that contains 2 arms (7 joints), 1 torso (2 joints), and one head (2 joints) and I can send goals that move all of it together (in joint space).

From the Rviz plugin I can send, for example, a goal for both of the arms (i have 2 interactive markers, one for each hand). I attach a series of images showing it.

What I can't tell is if it's possible to send a pose-based goal for a group that contains two final elements (grippers, hands...). At least I couldnt figure out how.

Also the goal sent my the Rviz plugin looks like:

$ rostopic echo /move_group/goal

WARNING: no messages received and simulated time is active.

Is /clock being published?

header:

seq: 0

stamp:

secs: 110

nsecs: 577000000

frame_id: ''

goal_id:

stamp:

secs: 110

nsecs: 577000000

id: /rviz_slovakia_26230_461042831-1-110.577000000

goal:

request:

workspace_parameters:

header:

seq: 0

stamp:

secs: 110

nsecs: 554000000

frame_id: /odom

min_corner:

x: -1.0

y: -1.0

z: -1.0

max_corner:

x: 1.0

y: 1.0

z: 1.0

start_state:

joint_state:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: /odom

name: ['caster_left_1_joint', 'caster_left_2_joint', 'caster_right_1_joint', 'caster_right_2_joint', 'torso_1_joint', 'torso_2_joint', 'arm_left_1_joint', 'arm_left_2_joint', 'arm_left_3_joint', 'arm_left_4_joint', 'arm_left_5_joint', 'arm_left_6_joint', 'arm_left_7_joint', 'hand_left_index_1_joint', 'hand_left_index_2_joint', 'hand_left_index_3_joint', 'hand_left_index_joint', 'hand_left_middle_1_joint', 'hand_left_middle_2_joint', 'hand_left_middle_3_joint', 'hand_left_middle_joint', 'hand_left_thumb_joint', 'arm_right_1_joint', 'arm_right_2_joint', 'arm_right_3_joint', 'arm_right_4_joint', 'arm_right_5_joint', 'arm_right_6_joint', 'arm_right_7_joint', 'hand_right_index_1_joint', 'hand_right_index_2_joint', 'hand_right_index_3_joint', 'hand_right_index_joint', 'hand_right_middle_1_joint', 'hand_right_middle_2_joint', 'hand_right_middle_3_joint', 'hand_right_middle_joint', 'hand_right_thumb_joint', 'head_1_joint', 'head_2_joint', 'wheel_left_joint', 'wheel_right_joint']

position: [-0.010664067134431576, -0.04220467071079792, -0.010622474342623534, -0.038504462367810355, -1.1879243595913636e-05, -0.0013873047554513462, -0.00031669175033677277, -0.0013779580168780114, -3.9812721920995386e-05, 0.0005234226472738968, 0.00028375395530844116, -0.002577825402061862, -0.00011292892022662926, 0.26350967930444114, 0.20542991414103629, 0.20543000231878672, 0.6637730907990882, 0.26351080218527656, 0.20543113037078253, 0.20543159944998202, 0.6637770114774009, 8.245539575568728e-06, -0.0003162206962654679, -0.0013793843595104605, -4.023479261228147e-05, 0.0005224510543735406, 0.0002831213220169104, -0.002584746630304835, -0.00011556622528097193, 0.263506013818688, 0.20542691794811851, 0.20542696261996785, 0.6637637710930324, 0.26350709876092004, 0.20542818779972816, 0.2054286623056285, 0.6637676867332667, 5.022306802437981e-06, 1.1587570476478959e-06, -0.0007330586266762751, -0.028721973683675017, -0.02433668214213114]

velocity: []

effort: []

multi_dof_joint_state:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: /odom

joint_names: ['virtual_joint']

transforms:

-

translation:

x: -0.00248168659548

y: -9.42602329273e-07

z: 0.0

rotation:

x: 0.0

y: 0.0

z: 0.000379835774621

w: 0.999999927862

twist: []

wrench: []

attached_collision_objects: []

is_diff: False

goal_constraints:

-

joint_constraints:

-

joint_name: torso_1_joint

position: -1.18268458733e-05

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: torso_2_joint

position: -0.00172372218096

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_1_joint

position: 1.52928612458

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_2_joint

position: 0.684831573103

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_3_joint

position: 0.0560176252795

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_4_joint

position: 0.389208280467

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_5_joint

position: -0.21895228878

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_6_joint

position: -0.994567799776

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_left_7_joint

position: 0.15147264697

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_1_joint

position: 2.02175397239

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_2_joint

position: 1.10299932308

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_3_joint

position: -1.64629271152

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_4_joint

position: 1.06558621901

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_5_joint

position: 0.175019530413

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_6_joint

position: -0.786536277448

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

-

joint_name: arm_right_7_joint

position: 1.01045103571

tolerance_above: 0.0001

tolerance_below: 0.0001

weight: 1.0

position_constraints: []

orientation_constraints: []

visibility_constraints: []

path_constraints:

joint_constraints: []

position_constraints: []

orientation_constraints: []

visibility_constraints: []

trajectory_constraints:

constraints: []

planner_id: ''

group_name: both_arms

num_planning_attempts: 1

allowed_planning_time: 5.0

planning_options:

planning_scene_diff:

robot_state:

joint_state:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: ''

position: []

velocity: []

effort: []

multi_dof_joint_state:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: ''

joint_names: []

transforms: []

twist: []

wrench: []

attached_collision_objects: []

is_diff: True

robot_model_name: ''

fixed_frame_transforms: []

allowed_collision_matrix:

entry_names: []

entry_values: []

default_entry_names: []

default_entry_values: []

link_padding: []

link_scale: []

object_colors: []

world:

collision_objects: []

octomap:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: ''

origin:

position:

x: 0.0

y: 0.0

z: 0.0

orientation:

x: 0.0

y: 0.0

z: 0.0

w: 0.0

octomap:

header:

seq: 0

stamp:

secs: 0

nsecs: 0

frame_id: ''

binary: False

id: ''

resolution: 0.0

data: []

is_diff: True

plan_only: False

look_around: False

look_around_attempts: 0

max_safe_execution_cost: 0.0

replan: False

replan_attempts: 0

replan_delay: 2.0

reem_both_arms1.jpg

reem_both_arms2.jpg

reem_both_arms3.jpg

Sachin Chitta

unread,

Jan 21, 2014, 6:22:59 PM1/21/14

to Sam Pfeiffer, moveit-users, Philippe Capdepuy, Sudarshan Srinivasan

You can do this - I added an example to the move_group_interface_tutorial - you can find the pull request in the moveit_pr2 package if you want to have a look at the code. It should show up on the webpage (http://docs.ros.org/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html) in a few days (once rosdoc runs). Let me know if this is what you were looking for.

Sachin

Li Yang Ku

unread,

Aug 18, 2014, 12:02:30 PM8/18/14

to moveit...@googlegroups.com, r0s...@gmail.com, p...@generationrobots.com, sudar...@gmail.com

Hi,

I am currently working on the Robonaut 2 simulator with moveit! (Hydro version https://bitbucket.org/swhart115/nasa_robot_teleop ). I am trying to control both arms simultaneously by adding a "dual_arms" group that contains two subgroups "left_arm" and "right_arm". When I test the GUI moveit! interface in Rviz, the interactive marker of the left arm showed up and I am able to control just the left arm correctly both through the python interface (setting goal pose) and the rviz interface. When I click on generate random pose, it also correctly generates a plan for both right and left arm and is able to execute both arm actions simultaneously. I am also able to control both arms in joint space through the python interface. However, when I try to send a pose goal for the right arm (or both right and left arm) through the python interface it fails and shows the following error message.

[ERROR] [1408376952.454978116, 31.732000000]: RRTConnect: Unable to sample any valid states for goal tree

The python code is shown below. I tested the right hand part individually and confirmed that the coordinate works correctly when the group is just "right_arm"

moveit_commander.roscpp_initialize(sys.argv)

rospy.init_node('move_group_python_interface_tutorial',

anonymous=True)

robot = moveit_commander.RobotCommander()

scene = moveit_commander.PlanningSceneInterface()

group = moveit_commander.MoveGroupCommander("arms")

# Joint Space works

# group_variable_values = group.get_current_joint_values()

# print "============ Joint values: ", group_variable_values

#

# group_variable_values[0] = 0.8

# group_variable_values[7] = -0.8

# group.set_joint_value_target(group_variable_values)

roll = -1.57

pitch = 0

yaw = 0

q = quaternion_from_euler(roll, pitch, yaw)

pose_target = geometry_msgs.msg.PoseStamped()

pose_target.header.frame_id = 'r2/robot_reference'

pose_target.pose.orientation.x = q[0]

pose_target.pose.orientation.y = q[1]

pose_target.pose.orientation.z = q[2]

pose_target.pose.orientation.w = q[3]

pose_target.pose.position.x = 0.55

pose_target.pose.position.y = -0.25

pose_target.pose.position.z = -0.3

group.set_pose_target(pose_target,"r2/left_palm")

roll = 1.57

pitch = 0

yaw = 0

q = quaternion_from_euler(roll, pitch, yaw)

pose_target_r = geometry_msgs.msg.PoseStamped()

pose_target_r.header.frame_id = 'r2/robot_reference'

pose_target_r.pose.orientation.x = q[0]

pose_target_r.pose.orientation.y = q[1]

pose_target_r.pose.orientation.z = q[2]

pose_target_r.pose.orientation.w = q[3]

pose_target_r.pose.position.x = 0.55

pose_target_r.pose.position.y = 0.25

pose_target_r.pose.position.z = -0.3

group.set_pose_target(pose_target_r,"r2/right_palm")

plan1 = group.plan()

group.go(wait=True)

Please let me know if anyone has any insight on why this is not working. Thanks!

Best Regards,

Li

Sam Pfeiffer

unread,

Aug 22, 2014, 7:28:21 AM8/22/14

to moveit...@googlegroups.com, r0s...@gmail.com, p...@generationrobots.com, sudar...@gmail.com

Hello Li,

I remember having the same problem (concretely, not having 2 markers in Rviz to move both arms) and I remember I fixed it just playing with setup assistant options on adding the two groups and it's end effectors. I can't recall what I did but you can check the configuration of REEM:

https://github.com/pal-robotics/reem_moveit_config

The group is here:

https://github.com/pal-robotics/reem_moveit_config/blob/master/config/reem.srdf#L100

To have a reference robot you could follow the install instructions of REEM simulation:

http://wiki.ros.org/Robots/REEM/Tutorials/Launching%20a%20REEM%20Gazebo%20simulation

And it's moveit tutorial:

http://wiki.ros.org/Robots/REEM/Tutorials/moveit

Good luck :)

Li Yang Ku

unread,

Sep 2, 2014, 4:51:37 PM9/2/14

to moveit...@googlegroups.com, r0s...@gmail.com, p...@generationrobots.com, sudar...@gmail.com

So I changed the groups left_arm and right_arm from using chain to joints and it worked for the both_arm group I created, which includes all joints in the right_arm and left_arm group. I also deleted the parent group of the two end effectors. Both interactive markers show up and I am able to control it through the python API now.

Thanks Sam for providing a working example.

Below are the sections that I modified in the srdf file.

</group>

</group>

</group>

<end_effector name="left_hand_ee" parent_link="r2/left_palm" group="left_hand" />

<end_effector name="right_hand_ee" parent_link="r2/right_palm" group="right_hand" />