kth-ros-pkg / yumi

ROS packages pertaining to the ABB YuMi (IRB 14000) robot

Geek Repo:Geek Repo

Github PK Tool:Github PK Tool

How to control both arms?

kappa95 opened this issue · comments

I would like to control both arm with poses instead of joint angles. I tried something like:

    Pose_L = create_pose_euler(target[0][0], target[0][1], target[0][2], target[0][3], target[0][4], target[0][5])
    Pose_R = create_pose_euler(target[1][0], target[1][1], target[1][2], target[1][3], target[1][4], target[1][5])
    group_l.set_pose_target(Pose_L)
    group_r.set_pose_target(Pose_R)
    planR = group_r.plan()
    planL = group_l.plan()
    group_l.go(wait=False)
    group_r.go(wait=True)

But i receive this error:
terminate called after throwing an instance of 'boost::exception_detail::clone_impl<boost::exception_detail::error_info_injector<boost::lock_error> >' what(): boost: mutex lock failed in pthread_mutex_lock: Invalid argument

Solved modifying the URDF/moveit config with the moveit assistant in this way:

  • Create another group for both arm in which are used the set of the joints of the 2 arms instead of the subgroups or the chain.

Example for using with Python:

group_both = MoveGroupCommander("both_arms")
group_both.set_pose_target(poseR, "yumi_link_7_r")
group_both.set_pose_target(poseL, "yumi_link_7_l")
plan = group_both.plan()
group_both.execute(plan)