def grasp_object(arm_name, hand_name, graspable_object):
error_code = ErrorCode('grasp_not_available')
gp = GraspPlannerInterface()
for gc in gp.get_configurations(arm_name, hand_name, graspable_object)
mp = MotionPlan()
# mp += MoveBase(gc.location)
mp += MoveHand(hand_name, gc.hand_open, blocking=False)
mp += MoveArm(arm_name, gc.arm_pose) #Planned with open hand, in object link
mp += MoveHand(hand_name, gc.hand_grasp, verify_cb = gc.verify) # verification is done in executing stage only
mp += AttachObject(hand_name, graspable_object, gc)
mp += MoveArmInterpolated(arm_name ,gc.arm_lift) #Planned with object
mp += MoveArm(arm_name,'hold') #Planned with object
error_code = mp.plan(retries = 5)
if error_code.success: # planned all steps
error_code = mp.excute(retries = 2):
break # error handling is not done here
else:
gc.reject(error_code)
return error_code
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x,o.y,o.z,o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
p.y += -0.05
p.z += 0.03
mp = MotionPlan()
#mp += MoveArm('arm', 'pregrasp')
mp += MoveComponent('tray', 'down')
mp += MoveComponent('sdh', 'cylopen')
mp += MoveArm("arm",[grasp_pose,['sdh_grasp_link']])
mp += MoveComponent('sdh', 'cylclosed')
mp += AttachObject('arm', 'milk')
mp += EnableCollision('milk','table_ikea')
mp += MoveArm("arm",[lift_pose,['sdh_grasp_link']])
mp += ResetCollisions()
#mp += MoveArm('arm', 'home')
#mp += MoveComponent('tray', 'up')
#mp += MoveArm('arm', 'hold')
mp += MoveArm('arm', [['base_link',[0.64-0.161, -0.11+0.056,0.832+0.3],[-1.441, 0.118, -0.078]],['sdh_grasp_link']])
mp += MoveComponent('tray', 'up')
