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')