Can I move both arms of YuMi at the same time using moveit!

[Miya-imslab]

Can I move both arms of YuMi at the same time using moveit!
If possible, I would like to know how to write a program to move both arms at the same time.

[kappa95]

The method that I had discovered two years ago was to change the moveit package to have a newer moveit group with both arm together and defining two different end effector. Then specify each end effector in the set goal pose...

Good luck!

[zhyma]

I forked this repo and made some modifications. Here is my implementation for moving both arms at the same time: https://github.com/zhyma/yumi/tree/bimanual_rws

usage:

...
ctrl_group = moveit_commander.MoveGroupCommander('both_arms')
client = actionlib.SimpleActionClient('/yumi/joint_traj_pos_controller_both/follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
...
val = [-1.1694, -2.3213, 1.1694, -0.3665, 0.2792, 1.2392, -0.3491,\
                       1.1694, -2.3213, -1.1694, -0.3665, 0.2792, 1.2392, -0.3491]
ctrl_group.set_joint_value_target(value)
ctrl_group.go(wait=True)
ctrl_group.stop()

and

def exec(self, group, value_list, dt, start_time=0):
        goal =  FollowJointTrajectoryGoal()
        goal.trajectory.joint_names = ['yumi_joint_1_l','yumi_joint_2_l','yumi_joint_7_l','yumi_joint_3_l','yumi_joint_4_l','yumi_joint_5_l','yumi_joint_6_l',\
                                           'yumi_joint_1_r','yumi_joint_2_r','yumi_joint_7_r','yumi_joint_3_r','yumi_joint_4_r','yumi_joint_5_r','yumi_joint_6_r']

        for i in range(len(value_list)):
            point = JointTrajectoryPoint()
            point.positions = value_list[i]
            point.time_from_start = rospy.Duration(start_time+dt*i)

            goal.trajectory.points.append(point)

        self.client.send_goal(goal)
        self.client.wait_for_result()

        return self.client.get_result()