How to control both arms?
kappa95 opened this issue · comments
Federico Insero commented
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
Federico Insero commented
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)