def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. release the hand 2. backup the hand 3. plan a path to the a place 15cm back from the new pose 4. use guarded motion to move forward """ print 'adjustment:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1,0,0,0], [0,1,0,0], [0,0,1,-.05], [0,0,0,1]]); back_from_arm_goal = arm_goal * backup #move back by 10cm #move_forward(-.1) #raw_input("Press enter to go to the target pose...") #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) send_cartesian_goal(back_from_arm_goal, True) raw_input("Press enter to use guarded motion to move forward...") #guarded move forward tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]]) GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #tp.move_hand_velocity([0.5,0.5,0.5,0.0]) GuardedCloseHand(self.global_data) selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' success = tp.lift_arm(.1, True) if not success: grasp_status = graspit_msgs.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object'
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. release the hand 2. backup the hand 3. plan a path to the a place 15cm back from the new pose 4. use guarded motion to move forward """ print 'adjustment:' print grasp_msg #release the object tp.open_barrett() #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -.05], [0, 0, 0, 1]]) back_from_arm_goal = arm_goal * backup #move back by 10cm #move_forward(-.1) #raw_input("Press enter to go to the target pose...") #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) send_cartesian_goal(back_from_arm_goal, True) raw_input("Press enter to use guarded motion to move forward...") #guarded move forward tp.MoveHandSrv(1, [0, 0, 0, grasp_msg.pre_grasp_dof[0]]) GuardedMoveUntilHit(self.global_data, array([0, 0, 1]), 'PALM', 0.05, 20) raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #tp.move_hand_velocity([0.5,0.5,0.5,0.0]) GuardedCloseHand(self.global_data) selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' success = tp.lift_arm(.1, True) if not success: grasp_status = graspit_msgs.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object'
def adjust(self, adjust_msg, close_hand = True): """ adjust_msg specifies the adjustment in palm's local coordinate system """ print 'adjust:' print adjust_msg #release the object tp.open_barrett() #tp.move_hand_percentage(0.8) #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(adjust_msg.offset)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1,0,0,0], [0,1,0,0], [0,0,1,-.05], [0,0,0,1]]); back_from_arm_goal = arm_goal * backup #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) #send_cartesian_goal(back_from_arm_goal, True) send_cartesian_goal(arm_goal, True) #raw_input("Press enter to use guarded motion to move forward...") #guarded move forward #GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) #raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #gentlly close the fingers if close_hand: hu.GuardedCloseHand(self.global_data) tp.move_hand_velocity([0.1,0.1,0.1,0])
def adjust(self, adjust_msg, close_hand=True): """ adjust_msg specifies the adjustment in palm's local coordinate system """ print 'adjust:' print adjust_msg #release the object tp.open_barrett() #tp.move_hand_percentage(0.8) #get the robot's current location and calculate the absolute tran after the adjustment #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) adjustInPalm = pm.toMatrix(pm.fromMsg(adjust_msg.offset)) arm_goal = tp.convert_cartesian_relative_goal(self.global_data, adjustInPalm) backup = numpy.matrix([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -.05], [0, 0, 0, 1]]) back_from_arm_goal = arm_goal * backup #update the robot status tp.update_robot(self.global_data.or_env.GetRobots()[0]) #just go to the new place without a OpenRave trajectory plan #adjustInPalm = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) #blocking motion set to be true #send_cartesian_relative_goal(self.global_data, adjustInPalm, True) #send_cartesian_goal(back_from_arm_goal, True) send_cartesian_goal(arm_goal, True) #raw_input("Press enter to use guarded motion to move forward...") #guarded move forward #GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) #raw_input("Press enter to close the hand...") #close the hand #tp.close_barrett() #gentlly close the fingers if close_hand: hu.GuardedCloseHand(self.global_data) tp.move_hand_velocity([0.1, 0.1, 0.1, 0])