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 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 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 __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():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('straight_gripper_motion_demo')
wait_for_time()
moveit_robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander('arm')
def on_shutdown():
group.stop()
moveit_commander.roscpp_shutdown()
rospy.on_shutdown(on_shutdown)
# Set the torso height before running this demo.
#torso = fetch_api.Torso()
#torso.set_height(0.4)
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.502
pose1.pose.position.y = -0.510
pose1.pose.position.z = 1.084
pose1.pose.orientation.x = -0.498
pose1.pose.orientation.y = 0.508
pose1.pose.orientation.z = 0.475
pose1.pose.orientation.w = 0.519
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.502
pose2.pose.position.y = 0.510
pose2.pose.position.z = 1.084
pose2.pose.orientation.x = 0.522
pose2.pose.orientation.y = -0.484
pose2.pose.orientation.z = -0.499
pose2.pose.orientation.w = -0.494
gripper_poses = [pose1, pose2]
arm = fetch_api.Arm()
while not rospy.is_shutdown():
for pose_stamped in gripper_poses:
error = arm.straight_move_to_pose(group,
pose_stamped,
jump_threshold=2.0)
if error is not None:
rospy.logerr(error)
rospy.sleep(0.25)
moveit_commander.roscpp_shutdown()
def main():
rospy.init_node('access_gripper_teleop')
wait_for_time()
arm = fetch_api.Arm()
move_group = MoveGroupCommander("arm")
status_publisher = rospy.Publisher('/access_teleop/arm_status',
String,
queue_size=1)
gripper_publisher = rospy.Publisher('/access_teleop/gripper_pixels',
PX,
queue_size=1)
info_pubs = []
for camera_name in camera_names:
info_pubs.append([
camera_name,
rospy.Publisher(camera_name + '/camera_info',
camera_info_messages.CameraInfo,
queue_size=10)
])
tb = TransformBroadcaster()
camera_model = PinholeCameraModel()
move_by_delta = MoveByDelta(arm, move_group)
move_by_delta.start()
move_by_absolute = MoveByAbsolute(arm, move_group, status_publisher)
move_by_absolute.start()
move_and_orient = MoveAndOrient(arm, move_group)
move_and_orient.start()
orient = Orient(arm, move_group)
orient.start()
wrist_roll = WristRoll(arm, move_group)
wrist_roll.start()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
publish_camera_transforms(tb)
publish_camera_info(info_pubs)
publish_gripper_pixels(camera_model, move_group, gripper_publisher)
rate.sleep()
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 main():
rospy.init_node('arm_demo')
wait_for_time()
argv = rospy.myargv()
DISCO_POSES = [[
-1.605528329547318, 1.41720603380179, 2.018610841968549,
1.5522558117738399, -1.5635699410855368, 0.7653977094751401,
-1.3914909133500242
]]
#torso = fetch_api.Torso()
#torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
for vals in DISCO_POSES:
arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
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():
rospy.init_node('hallucinated_demo')
wait_for_time()
argv = rospy.myargv()
torso = fetch_api.Torso()
torso.set_height(0.4)
start = PoseStamped()
start.header.frame_id = 'base_link'
start.pose = Pose(orientation=Quaternion(0, 0, 0, 1))
start.pose.position.x = 0.5
start.pose.position.y = 0.5
start.pose.position.z = 0.75
arm = fetch_api.Arm()
arm.move_to_pose(start)
success = []
reader = ArTagReader()
sub = rospy.Subscriber(
'/ar_pose_marker', AlvarMarkers,
reader.callback) # Subscribe to AR tag poses, use reader.callback
while len(reader.markers) == 0:
rospy.sleep(0.1)
for marker in reader.markers:
#TODO: get the pose to move to
#goal.pose = Pose(orientation=Quaternion(0,0,0,1))
moveTo = PoseStamped()
moveTo.header.frame_id = 'base_link'
moveTo.pose.position = marker.pose.pose.position
moveTo.pose.position.x -= GRIPPER_OFFSET
moveTo.pose.orientation = Quaternion(0, 0, 0, 1)
error = arm.move_to_pose(moveTo, allowed_planning_time=30.0)
if error is None:
rospy.loginfo('Moved to marker {}'.format(marker.id))
success.append(marker.id)
else:
rospy.logwarn('Failed to move to marker {}'.format(marker.id))
if len(success) > 1:
rospy.loginfo('Moved to markers listed above')
else:
rospy.logerr('Failed to move to any markers!')
def main():
rospy.init_node('arm_demo')
wait_for_time()
argv = rospy.myargv()
DISCO_POSES = [[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0],
[0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[-1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0],
[-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
[0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
for vals in DISCO_POSES:
arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
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('cart_arm_demo')
wait_for_time()
argv = rospy.myargv()
pose1 = PoseStamped()
pose1.header.frame_id = 'base_link'
pose1.pose.position.x = 0.042
pose1.pose.position.y = 0.384
pose1.pose.position.z = 1.826
pose1.pose.orientation.x = 0.173
pose1.pose.orientation.y = -0.693
pose1.pose.orientation.z = -0.242
pose1.pose.orientation.w = 0.657
pose2 = PoseStamped()
pose2.header.frame_id = 'base_link'
pose2.pose.position.x = 0.047
pose2.pose.position.y = 0.545
pose2.pose.position.z = 1.822
pose2.pose.orientation.x = -0.274
pose2.pose.orientation.y = -0.701
pose2.pose.orientation.z = 0.173
pose2.pose.orientation.w = 0.635
gripper_poses = [pose1, pose2]
arm = fetch_api.Arm()
def on_shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(on_shutdown)
while not rospy.is_shutdown():
for pose_stamped in gripper_poses:
error = arm.move_to_pose(pose_stamped,
replan=True,
execution_timeout=2)
if error is not None:
rospy.logerr(error)
rospy.sleep(1)
def main():
rospy.init_node('cart_arm_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)
header = Header()
header.frame_id = 'base_link'
# Create desired poses
pose1 = Pose(position=Point(0.042, 0.384, 1.826),
orientation=Quaternion(0.173, -0.693, -0.242, 0.657))
pose2 = Pose(position=Point(0.047, 0.545, 1.822),
orientation=Quaternion(-0.274, -0.701, 0.173, 0.635))
pose1_st = PoseStamped(header, pose1)
pose2_st = PoseStamped(header, pose2)
gripper_poses = [pose1_st, pose2_st]
while True:
# Move the arm to each position!
for pose in gripper_poses:
error = arm.move_to_pose(pose)
# Deal with potential errors
if error is not None:
rospy.logerr(error)
rospy.sleep(1)
def main():
rospy.init_node('limb_ar_demo')
wait_for_time()
start = PoseStamped()
start.header.frame_id = 'base_link'
start.pose.position.x = 0.5
start.pose.position.y = 0.5
start.pose.position.z = 0.75
arm = fetch_api.Arm()
arm.move_to_pose(start)
reader = ArTagReader()
sub = rospy.Subscriber('ar_pose_marker',
AlvarMarkers,
callback=reader.callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
while len(reader.markers) == 0:
rospy.sleep(0.1)
# for marker in reader.markers:
# pose = marker.pose
# pose.header.frame_id = marker.header.frame_id
# error = arm.move_to_pose(pose)
# if error is None:
# rospy.loginfo('Moved to marker {}'.format(marker.id))
# return
# else:
# rospy.logwarn('Failed to move to marker {}'.format(marker.id))
# rospy.logerr('Failed to move to any markers!')
for marker in reader.markers:
pose = marker.pose
pose.header.frame_id = marker.header.frame_id
print(pose)
rate.sleep()
def main():
rospy.init_node('arm_demo')
wait_for_time()
# moveit_commander.roscpp_initialize(sys.argv)
# moveit_robot = moveit_commander.RobotCommander()
# moveit_group = moveit_commander.MoveGroupCommander('arm')
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
# DISCO_POSES = [[1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0],
# [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0]]
# # [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
# # [-1.5, 1.1, -3.0, -0.5, -3.0, -1.0, -3.0],
# # [-0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
# # [0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
# # [1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
# for vals in DISCO_POSES:
# arm.move_to_joints(fetch_api.ArmJoints.from_list(vals))
goal = [('shoulder_pan_joint', 1.5), ('shoulder_lift_joint', -0.6),
('upperarm_roll_joint', 3.0), ('elbow_flex_joint', 1.0),
('forearm_roll_joint', 3.0), ('wrist_flex_joint', 1.0),
('wrist_roll_joint', 3.0)]
error = arm.move_to_joint_goal(goal,
allowed_planning_time=120.0,
execution_timeout=rospy.Duration(60.0),
num_planning_attempts=5,
replan=True,
replan_attempts=5)
if error is not None:
arm.cancel_all_goals()
rospy.logerr('Failed: {}'.format(error))
else:
rospy.loginfo('Succeeded')
def create_gripper_marker(x, z, pose):
# Create marker for gripper base
base_marker = Marker()
base_marker.type = Marker.MESH_RESOURCE
base_marker.mesh_resource = GRIPPER_MESH
base_marker.pose.position.x += x + GRIPPER_X_OFFSET
base_marker.pose.position.z += z
arm = fetch_api.Arm()
if arm.compute_ik(create_pose_stamped(pose)):
base_marker.color = GREEN
else:
base_marker.color = RED
# Create marker for left gripper prong
left_marker = copy.deepcopy(base_marker)
left_marker.mesh_resource = L_FINGER_MESH
left_marker.scale.y = FINGER_Y_SCALE
# Create marker for right gripper prong
right_marker = copy.deepcopy(left_marker)
right_marker.mesh_resource = R_FINGER_MESH
return [base_marker, left_marker, right_marker]
def main():
rospy.init_node('hallucinate_demo')
wait_for_time()
start = PoseStamped()
start.header.frame_id = 'base_link'
start.pose.position.x = 0.5
start.pose.position.y = 0.5
start.pose.position.z = 0.75
arm = fetch_api.Arm()
arm.move_to_pose(start)
reader = ArTagReader()
# Subscribe to AR tag poses, use reader.callback
sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, reader.callback)
while len(reader.markers) == 0:
print 'sleep'
rospy.sleep(0.1)
successes = []
for marker in reader.markers:
print marker
ps = PoseStamped()
ps.pose.position = marker.pose.pose.position
ps.header.frame_id = 'base_link'
ps.pose.orientation = Quaternion(0, 0, 0, 1)
ps.pose.position.x -= 0.3
error = arm.move_to_pose(ps, allowed_planning_time=30)
if error is None:
rospy.loginfo('Moved to marker {}'.format(marker.id))
successes.append(marker.id)
else:
rospy.logwarn('Failed to move to marker {}'.format(marker.id))
if len(successes) > 0:
rospy.loginfo('Moved to markers {}!'.format(successes))
else:
rospy.logerr('Fail to move to any marker!')
def main():
rospy.init_node('gripper_teleop')
wait_for_time()
# Added by Xinyi:
# Initialize Settings for the Test (Part 1)
# Raise the torso to allow arm movement
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
# Set arm joints
# Order: body ---> gripper
# b: blue joint
# g: gray joint
# order: [b, g, b, g, b, g, b]
# OPTION 1: Follow given trajectory
arm = fetch_api.Arm()
arm_initial_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0]
arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_initial_poses))
# for recording bag file
# arm_viz_poses = [1.0, 1.25, 1.0, -2.25, 2.25, 2.25, 0.0]
# arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_viz_poses))
# OPTION 2: Use motion planning
# INITIAL_POSES = [
# ("shoulder_pan_joint", 1.0), ("shoulder_lift_joint", 1.2), ("upperarm_roll_joint", 1.0), ("elbow_flex_joint", -2.0),
# ("forearm_roll_joint", -0.3), ("wrist_flex_joint", 1.2), ("wrist_roll_joint", 0.0)]
# arm.move_to_joint_goal(INITIAL_POSES, replan=True)
# INITIAL_POSES = PoseStamped()
# INITIAL_POSES.header.frame_id = 'base_link'
# INITIAL_POSES.pose.position.x = 0.6
# INITIAL_POSES.pose.position.y = -0.1
# INITIAL_POSES.pose.position.z = 0.3
# INITIAL_POSES.pose.orientation.w = 1
# error = arm.move_to_pose(INITIAL_POSES, replan=True)
# if error is not None:
# arm.cancel_all_goals()
# rospy.logerr('FAIL TO MOVE TO INITIAL_POSES {}'.format(error))
# else:
# rospy.loginfo('MOVED TO INITIAL_POSES')
# Set the base position
base = fetch_api.Base()
# OPTION 1: Start from origin
# move Fetch from the origin to the table
# base.go_forward(1.6, 0.5) # value (distance in meters)
# base.turn(math.pi / 3) # value (angle in degrees)
# base.go_forward(3.3, 0.5) # value (distance in meters)
# base.turn(-math.pi / 3)
# OPTION 2: Start right in front of the table
base.align_with_x_axis_pos()
base.go_forward(0.3, 0.5)
# Avoid occlusion
# OPTION 1: Freeze point cloud
# freeze_pub = rospy.Publisher('/access_teleop/freeze_cloud', Bool, queue_size=5)
# rospy.sleep(0.5)
# publish freeze point cloud
# freeze_pub.publish(Bool(data=True))
# OPTION 2: Record a bag file of the surrounding
head = fetch_api.Head()
# tilt the head from -0.35 to 0.85
# for i in range(6):
# head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], -0.35 + i * 0.2)
# os.system("rosrun perception save_cloud world $(rospack find access_teleop)/bags/")
# rospy.set_param("bag_file_refreshed", "true")
# (end)
move_group = MoveGroupCommander("arm")
status_publisher = rospy.Publisher('/access_teleop/arm_status', String, queue_size=1)
gripper_publisher = rospy.Publisher('/access_teleop/gripper_pixels', PX, queue_size=1)
info_pubs = []
for camera_name in camera_names:
info_pubs.append([camera_name,
rospy.Publisher(camera_name + '/camera_info', camera_info_messages.CameraInfo, queue_size=1)])
# Added by Xinyi
# Debug: visualize camera positions
vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=5)
rospy.sleep(0.5)
# (end)
tb = TransformBroadcaster()
camera_model = PinholeCameraModel()
move_by_delta = MoveByDelta(arm, move_group)
move_by_delta.start()
move_by_absolute = MoveByAbsolute(arm, move_group, status_publisher)
move_by_absolute.start()
move_and_orient = MoveAndOrient(arm, move_group)
move_and_orient.start()
orient = Orient(arm, move_group)
orient.start()
# Added by Xinyi
wrist_roll = WristRoll(arm, move_group)
wrist_roll.start()
base_switch_task = BaseSwitchTask(base)
base_switch_task.start()
head_tilt = HeadTilt(head)
head_tilt.start()
# Initialize Settings for the Test (Part 2)
# Add the first test object to Gazebo
os.system("$(rospack find access_teleop)/scripts/switch_object.sh " + "NONE" + " " + str(MODELS[0]))
# Move arm joints to positions for test
arm_test_poses = [1.0, 1.25, 1.0, -2.25, -0.3, 1.0, 0.0]
arm.move_to_joints(fetch_api.ArmJoints.from_list(arm_test_poses))
# Adjust the head to look at the table
# OPTION 1: Tilt by angle
# head.pan_tilt(0, math.pi / 2)
# OPTION 2: Look at a point in space
head.look_at("base_link", HEAD_POSE[0], HEAD_POSE[1], HEAD_POSE[2])
rospy.sleep(0.5)
# (end)
rate = rospy.Rate(200)
while not rospy.is_shutdown():
publish_camera_transforms(tb, vis_pub)
publish_camera_info(info_pubs)
publish_gripper_pixels(camera_model, move_group, gripper_publisher)
# publish freeze point cloud
# freeze_pub.publish(Bool(data=True))
# # get the current model position and publish a marker of current model position
# model_state = rospy.ServiceProxy('gazebo/get_model_state', GetModelState)
# model_pos = model_state(MODELS[current_model_idx], 'base_link')
# if model_pos.success:
# marker = Marker(
# type=Marker.SPHERE,
# id=current_model_idx,
# pose=Pose(Point(model_pos.pose.position.x, model_pos.pose.position.y, model_pos.pose.position.z),
# Quaternion(model_pos.pose.orientation.x, model_pos.pose.orientation.y, model_pos.pose.orientation.z, model_pos.pose.orientation.w)),
# scale=Vector3(0.05, 0.05, 0.05),
# header=Header(frame_id='base_link'),
# color=ColorRGBA(1.0, 0.5, 1.0, 0.5))
# vis_pub.publish(marker)
rate.sleep()
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 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('arm_obstacle_demo')
wait_for_time()
planning_scene = PlanningSceneInterface('base_link')
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
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
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
# and add an orientation constraint to pose 2:
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
kwargs = {
'allowed_planning_time': 100,
'execution_timeout': 100,
'num_planning_attempts': 15,
'replan': True,
'replan_attempts': 10
}
# Before moving to the first pose
planning_scene.removeAttachedObject('tray')
error = arm.move_to_pose(pose1, **kwargs)
# If the robot reaches the first pose successfully, then "attach" an object there
# Of course, you would have to close the gripper first and ensure that you grasped the object properly
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)
kwargs['orientation'] = oc
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
def main():
rospy.init_node('arm_obstacle_demo')
wait_for_time()
argv = rospy.myargv()
torso = fetch_api.Torso()
torso.set_height(fetch_api.Torso.MAX_HEIGHT)
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
planning_scene = PlanningSceneInterface('base_link')
# Create table obstacle
planning_scene.removeCollisionObject('table')
table_size_x = 0.5
table_size_y = 1
table_size_z = 0.03
table_x = 0.8
table_y = 0
table_z = 0.6
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)
planning_scene.removeAttachedObject('tray')
arm = fetch_api.Arm()
def shutdown():
arm.cancel_all_goals()
rospy.on_shutdown(shutdown)
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 40,
'num_planning_attempts': 50,
'replan': False,
}
error = arm.move_to_pose(pose1, **kwargs)
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)
#kwargs['orientation_constraint'] = oc
error = arm.move_to_pose(pose2, **kwargs)
if error is not None:
rospy.logerr('Pose 2 failed: {}'.format(error))
else:
rospy.loginfo('Pose 2 succeeded')
planning_scene.removeCollisionObject('table')
planning_scene.removeCollisionObject('divider')
# At the end of your program
planning_scene.removeAttachedObject('tray')
planning_scene.clear()
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')
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 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 __init__(self):
self._torso = fetch_api.Torso()
self._head = fetch_api.Head()
self._arm = fetch_api.Arm()
self._gripper = fetch_api.Gripper()
def __init__(self):
self._grip = fetch_api.Gripper()
self._arm = fetch_api.Arm()
pass
