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