def execute(self, userdata): theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) return 'outcome1'
def execute(self, userdata): #Announce start of Reset pubReset = rospy.Publisher('robot_reset', Robot, queue_size=1,latch=True) pubReset.publish(1) rospy.sleep(0.1) #Target position of reset: centre of hand in x=0.0, y=0.2, z=0.585 set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) #Select random initial position for the robot set_robot_joints([0.0, 0.0, 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5), 0.0, 0.0, random.uniform(-0.5,0.5),0.0]) return 'outcome1'
def execute(self, userdata): #global cov #cov.start() #Path planning towards the piece theplans = interface([-0.5,0.0,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([-1.15,0.05,-0.75,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([-1.15,0.05,-0.48,-1000.0,1.39,-1000.0,-1000.0,-0.5,-1000.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1,latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1,latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface([0.0,0.05,-0.75,0.0,1.39,0.0,0.0,-0.5,0.0]) #print theplans for i,plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1,latch=True) hand2.publish(1) rospy.sleep(0.1) #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): #global cov #cov.start() # If online the assert_normal value will be set by a publisher subscriber # interaction to force the drop pathway if assert_normal == 1: trigger_drop_pathway = 1 else: # Randomly assign the value trigger_drop_pathway = random.randint(0, 10) theplans = interface( [-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) #print theplans for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) # Introduce state fork if trigger_drop_pathway == 1: return 'outcome2' else: return 'outcome1'
def execute(self, userdata): #Reset position with hand open #global cov #cov.start() theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) for i,plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) global table_timeout table_timeout = time.time() #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): #Reset position with hand open #global cov #cov.start() theplans = interface([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) set_robot_joints( [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) move_hand('open') rospy.sleep(0.5) global table_timeout table_timeout = time.time() #cov.stop() #cov.save() return 'outcome1'
def execute(self, userdata): #global cov #cov.start() #Path planning towards the piece #x = random.randint(0, 10) trigger_drop_pathway = 2 theplans = interface( [-0.5, 0.0, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.75, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) theplans = interface([ -1.15, 0.05, -0.48, -1000.0, 1.39, -1000.0, -1000.0, -0.5, -1000.0 ]) for i, plan in enumerate(theplans): set_robot_joints(plan) move_hand('close') rospy.sleep(0.5) hand = rospy.Publisher('robot_gripper', Int8, queue_size=1, latch=True) hand.publish(1) rospy.sleep(0.5) hand.publish(0) pubpress = rospy.Publisher('pressure_e1', Int8, queue_size=1, latch=True) pubpress.publish(1) rospy.sleep(0.2) #Path planning towards goal location theplans = interface( [0.0, 0.05, -0.75, 0.0, 1.39, 0.0, 0.0, -0.5, 0.0]) #print theplans for i, plan in enumerate(theplans): set_robot_joints(plan) rospy.sleep(1) hand2 = rospy.Publisher('robot_has_piece', Int8, queue_size=1, latch=True) hand2.publish(1) rospy.sleep(0.1) if trigger_drop_pathway == 2: #Will I need to thread this return 'outcome2' #cov.stop() #cov.save() return 'outcome1'
def main(): global is_a_sim is_a_sim = 1 rospy.set_param('is_this_a_simulation',is_a_sim) rospy.init_node('robot', anonymous=True) #Start node first if is_a_sim: set_robot_joints([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Make sure some joint angles have been published to bert2/joint_states before # instantiating bert2_interface.Body(). global bert2 bert2 = bert2_interface.Body() setmodel = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) for k in range(0,10): Reset() Move() Release() # Reset object rospy.wait_for_service('/gazebo/set_model_state') setmodel(ModelState('object',Pose(Point(0.3,-0.44,1.2),Quaternion(0.0,0.0,0.0,1.0)),Twist(Vector3(0.0,0.0,0.0),Vector3(0.0,0.0,0.0)),'world'))