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