def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs):
use_width = world.robot_name == FRANKA_CARTER
body = world.get_body(name)
#fraction = 0.25
obj_type = type_from_name(name)
body_pose = REFERENCE_POSE.get(obj_type, unit_pose())
center, extent = approximate_as_prism(body, body_pose)
for grasp_type in grasp_types:
if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)):
continue
#assert is_valid_grasp_type(name, grasp_type)
if grasp_type == TOP_GRASP:
grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, max_width=np.inf, **kwargs)
elif grasp_type == SIDE_GRASP:
# Take max of height and something
grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little
x, z = pre_distance * get_unit_vector([3, -1])
pre_direction = [0, 0, x]
post_direction = [0, 0, z]
top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0]
# Under grasps are actually easier for this robot
# TODO: bug in under in that it grasps at the bottom
generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs)
# if world.robot_name == FRANKA_CARTER else unit_pose()
generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp)
for grasp in generator for yaw in [0, np.pi])
else:
raise ValueError(grasp_type)
grasp_poses = randomize(list(generator))
if obj_type in CYLINDERS:
# TODO: filter first
grasp_poses = (multiply(grasp_pose, Pose(euler=Euler(
yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses))
for i, grasp_pose in enumerate(grasp_poses):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose)
with BodySaver(body):
grasp.get_attachment().assign()
with BodySaver(world.robot):
grasp.grasp_width = close_until_collision(
world.robot, world.gripper_joints, bodies=[body])
#print(get_joint_positions(world.robot, world.arm_joints)[-1])
#draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link)
#grasp.get_attachment().assign()
#wait_for_user()
##for value in get_joint_limits(world.robot, world.arm_joints[-1]):
#for value in [-1.8973, 0, +1.8973]:
# set_joint_position(world.robot, world.arm_joints[-1], value)
# grasp.get_attachment().assign()
# wait_for_user()
if use_width and (grasp.grasp_width is None):
continue
yield grasp
def get_pour_feature(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_reference = get_reference_pose(bowl_name)
_, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body,
body_pose=bowl_reference)
bowl_vertices = vertices_from_rigid(bowl_body)
#bowl_mesh = read_obj(load_cup_bowl_obj(get_type(bowl_name))[0])
#print(len(bowl_vertices), len(bowl_mesh.vertices))
cup_body = world.get_body(cup_name)
cup_reference = (unit_point(), get_liquid_quat(cup_name))
_, (cup_d, _, cup_h) = approximate_as_prism(cup_body,
body_pose=cup_reference)
cup_vertices = vertices_from_rigid(cup_body)
#cup_mesh = read_obj(load_cup_bowl_obj(get_type(cup_name))[0])
# TODO: compute moments/other features from the mesh
feature = {
'bowl_name': bowl_name,
'bowl_type': get_type(bowl_name),
'bowl_diameter': bowl_d,
'bowl_height': bowl_h,
'bowl_base_diameter': compute_base_diameter(bowl_vertices),
'cup_name': cup_name,
'cup_type': get_type(cup_name),
'cup_diameter': cup_d,
'cup_height': cup_h,
'cup_base_diameter': compute_base_diameter(cup_vertices),
}
return feature
def get_scoop_feature(world, bowl_name, spoon_name):
bowl_body = world.get_body(bowl_name)
_, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
bowl_vertices = vertices_from_rigid(bowl_body)
#bowl_mesh = read_obj(load_cup_bowl_obj(get_type(bowl_name))[0])
#print(len(bowl_vertices), len(bowl_mesh.vertices))
spoon_c, (spoon_w, spoon_l, spoon_h) = approximate_as_prism(
world.get_body(spoon_name),
body_pose=(unit_point(), lookup_orientation(spoon_name, STIR_QUATS)))
# TODO: compute moments/other features from the mesh
feature = {
'bowl_name': bowl_name,
'bowl_type': get_type(bowl_name),
'bowl_diameter': bowl_d,
'bowl_height': bowl_h,
'bowl_base_diameter': compute_base_diameter(bowl_vertices),
# In stirring orientation
'spoon_name': spoon_name,
'spoon_type': get_type(spoon_name),
'spoon_width': spoon_w,
'spoon_length': spoon_l,
'spoon_height': spoon_h,
}
return feature
def get_urdf_from_z_axis(body, z_fraction, reference_quat=unit_quat()):
# AKA the pose of the body's center wrt to the body's origin
# z_fraction=0. => bottom, z_fraction=0.5 => center, z_fraction=1. => top
ref_from_urdf = (unit_point(), reference_quat)
center_in_ref, (_,
height) = approximate_as_cylinder(body,
body_pose=ref_from_urdf)
center_in_ref[2] += (z_fraction - 0.5) * height
ref_from_center = (center_in_ref, unit_quat()
) # Maps from center frame to origin frame
urdf_from_center = multiply(invert(ref_from_urdf), ref_from_center)
return urdf_from_center
def get_grasp_presses(world, knob, pre_distance=APPROACH_DISTANCE):
knob_link = link_from_name(world.kitchen, knob)
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
for i, grasp_pose in enumerate(
get_top_presses(world.kitchen,
link=knob_link,
tool_pose=TOOL_POSE,
top_offset=FINGER_EXTENT[0] / 2 + 5e-3)):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, knob, TOP_GRASP, i, grasp_pose, pregrasp_pose)
yield grasp
def pour_path_from_parameter(world, feature, parameter, cup_yaw=None):
cup_body = world.get_body(feature['cup_name'])
#cup_urdf_from_center = get_urdf_from_center(cup_body, reference_quat=get_liquid_quat(feature['cup_name']))
ref_from_urdf = (unit_point(), get_liquid_quat(feature['cup_name']))
cup_center_in_ref, _ = approximate_as_prism(cup_body,
body_pose=ref_from_urdf)
cup_center_in_ref[:
2] = 0 # Assumes the xy pour center is specified by the URDF (e.g. for spoons)
cup_urdf_from_center = multiply(invert(ref_from_urdf),
Pose(point=cup_center_in_ref))
# TODO: allow some deviation around cup_yaw for spoons
if cup_yaw is None:
cup_yaw = random.choice([0, np.pi]) if 'spoon' in feature['cup_name'] \
else random.uniform(-np.pi, np.pi)
z_rotate_cup = Pose(euler=Euler(yaw=cup_yaw))
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
base_from_pivot = Pose(
Point(x=parameter['axis_in_cup_x'], z=parameter['axis_in_cup_z']))
initial_pitch = 0
final_pitch = parameter['pitch']
assert -np.pi <= final_pitch <= initial_pitch
cup_path_in_bowl = []
for pitch in list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + [initial_pitch]:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(bowl_from_pivot, rotate_pivot, invert(base_from_pivot),
z_rotate_cup, invert(cup_urdf_from_center)))
cup_times = constant_velocity_times(cup_path_in_bowl)
# TODO: check for collisions here?
return cup_path_in_bowl, cup_times
def get_reference_pose(name): # TODO: bluecup_rotated
for model, quat in STABLE_QUATS.items():
if is_obj_type(name, model):
return (unit_point(), quat)
return unit_pose()
def main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
identity_pose = (unit_point(), unit_quat())
origin_handles = draw_pose(
identity_pose, length=1.0
) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE)
# Bodies are described by an integer index
floor = create_box(w=floor_width, l=floor_width, h=0.001,
color=TAN) # Creates a tan box object for the floor
set_point(floor,
[0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor
obstacle = create_box(w=0.5, l=0.5, h=0.1,
color=RED) # Creates a red box obstacle
set_point(
obstacle,
[0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle
print('Position:', get_point(obstacle))
set_euler(obstacle,
[0, 0, np.pi / 4
]) # Sets the [roll,pitch,yaw] orientation of the obstacle
print('Orientation:', get_euler(obstacle))
with LockRenderer(
): # Temporarily prevents the renderer from updating for improved loading efficiency
with HideOutput(): # Temporarily suppresses pybullet output
robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file
robot_z = stable_z(
robot, floor
) # Returns the z offset required for robot to be placed on floor
set_point(robot,
[0, 0, robot_z]) # Sets the z position of the robot
dump_body(robot) # Prints joint and link information about robot
set_all_static()
# Joints are also described by an integer index
# The turtlebot has explicit joints representing x, y, theta
x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x'
y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y'
theta_joint = joint_from_name(
robot, 'theta') # Looks up the robot joint named 'theta'
joints = [x_joint, y_joint, theta_joint]
base_link = link_from_name(
robot, 'base_link') # Looks up the robot link named 'base_link'
world_from_obstacle = get_pose(
obstacle
) # Returns the pose of the origin of obstacle wrt the world frame
obstacle_aabb = get_subtree_aabb(obstacle)
draw_aabb(obstacle_aabb)
random.seed(0) # Sets the random number generator state
handles = []
for i in range(10):
for handle in handles:
remove_debug(handle)
print('\nIteration: {}'.format(i))
x = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, x_joint,
x) # Sets the current value of the x joint
y = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, y_joint,
y) # Sets the current value of the y joint
yaw = random.uniform(-np.pi, np.pi)
set_joint_position(robot, theta_joint,
yaw) # Sets the current value of the theta joint
values = get_joint_positions(
robot,
joints) # Obtains the current values for the specified joints
print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values))
world_from_robot = get_link_pose(
robot,
base_link) # Returns the pose of base_link wrt the world frame
position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion)
x, y, z = position # Decomposing position into x, y, z
print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format(
x, y, z))
euler = euler_from_quat(
quaternion) # Converting from quaternion to euler angles
roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw
print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'.
format(roll, pitch, yaw))
handles.extend(
draw_pose(world_from_robot, length=0.5)
) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE)
obstacle_from_robot = multiply(
invert(world_from_obstacle),
world_from_robot) # Relative transformation from robot to obstacle
robot_aabb = get_subtree_aabb(
robot,
base_link) # Computes the robot's axis-aligned bounding box (AABB)
lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema
center = (lower + upper) / 2. # Computing the center of the AABB
extent = upper - lower # Computing the dimensions of the AABB
handles.extend(draw_aabb(robot_aabb))
collision = pairwise_collision(
robot, obstacle
) # Checks whether robot is currently colliding with obstacle
print('Collision: {}'.format(collision))
wait_for_duration(1.0) # Like sleep() but also updates the viewer
wait_for_user() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
def gen_fn(arm, conf1, conf2, fluents=[]):
arm_confs, object_grasps, object_poses, contains_liquid = parse_fluents(
world, fluents)
for a, q in arm_confs.items():
#print(a, q) # TODO: some optimistic values are getting through
set_joint_positions(world.robot, get_arm_joints(world.robot, a), q)
for name, pose in object_poses.items():
set_pose(world.bodies[name], pose)
obstacle_names = list(world.get_fixed()) + list(object_poses)
obstacles = [world.bodies[name] for name in obstacle_names]
attachments = {}
holding_water = None
water_attachment = None
for arm2, (obj, grasp) in object_grasps.items():
set_gripper_position(world.robot, arm, grasp.grasp_width)
attachment = get_grasp_attachment(world, arm2, grasp)
attachment.assign()
if arm == arm2: # The moving arm
if obj in contains_liquid:
holding_water = obj
water_attachment = attachment
attachments[obj] = attachment
else: # The stationary arm
obstacles.append(world.get_body(obj))
if not collisions:
obstacles = []
# TODO: dynamically adjust step size to be more conservative near longer movements
arm_joints = get_arm_joints(world.robot, arm)
set_joint_positions(world.robot, arm_joints, conf1)
weights, resolutions = get_weights_resolutions(world.robot, arm)
#print(holding_water, attachments, [get_body_name(body) for body in obstacles])
if teleport:
path = [conf1, conf2]
elif holding_water is None:
# TODO(caelan): unify these two methods
path = plan_joint_motion(world.robot,
arm_joints,
conf2,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
restarts=5,
iterations=50,
smooth=smooth)
else:
reference_pose = (unit_point(), get_liquid_quat(holding_water))
path = plan_water_motion(world.robot,
arm_joints,
conf2,
water_attachment,
resolutions=resolutions,
weights=weights,
obstacles=obstacles,
attachments=attachments.values(),
self_collisions=collisions,
disabled_collisions=disabled_collisions,
max_distance=COLLISION_BUFFER,
custom_limits=custom_limits,
reference_pose=reference_pose,
restarts=7,
iterations=75,
smooth=smooth)
if path is None:
return None
control = Control({
'action':
'move-arm',
#'objects': [],
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments=attachments),
'commands': [
ArmTrajectory(arm, path),
],
})
return (control, )
