def __init__(self):
self._torso = fetch_api.Torso()
self._gripper = fetch_api.Gripper()
self._head = fetch_api.Head()
self._arm = fetch_api.Arm()
self._current_pose = 0
def __init__(self):
self._torso = fetch_api.Torso()
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()
if __name__ == "__main__":
rospy.init_node("pick_demo")
dist = 0
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
planning_scene = PlanningSceneInterface('base_link')
planning_scene.clear()
planning_scene.removeCollisionObject('cube')
planning_scene.removeCollisionObject('table')
group = moveit_commander.MoveGroupCommander("arm")
planning_frame = group.get_planning_frame()
pd = pick_demo()
grasp = pd.calculate_grasp()
base = fetch_api.Base()
arm = fetch_api.Arm()
torso = fetch_api.Torso()
gripper = fetch_api.Gripper()
torso.set_height(0.4)
joint_goal = group.get_current_joint_values()
joint_goal = [0,0,0,-math.pi/2,0,math.pi/2,0]
group.go(joint_goal, wait=True)
group.stop()
get_ik = []
while get_ik == []:
base.go_forward(0.1, 0.1)
dist = dist + 0.1
target, origin, wrist, group_name, group_joint_name, base_link = pd.show_states()
final_trans = pd.calculate_transform(target, origin)
final_grasp = pd.grasp_to_body(final_trans, grasp)
final_msg_trans = pd.convert_to_message(final_grasp)
get_ik = pd.IK(final_msg_trans)
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