def main():
rospy.init_node('erase_whiteboard_demo')
wait_for_time()
argv = rospy.myargv()
ERASE_POSES = [[-0.239, -0.433, 1.401, 0.758, -2.997, 0.565, 1.742],
[-0.530, -0.423, 1.438, 0.714, -2.983, 0.266, 1.689],
[-0.251, -0.400, 1.469, 0.755, -2.993, 0.576, 1.667],
[-0.556, -0.395, 1.437, 0.726, -2.982, 0.181, 1.690]]
torso = fetch_api.Torso()
torso.set_height(0.4)
display = fetch_api.Display()
display.display_msg("Hello! I am going to erase the whiteboard for you!")
time.sleep(7)
gripper = fetch_api.Gripper()
gripper.close()
time.sleep(2)
arm = fetch_api.Arm()
torso.set_height(0.0)
for vals in ERASE_POSES:
arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
time.sleep(5)
display.display_msg("All done!")
time.sleep(2)
def execute(self):
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
for action in self.actions:
if action.action == ActionSaver.MOVE:
# Compute the pose of the wrist in base_link
ar = fetch_api.pose2matrix(action.arPose)
ar2wrist = fetch_api.pose2transform(action.arPose, action.wristPose, True)
# Save the ar marker's id and load it
wrist = np.dot(fetch_api.pose2matrix(filter(lambda x: x.id == action.arId, self.markers)[0].pose.pose), ar2wrist)
# Navigate the arm there
kwargs = {
'allowed_planning_time': 50,
'execution_timeout': 40,
'num_planning_attempts': 30,
'replan': False,
}
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
pose_stamped.pose = fetch_api.matrix2pose(wrist)
arm.move_to_pose(pose_stamped, **kwargs)
# Wait?
elif action.action == ActionSaver.OPEN:
gripper.open()
elif action.action == ActionSaver.CLOSE:
gripper.close()
def __init__(self, program_file=PROGRAM_FILE):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
Marker,
callback=self._markers_callback)
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._torso = fetch_api.Torso()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self._program_file = program_file
self._programs = self._read_in_programs()
self._joint_reader = JointStateReader()
mat = tft.identity_matrix()
mat[:, 0] = np.array([0, 0, -1, 0])
mat[:, 2] = np.array([1, 0, 0, 0])
o = tft.quaternion_from_matrix(mat)
self._constraint_pose = Pose(orientation=Quaternion(*o))
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'gripper_link'
oc.orientation = self._constraint_pose.orientation
oc.weight = 1.0
oc.absolute_z_axis_tolerance = 1.0
oc.absolute_x_axis_tolerance = 1.0
oc.absolute_y_axis_tolerance = 1.0
self._constraint = None
def __init__(self):
self._drink_orders = Queue()
self._is_working = False
# Initialize Navigation Server.
self._nav_server = NavigationServer()
self._nav_server.loadMarkers()
# Initialize Arm and Gripper
self._gripper = fetch_api.Gripper()
self._arm = fetch_api.Arm()
rospy.logdebug('Gripper and Arm instantiated.');
self._arm_server = ArmServer()
# Startup Fetch ARM
self._controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
self._controller_client.send_goal(goal)
# Wait for Fetch ARM
rospy.loginfo('Waiting for arm to start.')
self._controller_client.wait_for_result()
rospy.loginfo('Arm has started..')
# Instantiate subscription model with front-end
self._drink_order_subscriber = rospy.Subscriber('/drink_order', DrinkOrder, self.drink_order_handler)
self._drink_status_publisher = rospy.Publisher('/drink_status', DrinkStatus, queue_size=1)
def handle_put_box(req):
"""
Robot arrived at station, drop the box.
"""
global put_poses
# Remove the planning floor, since we don't want it to get in the way
remove_floor()
torso = fetch_api.Torso()
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
base = fetch_api.Base()
# Drop the box
print("Dropping the box")
move_pose(arm, DROP)
rospy.sleep(1.0)
gripper.open()
rospy.sleep(0.5)
# Move to carry pose
move_pose(arm, CARRY)
# Move back
print("Dropped box, backing up")
before_pos = robot_pose.position
after_pos = robot_pose.position
while distance(before_pos, after_pos) < 0.50:
after_pos = robot_pose.position
base.move(-0.1, 0.0)
return 0
def main():
rospy.init_node('interactive_gripper_demo')
im_server = InteractiveMarkerServer('gripper_im_server', q_size=10)
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
teleop = GripperTeleop(arm, gripper, im_server)
#auto_pick = AutoPickTeleop(arm, gripper, im_server)
rospy.spin()
def main():
global pose_marker_server
global nav_server
global target_pose_pub
global pose_names_pub
global current_pose
current_pose = geometry_msgs.msg.Pose(
orientation=geometry_msgs.msg.Quaternion(0, 0, 0, 1))
rospy.init_node('action_node')
wait_for_time()
pose_marker_server = InteractiveMarkerServer('simple_marker')
pose_names_pub = rospy.Publisher('/pose_names',
PoseNames,
queue_size=10,
latch=True)
nav_server = NavigationServer()
# Create
nav_server.loadMarkers()
arm_server = ArmServer()
message = PoseNames()
message.names = nav_server.pose_names_list
pose_names_pub.publish(message)
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
print 'Arm has been started.'
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
print 'Arm and gripper instantiated.'
# handle user actions
user_actions_sub = rospy.Subscriber('/user_actions', UserAction,
handle_user_actions)
arm_service = rospy.Service('barbot/arm_to_pose', ArmToPose,
arm_server.findGlass)
rospy.spin()
def execute(self):
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
for action in self.actions:
if action.action == ActionSaver.MOVE:
# Compute the pose of the wrist in base_link
ar = fetch_api.pose2matrix(action.arPose)
ar2wrist = fetch_api.pose2transform(action.arPose,
action.wristPose, True)
# Save the ar marker's id and load it
wrist = np.dot(
fetch_api.pose2matrix(
filter(lambda x: x.id == action.arId,
self.markers)[0].pose.pose), ar2wrist)
# Navigate the arm there
kwargs = {
'allowed_planning_time': 50,
'execution_timeout': 40,
'num_planning_attempts': 30,
'replan': False,
}
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
pose_stamped.pose = fetch_api.matrix2pose(wrist)
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
marker.id = 0
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose.position.x = pose_stamped.pose.position.x
marker.pose.position.y = pose_stamped.pose.position.y
marker.pose.position.z = pose_stamped.pose.position.z
marker.pose.orientation.x = pose_stamped.pose.orientation.x
marker.pose.orientation.y = pose_stamped.pose.orientation.y
marker.pose.orientation.z = pose_stamped.pose.orientation.z
marker.pose.orientation.w = pose_stamped.pose.orientation.w
marker.scale.x = 0.2
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.viz_pub.publish(marker)
arm.move_to_pose(pose_stamped, **kwargs)
# Wait?
elif action.action == ActionSaver.OPEN:
gripper.open()
elif action.action == ActionSaver.CLOSE:
gripper.close()
def __init__(self):
self.actions = []
self.reader = ArTagReader()
rospy.sleep(0.1)
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.reader.callback)
rospy.sleep(0.1)
self.tf_listener = tf.TransformListener()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
self.gripper = fetch_api.Gripper()
def main():
rospy.init_node('gripper_im_server_node')
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
im_server = InteractiveMarkerServer('gripper_im_server', q_size=2)
auto_pick_server = InteractiveMarkerServer('auto_pick_im_server', q_size=2)
teleop = GripperTeleop(arm, gripper, im_server)
auto_pick = AutoPickTeleop(arm, gripper, auto_pick_server)
teleop.start()
auto_pick.start()
rospy.spin()
def main():
global nav_server
global WORKING
global arm_server
global orders
global sound_handler
sound_handler = SoundClient()
orders = []
WORKING = None
rospy.init_node('barbot_controller_node')
wait_for_time()
nav_server = NavigationServer()
nav_server.loadMarkers()
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
arm_server = ArmServer()
print 'Arm server instantiated.'
# MOVE TO MICHAEL_NODE
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
rospy.sleep(0.5)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
arm_server.lookup()
arm_server.set_init_poses()
arm_server.set_prepose()
# nav_server.goToMarker(BAR_TABLE)
print 'going home'
nav_server.goToMarker(HOME)
print 'please start to order now'
sound_handler.say('You may now order.')
# # handle user actions
drink_order_sub = rospy.Subscriber('/drink_order', DrinkOrder,
handle_user_actions)
signal.signal(signal.SIGINT, signal_handler)
while True:
query_orders()
rospy.sleep(0.1)
def __init__(self):
# TODO: Either implement behavior that fixes programs when markers change
# or only let this callback run once
self._markers_sub = rospy.Subscriber(SUB_NAME,
AlvarMarkers,
callback=self._markers_callback)
self._programs = self._read_in_programs()
self._curr_markers = None
self._tf_listener = tf.TransformListener()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
self._controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
def main():
rospy.init_node('gripper_teleop')
wait_for_time()
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
im_server = InteractiveMarkerServer('gripper_im_server', q_size=10)
teleop = GripperTeleop(arm, gripper, im_server)
teleop.start()
pp_im_server = InteractiveMarkerServer('pick_place_im_server', q_size=10)
pick_place = PickPlaceTeleop(arm, gripper, pp_im_server)
pick_place.start()
rospy.spin()
def __init__(self):
self._grip = fetch_api.Gripper()
self._base = fetch_api.Base()
self._arm = fetch_api.Arm()
self._torso = fetch_api.Torso()
self._torso.set_height(0.4)
self._head = fetch_api.Head()
self._head.pan_tilt(0, 0.0)
self.actions = None
try:
self.actions = pickle.load(open(PICKLE_FILE, "rb"))
print '{} loaded.'.format(PICKLE_FILE)
except:
print '{} could not be loaded.'.format(PICKLE_FILE)
self._reader = ArTagReader()
self._sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self._reader.callback)
def __init__(self):
# set arm and gripper, and fiducial id
self.arm = fetch_api.Arm()
self.gripper = fetch_api.Gripper()
# Will need to change this to something real at some point
#self.target_id = None# target_id
self.planning_scene = PlanningSceneInterface('base_link')
# First, load the poses for the fiducial insert movement
self.sequence = pickle.load(open(INSERT_GRASP_POSES, "rb"))
self.gripper_open = True
# # Init the reader for the tags
self.reader = ArTagReader()
self.sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers,
self.reader.callback)
def main():
# Initialize the interactive marker server for the gripper
rospy.init_node('gripper_demo')
im_server = InteractiveMarkerServer('gripper_im_server', q_size=2)
auto_pick_im_server = InteractiveMarkerServer('auto_pick_im_server',
q_size=2)
down_im_server = InteractiveMarkerServer('down_gripper_im_server',
q_size=2)
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
teleop = GripperTeleop(arm, gripper, im_server)
auto_pick = AutoPickTeleop(arm, gripper, auto_pick_im_server)
down_teleop = GripperTeleopDown(arm, gripper, down_im_server)
teleop.start()
auto_pick.start()
down_teleop.start()
rospy.spin()
def main():
rospy.init_node('gripper_demo')
wait_for_time()
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
command = argv[1]
gripper = fetch_api.Gripper()
effort = gripper.MAX_EFFORT
if command == 'close' and len(argv) > 2:
effort = float(argv[2])
if command == 'open':
gripper.open()
elif command == 'close':
gripper.close(effort)
else:
print_usage()
if __name__ == '__main__':
rospy.init_node('motion_demo')
base = fetch_api.Base()
base.go_forward(1.5)
print('Move Base Forward 1.5 meters')
head = fetch_api.Head()
head.pan_tilt(0, math.pi / 4)
head.pan_tilt(0, -math.pi / 2)
print('Head looks down 45 degrees')
print('Head looks up 90 degrees')
head.pan_tilt(0, 0)
head.pan_tilt(math.pi / 2, 0)
head.pan_tilt(0, 0)
print('Head looks left 90 degrees')
head.pan_tilt(-math.pi / 2, 0)
print('Head looks right 90 degrees')
head.pan_tilt(0, 0)
print('Head looks forward')
torso = fetch_api.Torso()
torso.set_height(0.4)
print('Raise torso to the maximum')
arm = fetch_api.Arm()
arm.move_to_joints(fetch_api.ArmJoints.from_list([0, 0, 0, 0, 0, 0, 0]))
print('Move arm to joint values [0,0,0,0,0,0,0]')
gripper = fetch_api.Gripper()
gripper.close()
print('Close the gripper')
gripper.open()
print('Open the gripper')
def __init__(self):
self._torso = fetch_api.Torso()
self._head = fetch_api.Head()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
def main():
pose_actions = None
# Get the actions from the pickel file.
argv = rospy.myargv()
if len(argv) < 2:
usage()
return
fileName = argv[1]
try:
pose_actions = pickle.load(open(fileName, "rb"))
print '{} loaded.'.format(fileName)
except:
print '{} could not be loaded.'.format(fileName)
usage()
return
rospy.init_node("load_file_actions")
wait_for_time()
print 'Time has begun.'
# Start the arm.
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
controller_client = actionlib.SimpleActionClient('/query_controller_states', QueryControllerStatesAction)
rospy.sleep(1.0)
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.RUNNING
goal.updates.append(state)
controller_client.send_goal(goal)
print 'Waiting for arm to start.'
controller_client.wait_for_result()
print 'Arm has been started.'
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
print 'Arm and gripper instantiated.'
rospy.sleep(1.0)
count = 0
# Run through each of the actions
for pose_action in pose_actions:
print 'Performing action.'
if pose_action.actionType == PoseExecutable.OPEN:
print 'Opening the gripper'
gripper.open()
elif pose_action.actionType == PoseExecutable.CLOSE:
print 'Closing the gripper'
gripper.close()
elif pose_action.actionType == PoseExecutable.MOVETO:
count += 1
print 'Moving to location.'
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
if pose_action.relativeFrame == 'base_link':
pose_stamped.pose = pose_action.pose
else:
for marker in reader.markers:
if pose_action.relativeFrame == marker.id:
wrist2 = makeMatrix(pose_action.pose)
tag = makeMatrix(pose_action.arPose)
tag2 = tf.transformations.inverse_matrix(tag)
result = np.dot(tag2, wrist2)
result2 = np.dot(makeMatrix(marker.pose.pose), result)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
pose_stamped.pose = transform_to_pose(result2)
error = arm.move_to_pose(pose_stamped, allowed_planning_time=40, num_planning_attempts=20)
if fileName == PICKLE_FILE_DISPENSE and count == 2:
print 'Dispensing!'
rospy.sleep(DISPENSE_TIME)
if error is not None:
print 'Error moving to {}.'.format(pose_action.pose)
else:
print 'invalid command {}'.format(pose_action.action)
def __init__(self):
print("create ActionRunner")
# rospy.init_node('action_runner')
wait_for_time()
# get gripper
self.gripper = fetch_api.Gripper()
print("done with gripper")
self.arm = fetch_api.Arm()
print("done with arm")
self.base = fetch_api.Base()
print("done with base")
self.reader = self.ArTagReader()
self.markers = {}
# get initial position of markers... it will continue updating in background
reachable = False
rospy.sleep(0.5)
while len(self.reader.markers) == 0:
# TODO[LOW PRIORITY]: implement looking for the AR tag by tilting its head up and down
print("waiting for marker")
self.base.turn(0.6)
print("im about to sleep")
rospy.sleep(1.5)
print("i had a nap")
print("found my marker!")
markers = self.reader.markers
print("Original marker pose is " + str(markers[0]))
debug_marker_pose = PoseStamped(pose=markers[0].pose.pose)
debug_marker_pose.header.stamp = rospy.Time.now()
debug_marker_pose.header.frame_id = "/base_link"
draw_debug_marker(debug_marker_pose, [0, 1, 0, 0.5])
original_pose_stamped = dot_poses(
lookup_transform("/odom", "/base_link"), markers[0].pose.pose)
print("Adjusting orientation")
number_of_turns = 0
while not reachable:
if number_of_turns > 5:
print("Turned enough, I guess...")
break
while len(self.reader.markers) == 0:
pass
# m = markers[0]
m = deepcopy(self.reader.markers[0])
pose_stamped = PoseStamped(pose=m.pose.pose)
pose_stamped.header.frame_id = "/base_link"
pose_stamped.header.stamp = rospy.Time.now()
turn = compute_turn(deepcopy(pose_stamped.pose))
print("\tComputed turn:" + str(turn))
if abs(turn) > 0.07:
self.base.turn(turn)
number_of_turns += 1
rospy.sleep(1.5)
print("\tExecuted turn")
# markers = self.reader.markers
if abs(compute_turn(m.pose.pose)) <= MIN_TURN_RADS:
print("\tComputed turn was less than " + str(MIN_TURN_RADS) +
": " + str(abs(compute_turn(m.pose.pose))))
print("...good enough")
break
print("Re-Orientation complete!")
print("Adjusting forward position...")
if self.arm.compute_ik(pose_stamped):
print("\tMarker is already reachable")
reachable = True
else:
reachable = False
print("\tMarker is too far away from the robot")
computed_forward = compute_dist(pose_stamped.pose) - 1
print("\tComputed forward distance is " + str(computed_forward))
self.base.go_forward(computed_forward)
print("\tMoving forward by " + str(computed_forward))
print("Moved to location")
# move gripper
#target_pose = PoseStamped(pose=original_pose_stamped.pose)
#target_pose = PoseStamped(pose=markers[0].pose.pose)
original_pose_stamped.pose.position.x -= 0.25
original_pose_stamped.pose.orientation.x = 0
original_pose_stamped.pose.orientation.y = 0.7
original_pose_stamped.pose.orientation.z = 0
original_pose_stamped.pose.orientation.w = -0.7
target_pose = dot_poses(lookup_transform("/base_link", "/odom"),
original_pose_stamped.pose)
target_pose.header.frame_id = "/base_link"
target_pose.header.stamp = rospy.Time.now()
print("Marker pose after moving is " + str(target_pose))
draw_debug_marker(target_pose, [0, 0, 1, 0.5])
if self.arm.compute_ik(target_pose):
self.success = True
print("reachable")
# FINISHED: the gripper moves significantly slower by changing MoveGroup's
# MotionPlanRequest's max_velocity_scaling factor.
print("move_to_pose result: " +
str(self.arm.move_to_pose(target_pose)))
else:
print("not reachable :(")
self.success = False
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
argv = rospy.myargv()
# Create the arm and safety measures
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
return
rospy.on_shutdown(shutdown)
planning_scene = PlanningSceneInterface(frame='base_link')
planning_scene.clear()
# add table to the scene
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.4
planning_scene.addBox('table', table_size_x, table_size_y, table_size_z,
table_x, table_y, table_z)
# Create divider obstacle
planning_scene.removeCollisionObject('divider')
size_x = 0.3
size_y = 0.01
size_z = 0.4
x = table_x - (table_size_x / 2) + (size_x / 2)
y = 0
z = table_z + (table_size_z / 2) + (size_z / 2)
planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.5
pose1.pose.position.y = -0.3
pose1.pose.position.z = 0.75
pose1.pose.orientation.w = 1
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.5
pose2.pose.position.y = 0.3
pose2.pose.position.z = 0.75
pose2.pose.orientation.w = 1
oc = OrientationConstraint()
oc.header.frame_id = 'base_link'
oc.link_name = 'wrist_roll_link'
oc.orientation.w = 1
oc.absolute_x_axis_tolerance = 0.1
oc.absolute_y_axis_tolerance = 0.1
oc.absolute_z_axis_tolerance = 3.14
oc.weight = 1.0
kwargs1 = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False,
}
kwargs2 = {
'allowed_planning_time': 15,
'execution_timeout': 10,
'num_planning_attempts': 5,
'replan': False,
'orientation_constraint': oc
}
gripper = fetch_api.Gripper()
planning_scene.removeAttachedObject('tray')
while True:
# Before moving to the first pose
error = arm.move_to_pose(pose1, **kwargs1)
if error is not None:
rospy.logerr('Pose 1 failed: {}'.format(error))
else:
rospy.loginfo('Pose 1 succeeded')
frame_attached_to = 'gripper_link'
frames_okay_to_collide_with = [
'gripper_link', 'l_gripper_finger_link',
'r_gripper_finger_link'
]
planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0,
frame_attached_to,
frames_okay_to_collide_with)
planning_scene.setColor('tray', 1, 0, 1)
planning_scene.sendColors()
rospy.sleep(1)
error = arm.move_to_pose(pose2, **kwargs2)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeAttachedObject('tray')
def main():
rospy.init_node('coke_can_node')
wait_for_time()
# controls of Fetch
##### TODO: Uncomment these if nessecary
# arm = fetch_api.Arm()
# arm_joints = fetch_api.ArmJoints()
# torso = fetch_api.Torso()
head = fetch_api.Head()
fetch_gripper = fetch_api.Gripper()
move_base_client = actionlib.SimpleActionClient('move_base',
MoveBaseAction)
move_base_client.wait_for_server()
rospy.sleep(0.5)
########## TODO: Uncomment this
# shutdown handler
# def shutdown():
# arm.cancel_all_goals()
# rospy.on_shutdown(shutdown)
# move base to the position near coke can
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 1.51860149414
goal.target_pose.pose.position.y = -4.3 # -3.90194324852
goal.target_pose.pose.orientation.z = -0.55
goal.target_pose.pose.orientation.w = 0.84
move_base_client.send_goal(goal)
wait = move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# move_base_client.get_result()
print "Ready to pick up the coke can!"
# move head
head.pan_tilt(0, 0.7)
print "Head moved!"
##### TODO: write code for moving the arm to pick up the coke can
# pose = PoseStamped()
# pose.header.frame_id = 'base_link'
# pose.pose.position.x = 0.8
# pose.pose.position.y = 0
# pose.pose.position.z = 0.75
# pose.pose.orientation.w = 1
# kwargs = {
# 'allowed_planning_time': 100,
# 'execution_timeout': 100,
# 'num_planning_attempts': 1,
# 'replan_attempts': 5,
# 'replan': True,
# 'orientation_constraint': None
# }
# error = arm.move_to_pose(pose, **kwargs)
# if error is not None:
# rospy.logerr('Pose failed: {}'.format(error))
# else:
# rospy.loginfo('Pose succeeded')
# print "Arm moved!"
# move base to the position near the shelf
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 4.50320185696
goal.target_pose.pose.position.y = -4.2
goal.target_pose.pose.orientation.z = -0.554336505677
goal.target_pose.pose.orientation.w = 0.832292639926
move_base_client.send_goal(goal)
wait = move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# move_base_client.get_result()
print "Ready to place the coke can!"
def main():
rospy.init_node('create_pbd_action')
wait_for_time()
# Check to see the proper args were given
argv = rospy.myargv()
if len(argv) < 2:
print_usage()
return
save_file_name = argv[1]
# This is the list of poses that we will save
sequence = []
# The Arm and the gripper on the robot
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
gripper_open = True
# Init a a tfListener for reading the gripper pose
listener = tf.TransformListener()
rospy.sleep(rospy.Duration.from_sec(1))
# Init the reader for the tags
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
# Step 1: Relax the arm
if not IN_SIM:
controller_client = actionlib.SimpleActionClient(
'/query_controller_states', QueryControllerStatesAction)
# Sleep for a second to make sure the client starts up
rospy.sleep(1.0)
# The rest of this code is given in the lab
goal = QueryControllerStatesGoal()
state = ControllerState()
state.name = 'arm_controller/follow_joint_trajectory'
state.state = ControllerState.STOPPED
goal.updates.append(state)
controller_client.send_goal(goal)
controller_client.wait_for_result()
# Get the user interface going
print_user_options()
running = True
while running:
user_input = raw_input("")
if user_input == "save_pose":
# Get the current pose with respect to the base_link
current_gripper_pose = get_current_gripper_pose(listener)
# Ask the user which frame they would like to save the pose to
# Eg: base_link, tag 1, tag 2
print "Please input the frame you would like to save the pose in. The options are:"
print " base_link"
for marker in reader.markers:
print " %s" % marker.id
frame = raw_input("")
# Check the frame the pose should be saved in
pose = Pose()
frame_id = frame
if frame == "base_link":
pose = current_gripper_pose
frame_id = 'base_link'
else:
for marker in reader.markers:
if frame == str(marker.id):
# First, we need to turn the poses into transform matrixes
pose_in_base_matrix = pose_to_transform(
current_gripper_pose)
tag_in_base_matrix = pose_to_transform(
marker.pose.pose)
# Then take the inverse of the tag in the base fram
inv_matrix = np.linalg.inv(tag_in_base_matrix)
# Then take the dot product
pose_to_save = np.dot(inv_matrix, pose_in_base_matrix)
# The target pose is then transformed back into a pose object
pose = transform_to_pose(pose_to_save)
frame_id = str(marker.id)
# Create and append the PBD_Pose
pbd_pose = PBD_Pose(pose, gripper_open, frame)
sequence.append(pbd_pose)
if user_input == "open_gripper":
gripper.open()
gripper_open = True
if user_input == "close_gripper":
gripper.close()
gripper_open = False
if user_input == "save_program":
pickle.dump(sequence, open(save_file_name, "wb"))
if user_input == "help":
print_user_options()
if user_input == "quit":
running = False
def main():
rospy.init_node('pour_scene')
wait_for_time()
target_name = 'cup1'
x_pose = 0.329202820349
y_pose = -0.01
z_pose = 0.060
x_ori, y_ori, z_ori, w_ori = cvt_e2q(0, 0, 0)
x, y, z, _ = getState('cup2')
head = fetch_api.Head()
arm = fetch_api.Arm()
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
head.look_at('base_link', x, y, z)
sess = tensorflow.Session()
model = load_model(sess)
x, y, z, _ = getState(target_name)
# observation test
get_observation()
move_arm_to(arm, x_pose, y + y_pose, z + z_pose, x_ori, y_ori, z_ori,
w_ori)
x, y, z, _ = getState('table1')
base = fetch_api.Base()
if x > 1:
base.go_forward(0.6, speed=0.2)
# collision in motion planning
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
length_table = 1
width_table = 1
height_table = 0.04
x_table, y_table, z_table, _ = getState('table1')
z_table = z + 0.7
planning_scene.addBox('table', length_table, width_table, height_table,
x_table, y_table, z_table)
length_box = 0.05
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup1')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_1', length_box, width_box, height_box, x_box,
y_box, z_box)
length_box = 0.03
width_box = 0.05
height_box = 0.2
x, y, z, _ = getState('cup2')
x_box = x
y_box = y
z_box = z
planning_scene.addBox('mug_2', length_box, width_box, height_box, x_box,
y_box, z_box)
# the initial position of gripper is (-0.5, 0, 0), and
# the final position of gripper is (-0.5+x_pose, y_pose, z_pose).
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose, x_ori, y_ori,
z_ori, w_ori)
planning_scene.removeCollisionObject('mug_1')
# planning_scene.removeCollisionObject('mug_2')
# planning_scene.removeCollisionObject('mug')
# planning_scene.removeCollisionObject('table')
gripper = fetch_api.Gripper()
effort = gripper.MAX_EFFORT
gripper.close(effort)
x, y, z, _ = getState(target_name)
move_arm_to(arm, x - 0.5 + x_pose, y + y_pose, z + z_pose + 0.06, x_ori,
y_ori, z_ori, w_ori)
x, y, z, _ = getState('cup2')
head.look_at('base_link', x, y, z)
for _ in xrange(50):
obs = get_observation()
act = model.choose_action(obs)
print('obs: {}, action: {}'.format(obs, act))
execute_action(act, arm)
time.sleep(0.5)
def main():
rospy.init_node("book_grasp_procedure")
wait_for_time()
# First, load the poses for the fiducial insert movement
sequence = pickle.load( open(INSERT_GRASP_POSES, "rb") )
gripper = fetch_api.Gripper()
arm = fetch_api.Arm()
gripper_open = True
# # Init the reader for the tags
reader = ArTagReader()
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
rospy.sleep(0.5)
# Init the planning scene for collisions
planning_scene = PlanningSceneInterface('base_link')
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses()
planning_scene.addBox('surface', (response.surface_x_size - 0.17), (response.surface_y_size), response.surface_z_size,
response.surface_pose.position.x, response.surface_pose.position.y, response.surface_pose.position.z)
gripper.open()
gripper_open = True
target_marker_pose = None
check = 0
print "Searching for fiducial...."
while target_marker_pose == None and check < 100:
# If the fiducial was not seen, try again
rospy.sleep(0.1)
check += 1
for marker in reader.markers:
if TARGET_ID == marker.id:
target_marker_pose = marker.pose.pose
if target_marker_pose == None:
print "Failed to find fiducial"
print "Surface position z"
print response.surface_pose.position.z
print "target marker position z"
print target_marker_pose.position.z
# This is the same as the pbd action stuff, not making any changes at the moment
for pbd_pose in sequence:
move_pose = PoseStamped()
move_pose.header.frame_id = 'base_link'
if pbd_pose.frame == 'base_link':
move_pose.pose = pbd_pose.pose
else:
# for marker in reader.markers:
# if TARGET_ID == marker.id:
print "Calculating pose relative to marker...."
# Transform the pose to be in the base_link frame
pose_in_tag_frame = pose_to_transform(pbd_pose.pose)
#tag_in_base_frame = pose_to_transform(marker.pose.pose)
tag_in_base_frame = pose_to_transform(target_marker_pose)
target_matrix = np.dot(tag_in_base_frame, pose_in_tag_frame)
target_pose = transform_to_pose(target_matrix)
move_pose.pose = target_pose
rospy.sleep(1)
err = arm.move_to_pose(move_pose, replan=True)
print "Error in move to pose: ", err
# Check the gripper to open/close
if pbd_pose.gripper_open != gripper_open:
if gripper_open == True:
gripper.close()
gripper_open = False
else:
gripper.open()
gripper_open = True
print "Take this opportunity to change to a different mock point cloud, if necessary"
user_input = raw_input("Press enter to continue")
# After this we calculate the spine_pose closest to the fiducial. I will test that if I can get the service call working
target_fiducial = None
check = 0
print "Searching for fiducial"
while target_fiducial == None and check < 100:
# If the fiducial was not seen, try again
rospy.sleep(0.1)
check += 1
for marker in reader.markers:
if marker.id == TARGET_ID:
target_fiducial = marker
if target_fiducial == None:
print "Failed to find fiducial"
print target_fiducial.id
# Now, we make a request to the perception side of things
spine_poses = []
# code for checking
check = 0
found_good_pose = False
closest_pose = None
print "waiting for service...."
rospy.wait_for_service('get_spines')
print "found service!"
print "Searching for a good grasp pose...."
try:
while check < 20 and found_good_pose == False:
get_spine_poses = rospy.ServiceProxy('get_spines', GetSpineLocations)
response = get_spine_poses()
spine_poses = response.spine_poses
min_dist = float('inf')
for pose in spine_poses:
distance = calculate_euclidean_distance(pose, target_fiducial.pose.pose)
if distance < min_dist:
min_dist = distance
closest_pose = pose
check += 1
if closest_pose.position.x > target_fiducial.pose.pose.position.x and closest_pose.position.y < (target_fiducial.pose.pose.position.y + 0.025) and closest_pose.position.y > (target_fiducial.pose.pose.position.y - 0.025):
found_good_pose = True
# debugging line
# for pose in spine_poses:
# print pose
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def handle_grab_box(req):
"""
Given a req, grabs the shoe box at the shoe shelf
"""
global markers
global grab_poses
global viz_pub
global robot_pose
global planning
# Attach the floor before doing any movements
attach_floor()
arm = fetch_api.Arm()
gripper = fetch_api.Gripper()
torso = fetch_api.Torso()
head = fetch_api.Head()
base = fetch_api.Base()
# Prepare for grabbing the box
print("Preparing to Grab Box, setting height + arm")
# Move the arm, gripper, toros and head to initial position
gripper.open()
beginHeight = 0.1
# Navigate the arm to prepare
move_pose(arm, PREPARE)
# Move torso higher if on top shelf:
if req.ar_id == 6 or req.ar_id == 4:
beginHeight = 0.2
torso.set_height(beginHeight)
head.pan_tilt(*SHELF_HEAD_POSE)
rospy.sleep(3.0)
# Wait 5 seconds until markers are updated
target_marker = None
for i in range(5):
print("Waiting for the markers... {}/3".format(i))
if any(markers):
print([marker.id for marker in markers])
target_marker = filter(lambda x: x.id == req.ar_id, markers)
if any(target_marker):
break
if i == 4:
print("Didn't find any marker")
move_dist(0.1, -0.1)
torso.set_height(torso.MAX_HEIGHT)
move_pose(arm, CARRY)
rospy.sleep(1.0)
return 1
rospy.sleep(1.0)
print[marker.id for marker in markers]
print("Target: {}".format(req.ar_id))
if not any(target_marker):
print("Didn't find the target marker")
markers = []
move_dist(0.1, -0.1)
torso.set_height(torso.MAX_HEIGHT)
move_pose(arm, CARRY)
rospy.sleep(1.0)
return 1
# Navigate the gripper
print("Navigating arm to the target box")
for action in grab_poses:
if action[0] == 'MOVE':
arPose = action[1]
wristPose = action[2]
# Compute the pose of the wrist in base_link
ar = fetch_api.pose2matrix(arPose)
ar2wrist = fetch_api.pose2transform(arPose, wristPose, True)
wrist = np.dot(fetch_api.pose2matrix(target_marker[0].pose.pose),
ar2wrist)
print(target_marker[0].pose.header.frame_id)
# Navigate the arm there
kwargs = {
'allowed_planning_time': 50,
'execution_timeout': 40,
'num_planning_attempts': 30,
'replan': False,
}
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = "base_link"
pose_stamped.pose = fetch_api.matrix2pose(wrist)
mp_marker = Marker()
mp_marker.header.frame_id = "base_link"
mp_marker.header.stamp = rospy.Time()
mp_marker.id = 0
mp_marker.type = Marker.ARROW
mp_marker.action = Marker.ADD
mp_marker.ns = 'motion_plan_goal'
mp_marker.pose.position.x = pose_stamped.pose.position.x
mp_marker.pose.position.y = pose_stamped.pose.position.y
mp_marker.pose.position.z = pose_stamped.pose.position.z
mp_marker.pose.orientation.x = pose_stamped.pose.orientation.x
mp_marker.pose.orientation.y = pose_stamped.pose.orientation.y
mp_marker.pose.orientation.z = pose_stamped.pose.orientation.z
mp_marker.pose.orientation.w = pose_stamped.pose.orientation.w
mp_marker.scale.x = 0.2
mp_marker.scale.y = 0.05
mp_marker.scale.z = 0.05
mp_marker.color.a = 1.0
mp_marker.color.r = 1.0
mp_marker.color.g = 0.0
mp_marker.color.b = 0.0
viz_pub.publish(mp_marker)
error = arm.move_to_pose(pose_stamped, **kwargs)
if error is None:
rospy.loginfo('Moved to the target marker')
else:
rospy.logwarn('Failed to move to the target marker')
move_dist(0.1, -0.1)
torso.set_height(torso.MAX_HEIGHT)
move_pose(arm, CARRY)
return 1
rospy.sleep(1.0)
elif action[0] == 'OPEN':
gripper.open()
elif action[0] == 'CLOSE':
gripper.close()
gripper.close()
print("Grabbed the box, moving torso up")
torso.set_height(beginHeight + 0.1)
# Back up the base, set torso back down and head back up
head.pan_tilt(*MOVING_HEAD_POSE)
move_dist(0.19, -0.1)
torso.set_height(torso.MAX_HEIGHT)
# Set arm to initial position
move_pose(arm, CARRY)
# Empty markers for the next call
markers = []
return 0
def __init__(self):
self._grip = fetch_api.Gripper()
self._arm = fetch_api.Arm()
pass
