def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. plan a path to the a place 15cm back from the new pose """ print 'regular move:' 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]) handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) try: hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase)) bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal", "armbase") self.global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = self.global_data.listener.lookupTransform( 'armbase', 'arm_goal', now) armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_world_goal failed: error %s.' % e)
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])
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to make the adjustment specified by grasp_msg 1. plan a path to the a place 15cm back from the new pose """ print 'regular move:' 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]) handGoalInBase = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) try: hand_tf = pm.toTf(pm.fromMatrix(handGoalInBase)) bc = tf.TransformBroadcaster() now = rospy.Time.now() bc.sendTransform(hand_tf[0], hand_tf[1], now, "hand_goal", "armbase") self.global_data.listener.waitForTransform("armbase", "arm_goal", now, rospy.Duration(1.0)) armtip_robot = self.global_data.listener.lookupTransform('armbase', 'arm_goal', now) armGoalInBase = pm.toMatrix(pm.fromTf(armtip_robot)) except Exception, e: handle_fatal('convert_cartesian_world_goal failed: error %s.'%e)
def process_explore(self, explore_msg): """@brief - Attempt to explore """ #number of points we want to collect along x, z directions count_x = 5 count_z = 5 #distance we want to cover along x, z directions dist_x = 0.03 dist_z = 0.03 tp.open_barrett() #first want to find the boundary along z-direction, for 3cm to test distance_moved, isHit = hu.GuardedMoveUntilHit(self.global_data, array([0, 0, 1]), 'PALM', dist_z / 2) #pdb.set_trace() #move back to the ideal location to start local exploration hu.GuardedMoveUntilHit(self.global_data, array([0, 0, -1]), 'PALM', dist_z, 500) #start local exploration count = 0 self.start_explore() adjust_msg = Adjust() #identity matrix initially adjust_z_msg = Adjust() adjust_accumulated = Adjust() adjust_msg.offset.orientation.w = 1 #it is zero initially adjust_msg.offset.position.x = dist_x / (count_x - 1 ) #step size:3mm along x adjust_z_msg.offset.orientation.w = 1 adjust_z_msg.offset.position.z = dist_z / (count_z - 1 ) #step size:5mm along z adjust_accumulated.offset.orientation.w = 1 #initial x,z is set so that the fingers are centered at the middle point of the range adjust_accumulated.offset.position.x = dist_x / 2.0 adjust_accumulated.offset.position.z = -dist_z / 2.0 """ At the beginning we need to make sure the hand is backed up This is the first step for local reconstruction """ pdb.set_trace() self.adjust(adjust_accumulated) #hu.GuardedCloseHand(self.global_data) #tp.move_hand_velocity([0.1,0.1,0.1,0]) while (count < count_x * count_z): #pdb.set_trace() self.collect_point_cloud(adjust_accumulated.offset) if (count == count_x * count_z - 1): break #make a move if mod(count + 1, count_x) == 0: #end of one vertical line, move forward self.adjust(adjust_z_msg) adjust_accumulated.offset.position.z = adjust_accumulated.offset.position.z + adjust_z_msg.offset.position.z else: #not the end of the vertical line if mod( count, count_x ) == 0: #start of one vertical line, change up/down direction and move up/down adjust_msg.offset.position.x = -adjust_msg.offset.position.x adjust_accumulated.offset.position.x = adjust_accumulated.offset.position.x + adjust_msg.offset.position.x self.adjust(adjust_msg) count = count + 1 #now go back to the original position adjust_back = Adjust() adjust_back.offset.position.x = -adjust_accumulated.offset.position.x adjust_back.offset.position.z = -adjust_accumulated.offset.position.z pdb.set_trace() self.adjust(adjust_back, False) self.end_explore()
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to grasp the object and lift it First moves the arm to pregrasp the object, then attempts to grasp and lift it. The grasp and lifting phase is currently completely open loop """ try: #Lock the openrave environment - it will restore the robot and collision state #to it's previous state when it finishes automagically. with self.global_data.or_env: #Reject multiple grasp messages sent too fast. They are likely a mistake #FIXME - We should probably just set the queue size to 1. if (time() - self.last_grasp_time) < 30: return [], [] self.last_grasp_time = time() print grasp_msg grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS grasp_status_msg = "grasp_succeeded" success = 1 # Send the robot to its home position if it is actually running #and not currently there if self.global_data.robot_running and not tp.is_home(): print 'go home' tp.go_home(self.global_data) #This sometimes fails near the end of the trajectory because the last few #steps of the trajectory are slow because our blending parameters #are probably wrong. It is basically in the home position, so we #don't check for success in reaching the home position because meaningful #failures are rare. #Open the hand - Leaves spread angle unchanged if success and self.global_data.robot_running: success, grasp_status_msg, positions = tp.open_barrett() if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR if success: #Pregrasp the object #Convert the grasp message to a openrave transform grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) grasp_tran[0:3,3] /=1000 #mm to meters #Move the hand to the pregrasp spread angle - Update openrave's robot pose if self.global_data.robot_running: tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]]) tp.update_robot(self.global_data.or_env.GetRobots()[0]) print 'pre-grasp' #Publish the object TFs self.model_manager() print 'after model_manager()' #Calculate a trajectory to the pregrasp pose and move to it if necessary success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, grasp_msg.object_name, False, grasp_tran, run_it = self.global_data.robot_running) print 'after pre-grasp' #Failures shouldn't happen if grasp analysis is being used, so that's wierd. if not success: pdb.set_trace() #Make sure that the openrave environment is up to date in pregrasp pose if self.global_data.robot_running: tp.update_robot(self.global_data.or_env.GetRobots()[0]) else: #If the robot isn't actually running, just update the openrave visualization #so that the user can see what was planned. if success: print "setting robot joints" print j self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6)) self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(dof_list[-1]) self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6,10)) self.global_data.or_env.GetRobots()[0].SetActiveDOFValues(list(grasp_msg.pre_grasp_dof[1:]) + [grasp_msg.pre_grasp_dof[0]]) self.global_data.or_env.GetRobots()[0].SetActiveDOFs(range(6)) self.global_data.or_env.UpdatePublishedBodies() pdb.set_trace() success = False #If the pregrasp was unreachable, record the type of issue if not success: #If there is no joint list, the pregrasp transform had no inverse #kinematics solution, probably. if not j: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Pregrasp tran Out of range!" #Otherwise, something even wierder happened. else: grasp_status = graspit_msgs.msg.GraspStatus.FAILED grasp_status_msg = "Unknown planner failure!" #Move from the pregrasp to the final grasp pose in a straight line along the approach #direction if success: if not Hao: #Move forward open loop success = tp.move_forward(0.05, True) else: #Move forward using force transducer and palm contact sensors to detect any #collisions hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) success = True if not success: #If we failed to move forward - which might happen because #we do not test if the final grasp pose is reachable along #a straight line trajectory because the joint range limits #set on the robot are not actually exactly the same as those #in openrave and it usually doesn't cause any problems #in general trajectory testing. grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Unable to move forward to grasp!" if Hao: """new routine""" #Close the hand using the force torque and contact sensors to detect contacts and #stop the fingers hu.GuardedCloseHand(self.global_data) else: """old routine""" if success: #Just close the hand open loop, leaving the spread angle where it is now. success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]]) if success: #Close the hand completely until the motors stall or they hit #the final grasp DOFS success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]]) if success: #Now close the hand completely until the motors stall. success, grasp_status_msg, joint_angles = tp.close_barrett() if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR #Now wait for user input on whether or not to lift the object if success: selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' #Lift the object using the staubli's straight line path planner success = tp.lift_arm(.05, True) if not success: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object' #Maybe decide if it has been successfully lifted here... if success: rospy.logwarn(grasp_status_msg) else: rospy.logfatal(grasp_status_msg) #Tell graspit whether the grasp succeeded self.graspit_status_publisher.publish(grasp_status, grasp_status_msg, -1) print grasp_status_msg return grasp_status, grasp_status_msg except Exception as e: #print any exceptions and drop in to the debugger. import traceback print traceback.format_exc() pdb.set_trace()
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to grasp the object and lift it First moves the arm to pregrasp the object, then attempts to grasp and lift it. The grasp and lifting phase is currently completely open loop """ try: #Lock the openrave environment - it will restore the robot and collision state #to it's previous state when it finishes automagically. with self.global_data.or_env: #Reject multiple grasp messages sent too fast. They are likely a mistake #FIXME - We should probably just set the queue size to 1. if (time() - self.last_grasp_time) < 30: return [], [] self.last_grasp_time = time() print grasp_msg grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS grasp_status_msg = "grasp_succeeded" success = 1 # Send the robot to its home position if it is actually running #and not currently there if self.global_data.robot_running and not tp.is_home(): print 'go home' tp.go_home(self.global_data) #This sometimes fails near the end of the trajectory because the last few #steps of the trajectory are slow because our blending parameters #are probably wrong. It is basically in the home position, so we #don't check for success in reaching the home position because meaningful #failures are rare. #Open the hand - Leaves spread angle unchanged if success and self.global_data.robot_running: success, grasp_status_msg, positions = tp.open_barrett() if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR if success: #Pregrasp the object #Convert the grasp message to a openrave transform grasp_tran = pm.toMatrix( pm.fromMsg(grasp_msg.final_grasp_pose)) grasp_tran[0:3, 3] /= 1000 #mm to meters #Move the hand to the pregrasp spread angle - Update openrave's robot pose if self.global_data.robot_running: tp.MoveHandSrv(1, [0, 0, 0, grasp_msg.pre_grasp_dof[0]]) tp.update_robot(self.global_data.or_env.GetRobots()[0]) print 'pre-grasp' #Publish the object TFs self.model_manager() print 'after model_manager()' #Calculate a trajectory to the pregrasp pose and move to it if necessary success, final_tran, dof_list, j = tp.pregrasp_object( self.global_data, grasp_msg.object_name, False, grasp_tran, run_it=self.global_data.robot_running) print 'after pre-grasp' #Failures shouldn't happen if grasp analysis is being used, so that's wierd. if not success: pdb.set_trace() #Make sure that the openrave environment is up to date in pregrasp pose if self.global_data.robot_running: tp.update_robot(self.global_data.or_env.GetRobots()[0]) else: #If the robot isn't actually running, just update the openrave visualization #so that the user can see what was planned. if success: print "setting robot joints" print j self.global_data.or_env.GetRobots( )[0].SetActiveDOFs(range(6)) self.global_data.or_env.GetRobots( )[0].SetActiveDOFValues(dof_list[-1]) self.global_data.or_env.GetRobots( )[0].SetActiveDOFs(range(6, 10)) self.global_data.or_env.GetRobots( )[0].SetActiveDOFValues( list(grasp_msg.pre_grasp_dof[1:]) + [grasp_msg.pre_grasp_dof[0]]) self.global_data.or_env.GetRobots( )[0].SetActiveDOFs(range(6)) self.global_data.or_env.UpdatePublishedBodies() pdb.set_trace() success = False #If the pregrasp was unreachable, record the type of issue if not success: #If there is no joint list, the pregrasp transform had no inverse #kinematics solution, probably. if not j: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Pregrasp tran Out of range!" #Otherwise, something even wierder happened. else: grasp_status = graspit_msgs.msg.GraspStatus.FAILED grasp_status_msg = "Unknown planner failure!" #Move from the pregrasp to the final grasp pose in a straight line along the approach #direction if success: if not Hao: #Move forward open loop success = tp.move_forward(0.05, True) else: #Move forward using force transducer and palm contact sensors to detect any #collisions hu.GuardedMoveUntilHit(self.global_data, array([0, 0, 1]), 'PALM', 0.05, 20) success = True if not success: #If we failed to move forward - which might happen because #we do not test if the final grasp pose is reachable along #a straight line trajectory because the joint range limits #set on the robot are not actually exactly the same as those #in openrave and it usually doesn't cause any problems #in general trajectory testing. grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Unable to move forward to grasp!" if Hao: """new routine""" #Close the hand using the force torque and contact sensors to detect contacts and #stop the fingers hu.GuardedCloseHand(self.global_data) else: """old routine""" if success: #Just close the hand open loop, leaving the spread angle where it is now. success, grasp_status_msg, joint_angles = tp.move_hand([ grasp_msg.pre_grasp_dof[1], grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0] ]) if success: #Close the hand completely until the motors stall or they hit #the final grasp DOFS success, grasp_status_msg, joint_angles = tp.move_hand([ grasp_msg.final_grasp_dof[1], grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0] ]) if success: #Now close the hand completely until the motors stall. success, grasp_status_msg, joint_angles = tp.close_barrett( ) if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR #Now wait for user input on whether or not to lift the object if success: selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' #Lift the object using the staubli's straight line path planner success = tp.lift_arm(.05, True) if not success: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object' #Maybe decide if it has been successfully lifted here... if success: rospy.logwarn(grasp_status_msg) else: rospy.logfatal(grasp_status_msg) #Tell graspit whether the grasp succeeded self.graspit_status_publisher.publish(grasp_status, grasp_status_msg, -1) print grasp_status_msg return grasp_status, grasp_status_msg except Exception as e: #print any exceptions and drop in to the debugger. import traceback print traceback.format_exc() pdb.set_trace()
def process_grasp_msg(self, grasp_msg): """@brief - Attempt to grasp the object and lift it First moves the arm to pregrasp the object, then attempts to grasp and lift it. The grasp and lifting phase is currently completely open loop """ if (time() - self.last_grasp_time) < 30: return [], [] self.last_grasp_time = time() print grasp_msg grasp_status = graspit_msgs.msg.GraspStatus.SUCCESS grasp_status_msg = "grasp_succeeded" success = 1 if not tp.is_home(): print 'go home' tp.go_home(self.global_data) # if not success: # grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE # grasp_status_msg = "Unable to go home" if success: success, grasp_status_msg, positions = tp.open_barrett() if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR if success: grasp_tran = pm.toMatrix(pm.fromMsg(grasp_msg.final_grasp_pose)) grasp_tran[0:3,3] /=1000 #mm to meters tp.MoveHandSrv(1, [0,0,0, grasp_msg.pre_grasp_dof[0]]) tp.update_robot(self.global_data.or_env.GetRobots()[0]) print 'pre-grasp' self.model_manager() # success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, file_name_dict[self.target_object_name], grasp_tran) print 'after model_manager()' success, final_tran, dof_list, j = tp.pregrasp_object(self.global_data, self.target_object_name, False, grasp_tran) print 'after pre-grasp' #raw_input("Press enter...") tp.update_robot(self.global_data.or_env.GetRobots()[0]) if not success: if not j: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Pregrasp tran Out of range!" else: grasp_status = graspit_msgs.msg.GraspStatus.FAILED grasp_status_msg = "Unknown planner failure!" if success: if not Hao: success = tp.move_forward(0.05, True) else: hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', 0.05, 20) success = True if not success: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Unable to move forward to grasp!" if Hao: """new routine""" hu.GuardedCloseHand(self.global_data) else: """old routine""" if success: success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.pre_grasp_dof[1],grasp_msg.pre_grasp_dof[2], grasp_msg.pre_grasp_dof[3], grasp_msg.pre_grasp_dof[0]]) if success: success, grasp_status_msg, joint_angles = tp.move_hand([grasp_msg.final_grasp_dof[1],grasp_msg.final_grasp_dof[2], grasp_msg.final_grasp_dof[3], grasp_msg.final_grasp_dof[0]]) if success: success, grasp_status_msg, joint_angles = tp.close_barrett() if not success: grasp_status = graspit_msgs.msg.GraspStatus.ROBOTERROR if success: selection = int(raw_input('Lift up (1) or not (0): ')) if selection == 1: print 'lift up the object' success = tp.lift_arm(.05, True) if not success: grasp_status = graspit_msgs.msg.GraspStatus.UNREACHABLE grasp_status_msg = "Couldn't lift object" else: print 'not lift up the object' #Maybe decide if it has been successfully lifted here... if success: rospy.logwarn(grasp_status_msg) else: rospy.logfatal(grasp_status_msg) self.graspit_status_publisher.publish(grasp_status, grasp_status_msg) print grasp_status_msg return grasp_status, grasp_status_msg
def process_explore(self, explore_msg): """@brief - Attempt to explore """ #number of points we want to collect along x, z directions count_x = 5 count_z = 5 #distance we want to cover along x, z directions dist_x = 0.03 dist_z = 0.03 tp.open_barrett() #first want to find the boundary along z-direction, for 3cm to test distance_moved, isHit = hu.GuardedMoveUntilHit(self.global_data, array([0,0,1]), 'PALM', dist_z / 2) #pdb.set_trace() #move back to the ideal location to start local exploration hu.GuardedMoveUntilHit(self.global_data, array([0,0,-1]), 'PALM', dist_z, 500) #start local exploration count = 0 self.start_explore() adjust_msg = Adjust()#identity matrix initially adjust_z_msg = Adjust() adjust_accumulated = Adjust() adjust_msg.offset.orientation.w = 1#it is zero initially adjust_msg.offset.position.x = dist_x / (count_x-1) #step size:3mm along x adjust_z_msg.offset.orientation.w = 1 adjust_z_msg.offset.position.z = dist_z / (count_z-1) #step size:5mm along z adjust_accumulated.offset.orientation.w = 1 #initial x,z is set so that the fingers are centered at the middle point of the range adjust_accumulated.offset.position.x = dist_x / 2.0 adjust_accumulated.offset.position.z = - dist_z / 2.0 """ At the beginning we need to make sure the hand is backed up This is the first step for local reconstruction """ pdb.set_trace() self.adjust(adjust_accumulated) #hu.GuardedCloseHand(self.global_data) #tp.move_hand_velocity([0.1,0.1,0.1,0]) while (count < count_x * count_z): #pdb.set_trace() self.collect_point_cloud(adjust_accumulated.offset) if(count == count_x * count_z - 1): break #make a move if mod(count+1, count_x) == 0: #end of one vertical line, move forward self.adjust(adjust_z_msg) adjust_accumulated.offset.position.z = adjust_accumulated.offset.position.z + adjust_z_msg.offset.position.z else:#not the end of the vertical line if mod(count, count_x) == 0: #start of one vertical line, change up/down direction and move up/down adjust_msg.offset.position.x = - adjust_msg.offset.position.x adjust_accumulated.offset.position.x = adjust_accumulated.offset.position.x + adjust_msg.offset.position.x self.adjust(adjust_msg) count = count + 1 #now go back to the original position adjust_back = Adjust() adjust_back.offset.position.x = - adjust_accumulated.offset.position.x adjust_back.offset.position.z = - adjust_accumulated.offset.position.z pdb.set_trace() self.adjust(adjust_back, False) self.end_explore()