def fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
def load_plane(z=-1e-3):
add_data_path()
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
if z is not None:
set_point(plane, Point(z=z))
return plane
def test_push_pour(visualize):
arms = ARMS
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
items = [bowl_name, cup_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
update_world(world, table_body)
# TODO: can prevent the right arm from being able to pick
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
# Most of the time places the cup to exchange arms
return world, task
def main():
# TODO: move to pybullet-planning for now
parser = argparse.ArgumentParser()
parser.add_argument('-viewer',
action='store_true',
help='enable the viewer while planning')
args = parser.parse_args()
print(args)
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1)
floor = create_floor()
with HideOutput():
robot = load_pybullet(get_model_path(ROOMBA_URDF),
fixed_base=True,
scale=2.0)
for link in get_all_links(robot):
set_color(robot, link=link, color=ORANGE)
robot_z = stable_z(robot, floor)
set_point(robot, Point(z=robot_z))
#set_base_conf(robot, rover_confs[i])
data_path = add_data_path()
shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF),
fixed_base=True) # TODO: returns a tuple
dump_body(shelf)
#draw_aabb(get_aabb(shelf))
wait_for_user()
disconnect()
def pick_problem(arm='left', grasp_type='side'):
other_arm = get_other_arm(arm)
initial_conf = get_carry_conf(arm, grasp_type)
pr2 = create_pr2()
set_base_values(pr2, (0, -1.2, np.pi / 2))
set_arm_conf(pr2, arm, initial_conf)
open_arm(pr2, arm)
set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
close_arm(pr2, other_arm)
plane = create_floor()
table = load_pybullet(TABLE_URDF)
# table = create_table(height=0.8)
# table = load_pybullet("table_square/table_square.urdf")
box = create_box(.07, .05, .25)
set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2))
# set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1))
return Problem(robot=pr2,
movable=[box],
arms=[arm],
grasp_types=[grasp_type],
surfaces=[table],
goal_conf=get_pose(pr2),
goal_holding=[(arm, box)])
def pour_beads(world,
cup_name,
beads,
reset_contained=False,
fix_outside=True,
cup_thickness=0.01,
bead_offset=0.01,
drop_rate=0.02,
**kwargs):
if not beads:
return set()
start_time = time.time()
# TODO: compute the radius of each bead
bead_radius = np.average(approximate_as_prism(beads[0])) / 2.
masses = list(map(get_mass, beads))
savers = list(map(BodySaver, beads))
for body in beads:
set_mass(body, 0)
cup = world.get_body(cup_name)
local_center, (diameter, height) = approximate_as_cylinder(cup)
center = get_point(cup) + local_center
interior_radius = max(0.0, diameter / 2. - bead_radius - cup_thickness)
# TODO: fill up to a certain threshold
ty = get_type(cup_name)
if ty in SPOON_DIAMETERS:
# TODO: do this in a more principled way
interior_radius = 0
center[1] += (SPOON_LENGTHS[ty] - SPOON_DIAMETERS[ty]) / 2.
# TODO: some sneak out through the bottom
# TODO: could reduce gravity while filling
world.controller.set_gravity()
for i, bead in enumerate(beads):
# TODO: halton sequence
x, y = center[:2] + np.random.uniform(
0, interior_radius) * unit_from_theta(
np.random.uniform(-np.pi, np.pi))
new_z = center[2] + height / 2. + bead_radius + bead_offset
set_point(bead, [x, y, new_z])
set_mass(bead, masses[i])
world.controller.rest_for_duration(drop_rate)
world.controller.rest_for_duration(BEADS_REST)
print('Simulated {} beads in {:3f} seconds'.format(
len(beads), elapsed_time(start_time)))
contained_beads = get_contained_beads(cup, beads, **kwargs)
#wait_for_user()
for body in beads:
if fix_outside and (body not in contained_beads):
set_mass(body, 0)
for saver in savers:
if reset_contained or (saver.body not in contained_beads):
saver.restore()
#wait_for_user()
return contained_beads
def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model(
"models/drake/pr2_description/urdf/pr2_simplified.urdf")
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
# head_link = link_from_name(pr2, HEAD_LINK)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
head_link = head_joints[-1]
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
lineColorRGB=(1, 0, 0)) # addUserDebugText
p.addUserDebugLine(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
lineColorRGB=(0, 0, 1)) # addUserDebugText
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2, target_point)
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_for_user()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
def create_elements_bodies(node_points,
elements,
color=apply_alpha(RED, alpha=1),
diameter=ELEMENT_DIAMETER,
shrink=ELEMENT_SHRINK):
# TODO: could scale the whole environment
# TODO: create a version without shrinking for transit planning
# URDF_USE_IMPLICIT_CYLINDER
element_bodies = []
for (n1, n2) in elements:
p1, p2 = node_points[n1], node_points[n2]
height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0)
#if height == 0: # Cannot keep this here
# continue
center = (p1 + p2) / 2
# extents = (p2 - p1) / 2
delta = p2 - p1
x, y, z = delta
phi = np.math.atan2(y, x)
theta = np.math.acos(z / np.linalg.norm(delta))
quat = quat_from_euler(Euler(pitch=theta, yaw=phi))
# p1 is z=-height/2, p2 is z=+height/2
# Much smaller than cylinder
# Also faster, C_shape 177.398 vs 400
body = create_box(diameter,
diameter,
height,
color=color,
mass=STATIC_MASS)
set_color(body, color)
set_point(body, center)
set_quat(body, quat)
#dump_body(body, fixed=True)
# Visually, smallest diameter is 2e-3
# The geometries and bounding boxes seem correct though
# TODO: create_cylinder takes in a radius not diameter
#body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS)
#print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height))
#print(get_aabb_extent(get_aabb(body)).round(6).tolist())
#print(get_visual_data(body))
#print(get_collision_data(body))
#set_point(body, center)
#set_quat(body, quat)
#draw_aabb(get_aabb(body))
element_bodies.append(body)
#wait_for_user()
return element_bodies
def create_beads(num, radius, parameters={}, uniform_color=None, init_x=10.0):
beads = []
if num <= 0:
return beads
#timestep = 1/1200. # 1/350.
#p.setTimeStep(timestep, physicsClientId=get_client())
for i in range(num):
color = apply_alpha(
np.random.random(3)) if uniform_color is None else uniform_color
body = create_sphere(radius, color=color) # TODO: other shapes
#set_color(droplet, color)
#print(get_dynamics_info(droplet))
set_dynamics(body, **parameters)
x = init_x + i * (2 * radius + 1e-2)
set_point(body, Point(x=x))
beads.append(body)
return beads
def main():
parser = argparse.ArgumentParser()
parser.add_argument('-shape',
default='box',
choices=['box', 'sphere', 'cylinder', 'capsule'])
parser.add_argument('-video', action='store_true')
args = parser.parse_args()
video = 'video.mp4' if args.video else None
connect(use_gui=True, mp4=video)
if video is not None:
set_renderer(enable=False)
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
add_data_path()
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
set_point(plane, Point(z=-1e-3))
ramp = create_ramp()
dump_body(ramp)
obj = create_object(args.shape)
set_euler(obj, np.random.uniform(-np.math.radians(1), np.math.radians(1),
3))
set_point(obj, np.random.uniform(-1e-3, +1e-3, 3))
#set_velocity(obj, linear=Point(x=-1))
set_position(obj, z=2.)
#set_position(obj, z=base_aligned_z(obj))
dump_body(obj)
#add_pose_constraint(obj, pose=(target_point, target_quat), max_force=200)
set_renderer(enable=True)
if video is None:
wait_if_gui('Begin?')
simulate(controller=condition_controller(lambda step:
(step != 0) and at_rest(obj)))
if video is None:
wait_if_gui('Finish?')
disconnect()
def estimate_spoon_capacity(world, spoon_name, beads, max_beads=100):
beads = beads[:max_beads]
if not beads:
return 0
bead_radius = np.average(approximate_as_prism(beads[0])) / 2.
spoon_body = world.get_body(spoon_name)
spoon_mass = get_mass(spoon_body)
set_mass(spoon_body, STATIC_MASS)
set_point(spoon_body, (1, 0, 1))
set_quat(spoon_body, get_liquid_quat(spoon_name))
capacity_beads = fill_with_beads(world,
spoon_name,
beads,
reset_contained=True,
fix_outside=False,
height_fraction=1.0,
top_threshold=bead_radius)
#wait_for_user()
set_mass(spoon_body, spoon_mass)
return len(capacity_beads)
def load_world(use_floor=True):
obstacles = []
#side, height = 10, 0.01
robot = load_robot()
with HideOutput():
lower, _ = get_aabb(robot)
if use_floor:
#floor = load_model('models/short_floor.urdf', fixed_base=True)
#add_data_path()
#floor = load_pybullet('plane.urdf', fixed_base=True)
#set_color(floor, TAN)
#floor = create_box(w=side, l=side, h=height, color=apply_alpha(GROUND_COLOR))
floor = create_plane(color=apply_alpha(GROUND_COLOR))
obstacles.append(floor)
#set_point(floor, Point(z=lower[2]))
set_point(floor, Point(x=1.2, z=0.023 - 0.025)) # -0.02
else:
floor = None # TODO: make this an empty list of obstacles
#set_all_static()
return obstacles, robot
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 main():
connect(use_gui=True)
add_data_path()
set_camera(0, -30, 1)
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
cup = load_model('models/cup.urdf', fixed_base=True)
#set_point(cup, Point(z=stable_z(cup, plane)))
set_point(cup, Point(z=.2))
set_color(cup, (1, 0, 0, .4))
num_droplets = 100
#radius = 0.025
#radius = 0.005
radius = 0.0025
# TODO: more efficient ways to make all of these
droplets = [create_sphere(radius, mass=0.01)
for _ in range(num_droplets)] # kg
cup_thickness = 0.001
lower, upper = get_lower_upper(cup)
print(lower, upper)
buffer = cup_thickness + radius
lower = np.array(lower) + buffer * np.ones(len(lower))
upper = np.array(upper) - buffer * np.ones(len(upper))
limits = zip(lower, upper)
x_range, y_range = limits[:2]
z = upper[2] + 0.1
#x_range = [-1, 1]
#y_range = [-1, 1]
#z = 1
for droplet in droplets:
x = np.random.uniform(*x_range)
y = np.random.uniform(*y_range)
set_point(droplet, Point(x, y, z))
for i, droplet in enumerate(droplets):
x, y = np.random.normal(0, 1e-3, 2)
set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3)))
#dump_world()
wait_for_user()
#user_input('Start?')
enable_gravity()
simulate_for_duration(5.0)
# enable_real_time()
# try:
# while True:
# enable_gravity() # enable_real_time requires a command
# #time.sleep(dt)
# except KeyboardInterrupt:
# pass
# print()
#time.sleep(1.0)
wait_for_user('Finish?')
disconnect()
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
#ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB)
#ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box'))
#print(ycb_path)
#load_pybullet(ycb_path)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
#set_euler(table, Euler(yaw=np.pi/2))
with HideOutput(False):
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
#robot_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf')
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
#robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table)
set_point(block1, Point(z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=+0.25, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, z=block_z))
blocks = [block1, block2, block3]
add_line(Point(x=-TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
Point(x=+TABLE_WIDTH / 2,
z=block_z - BLOCK_SIDE / 2 + EPSILON),
color=RED)
set_camera_pose(camera_point=Point(y=-1, z=block_z + 1),
target_point=Point(z=block_z))
wait_for_user()
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
y_grasp, x_grasp = get_top_grasps(block1,
tool_pose=unit_pose(),
grasp_length=0.03,
under=False)
grasp = y_grasp # fingers won't collide
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user('Finish?')
disconnect()
def main():
# TODO: update this example
connect(use_gui=True)
with HideOutput():
pr2 = load_model(DRAKE_PR2_URDF)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
draw_point(target_point)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
#head_link = child_link_from_joint(head_joints[-1])
#head_name = get_link_name(pr2, head_link)
head_name = 'high_def_optical_frame' # HEAD_LINK_NAME | high_def_optical_frame | high_def_frame
head_link = link_from_name(pr2, head_name)
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
handles = [
add_line(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
color=RED),
add_line(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
color=BLUE),
]
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2,
target_point,
head_name=head_name,
head_joints=head_joints)
assert head_conf is not None
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_if_gui()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent/2.*np.ones(2),
+floor_extent/2.*np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height/2.))
wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.))
wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY)
set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.))
wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.))
wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY)
set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.))
walls = [wall1, wall2, wall3, wall4]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
obstacles = walls + list(initial_surfaces)
initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4])
goal_conf = -initial_conf
with HideOutput():
robot = load_model(TURTLEBOT_URDF)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(initial_surfaces, obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
return robot, base_limits, goal_conf, obstacles
def visualize_vector_field(learner, bowl_type='blue_bowl', cup_type='red_cup',
num=500, delta=False, alpha=0.5):
print(learner, len(learner.results))
xx, yy, ww = learner.xx, learner.yy, learner.weights
learner.hyperparameters = get_parameters(learner.model)
print(learner.hyperparameters)
learner.query_type = REJECTION # BEST | REJECTION | STRADDLE
world = create_world()
world.bodies[bowl_type] = load_cup_bowl(bowl_type)
world.bodies[cup_type] = load_cup_bowl(cup_type)
feature = get_pour_feature(world, bowl_type, cup_type)
set_point(world.bodies[cup_type], np.array([0.2, 0, 0]))
# TODO: visualize training data as well
# TODO: artificially limit training data to make a low-confidence region
# TODO: visualize mean and var at the same time using intensity and hue
print('Feature:', feature)
with LockRenderer():
#for parameter in islice(learner.parameter_generator(world, feature, valid=True), num):
parameters = []
while len(parameters) < num:
parameter = learner.sample_parameter()
# TODO: special color if invalid
if is_valid_pour(world, feature, parameter):
parameters.append(parameter)
center, _ = approximate_as_prism(world.get_body(cup_type))
bodies = []
with LockRenderer():
for parameter in parameters:
path, _ = pour_path_from_parameter(world, feature, parameter)
pose = path[0]
body = create_cylinder(radius=0.001, height=0.02, color=apply_alpha(GREY, alpha))
set_pose(body, multiply(pose, Pose(point=center)))
bodies.append(body)
#train_sizes = inclusive_range(10, 100, 1)
#train_sizes = inclusive_range(10, 100, 10)
#train_sizes = inclusive_range(100, 1000, 100)
#train_sizes = [1000]
train_sizes = [None]
# training_poses = []
# for result in learner.results[:train_sizes[-1]]:
# path, _ = pour_path_from_parameter(world, feature, result['parameter'])
# pose = path[0]
# training_poses.append(pose)
# TODO: draw the example as well
scores_per_size = []
for train_size in train_sizes:
if train_size is not None:
learner.xx, learner.yy, learner.weights = xx[:train_size], yy[:train_size], ww[:train_size]
learner.retrain()
X = np.array([learner.func.x_from_feature_parameter(feature, parameter) for parameter in parameters])
predictions = learner.predict_x(X, noise=False) # Noise is just a constant
scores_per_size.append([prediction['mean'] for prediction in predictions]) # mean | variance
#scores_per_size.append([1./np.sqrt(prediction['variance']) for prediction in predictions])
#scores_per_size.append([prediction['mean'] / np.sqrt(prediction['variance']) for prediction in predictions])
#plt.hist(scores_per_size[-1])
#plt.show()
#scores_per_size.append([scipy.stats.norm.interval(alpha=0.95, loc=prediction['mean'],
# scale=np.sqrt(prediction['variance']))[0]
# for prediction in predictions]) # mean | variance
# score = learner.score(feature, parameter)
if delta:
scores_per_size = [np.array(s2) - np.array(s1) for s1, s2 in zip(scores_per_size, scores_per_size[1:])]
train_sizes = train_sizes[1:]
all_scores = [score for scores in scores_per_size for score in scores]
#interval = (min(all_scores), max(all_scores))
interval = scipy.stats.norm.interval(alpha=0.95, loc=np.mean(all_scores), scale=np.std(all_scores))
#interval = DEFAULT_INTERVAL
print('Interval:', interval)
print('Mean: {:.3f} | Stdev: {:.3f}'.format(np.mean(all_scores), np.std(all_scores)))
#train_body = create_cylinder(radius=0.002, height=0.02, color=apply_alpha(BLACK, 1.0))
for i, (train_size, scores) in enumerate(zip(train_sizes, scores_per_size)):
print('Train size: {} | Average: {:.3f} | Min: {:.3f} | Max: {:.3f}'.format(
train_size, np.mean(scores), min(scores), max(scores)))
handles = []
# TODO: visualize delta
with LockRenderer():
#if train_size is None:
# train_size = len(learner.results)
#set_pose(train_body, multiply(training_poses[train_size-1], Pose(point=center)))
#set_pose(world.bodies[cup_type], training_poses[train_size-1])
for score, body in zip(scores, bodies):
score = clip(score, *interval)
fraction = rescale(score, interval, new_interval=(0, 1))
color = interpolate_hue(fraction)
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) # TODO: convex combination
set_color(body, apply_alpha(color, alpha))
#set_pose(world.bodies[cup_type], pose)
#draw_pose(pose, length=0.05)
#handles.extend(draw_point(tform_point(pose, center), color=color))
#extent = np.array([0, 0, 0.01])
#start = tform_point(pose, center - extent/2)
#end = tform_point(pose, center + extent/2)
#handles.append(add_line(start, end, color=color, width=1))
wait_for_duration(0.5)
# TODO: test on boundaries
wait_for_user()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5):
floor_extent = 5.0
base_limits = (-floor_extent / 2. * np.ones(2),
+floor_extent / 2. * np.ones(2))
floor_height = 0.001
floor = create_box(floor_extent, floor_extent, floor_height, color=TAN)
set_point(floor, Point(z=-floor_height / 2.))
wall1 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.))
wall2 = create_box(floor_extent + wall_side,
wall_side,
wall_side,
color=GREY)
set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.))
wall3 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.))
wall4 = create_box(wall_side,
floor_extent + wall_side,
wall_side,
color=GREY)
set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.))
wall5 = create_box(obst_width, obst_width, obst_height, color=GREY)
set_point(wall5, Point(z=obst_height / 2.))
walls = [wall1, wall2, wall3, wall4, wall5]
initial_surfaces = OrderedDict()
for _ in range(n_obstacles - 1):
body = create_box(obst_width, obst_width, obst_height, color=GREY)
initial_surfaces[body] = floor
pillars = list(initial_surfaces)
obstacles = walls + pillars
initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4])
goal_conf = -initial_conf
robot = load_pybullet(TURTLEBOT_URDF,
fixed_base=True,
merge=True,
sat=False)
base_joints = joints_from_names(robot, BASE_JOINTS)
# base_link = child_link_from_joint(base_joints[-1])
base_link = link_from_name(robot, BASE_LINK_NAME)
set_all_color(robot, BLUE)
dump_body(robot)
set_point(robot, Point(z=stable_z(robot, floor)))
#set_point(robot, Point(z=base_aligned_z(robot)))
draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5)
set_joint_positions(robot, base_joints, initial_conf)
sample_placements(
initial_surfaces,
obstacles=[robot] + walls,
savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)],
min_distances=10e-2)
# The first calls appear to be the slowest
# times = []
# for body1, body2 in combinations(pillars, r=2):
# start_time = time.time()
# colliding = pairwise_collision(body1, body2)
# runtime = elapsed_time(start_time)
# print(colliding, runtime)
# times.append(runtime)
# print(times)
return robot, base_limits, goal_conf, obstacles
def __init__(self,
robot_name=FRANKA_CARTER,
use_gui=True,
full_kitchen=False):
self.task = None
self.interface = None
self.client = connect(use_gui=use_gui)
set_real_time(False)
#set_caching(False) # Seems to make things worse
disable_gravity()
add_data_path()
set_camera_pose(camera_point=[2, -1.5, 1])
if DEBUG:
draw_pose(Pose(), length=3)
#self.kitchen_yaml = load_yaml(KITCHEN_YAML)
with HideOutput(enable=True):
self.kitchen = load_pybullet(KITCHEN_PATH,
fixed_base=True,
cylinder=True)
self.floor = load_pybullet('plane.urdf', fixed_base=True)
z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2]
point = np.array(get_point(self.kitchen)) - np.array([0, 0, z])
set_point(self.floor, point)
self.robot_name = robot_name
if self.robot_name == FRANKA_CARTER:
urdf_path, yaml_path = FRANKA_CARTER_PATH, None
#urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML
#elif self.robot_name == EVE:
# urdf_path, yaml_path = EVE_PATH, None
else:
raise ValueError(self.robot_name)
self.robot_yaml = yaml_path if yaml_path is None else load_yaml(
yaml_path)
with HideOutput(enable=True):
self.robot = load_pybullet(urdf_path)
#dump_body(self.robot)
#chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link'))
#wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link'))
#wait_for_user()
set_point(self.robot, Point(z=stable_z(self.robot, self.floor)))
self.set_initial_conf()
self.gripper = create_gripper(self.robot)
self.environment_bodies = {}
if full_kitchen:
self._initialize_environment()
self._initialize_ik(urdf_path)
self.initial_saver = WorldSaver()
self.body_from_name = {}
# self.path_from_name = {}
self.names_from_type = {}
self.custom_limits = {}
self.base_limits_handles = []
self.cameras = {}
self.disabled_collisions = set()
if self.robot_name == FRANKA_CARTER:
self.disabled_collisions.update(
tuple(link_from_name(self.robot, link) for link in pair)
for pair in DISABLED_FRANKA_COLLISIONS)
self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf)
#self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left'))
self.calibrate_conf = FConf(
self.robot, self.arm_joints,
self.default_conf) # Must differ from carry_conf
self.special_confs = [self.carry_conf] #, self.calibrate_conf]
self.open_gq = FConf(self.robot, self.gripper_joints,
get_max_limits(self.robot, self.gripper_joints))
self.closed_gq = FConf(self.robot, self.gripper_joints,
get_min_limits(self.robot, self.gripper_joints))
self.gripper_confs = [self.open_gq, self.closed_gq]
self.open_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.open_conf(joint)])
for joint in self.kitchen_joints
}
self.closed_kitchen_confs = {
joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)])
for joint in self.kitchen_joints
}
self._update_custom_limits()
self._update_initial()
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2,
top_color=TAN, cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)),
top_offset=0.02, grasp_length=0.03, under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def main(use_turtlebot=True):
parser = argparse.ArgumentParser()
parser.add_argument('-sim', action='store_true')
parser.add_argument('-video', action='store_true')
args = parser.parse_args()
video = 'video.mp4' if args.video else None
connect(use_gui=True, mp4=video)
#set_renderer(enable=False)
# print(list_pybullet_data())
# print(list_pybullet_robots())
draw_global_system()
set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5),
target_point=Point(-1.5, +1.5, 0))
plane = load_plane()
#door = load_pybullet('models/door.urdf', fixed_base=True) # From drake
#set_point(door, Point(z=-.1))
door = create_door()
#set_position(door, z=base_aligned_z(door))
set_point(door, base_aligned(door))
#set_collision_margin(door, link=0, margin=0.)
set_configuration(door, [math.radians(-5)])
dump_body(door)
door_joint = get_movable_joints(door)[0]
door_link = child_link_from_joint(door_joint)
#draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link)
draw_pose(Pose(), parent=door, parent_link=door_link)
wait_if_gui()
##########
start_x = +2
target_x = -start_x
if not use_turtlebot:
side = 0.25
robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE)
set_position(robot, x=start_x)
#set_velocity(robot, linear=Point(x=-1))
else:
turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF)
print(turtlebot_urdf)
#print(read(turtlebot_urdf))
robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True)
robot_joints = get_movable_joints(robot)[:3]
set_joint_positions(robot, robot_joints, [start_x, 0, PI])
set_all_color(robot, BLUE)
set_position(robot, z=base_aligned_z(robot))
dump_body(robot)
##########
set_renderer(enable=True)
#test_door(door)
if args.sim:
test_simulation(robot, target_x, video)
else:
assert use_turtlebot # TODO: extend to the block
test_kinematic(robot, door, target_x)
disconnect()
def add_obstacles():
ceiling = create_box(w=0.2, l=0.2, h=0.01, color=RED)
set_point(ceiling, Point(z=0.2))
pole = create_cylinder(radius=0.025, height=0.1, color=RED)
set_point(pole, Point(z=0.15))
