def create_object(shape, side=0.1, mass=1, color=BLUE):
# TODO: simulation parameters
# TODO: object dynamics parameters
if shape == 'box':
obj = create_box(w=side, l=side, h=side, mass=mass, color=color)
elif shape == 'sphere':
obj = create_sphere(radius=side, mass=mass, color=color)
elif shape == 'cylinder':
obj = create_cylinder(radius=side, height=side, mass=mass, color=color)
elif shape == 'capsule':
obj = create_capsule(radius=side, height=side, mass=mass, color=color)
else:
raise ValueError(shape)
return obj
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():
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 get_pour_gen_fn(world,
max_attempts=100,
collisions=True,
parameter_fns={}):
# TODO(caelan): could also simulate the predicated sample
# TODO(caelan): be careful with the collision_buffer
parameter_fn = parameter_fns.get(get_pour_gen_fn,
POUR_PARAMETER_FNS[DESIGNED])
parameter_generator = get_parameter_generator(world, parameter_fn,
is_valid_pour)
disabled_collisions = get_disabled_collisions(world.robot)
disabled_collisions.update(get_pairwise_arm_links(world.robot, world.arms))
obstacles = [world.get_body(surface)
for surface in world.get_fixed()] if collisions else []
#world.water_body = load_pybullet(get_body_urdf('red_sphere')) # red_sphere, blue_sphere
world.water_body = create_sphere(radius=0.005, color=(1, 0, 0, 1))
def gen_fn(arm, bowl_name, bowl_pose, cup_name, grasp):
if bowl_name == cup_name:
return
attachment = get_grasp_attachment(world, arm, grasp)
bowl_body = world.get_body(bowl_name)
cup_body = world.get_body(cup_name)
feature = get_pour_feature(world, bowl_name, cup_name)
# TODO: this may be called several times with different grasps
for parameter in parameter_generator(feature):
for i in range(max_attempts):
set_pose(bowl_body,
bowl_pose) # Reset because might have changed
set_gripper_position(world.robot, arm, grasp.grasp_width)
cup_path_bowl, times_from_start = pour_path_from_parameter(
world, feature, parameter)
rotate_bowl = Pose(euler=Euler(
yaw=random.uniform(-np.pi, np.pi)))
cup_path = [
multiply(bowl_pose, invert(rotate_bowl), cup_pose_bowl)
for cup_pose_bowl in cup_path_bowl
]
#if world.visualize:
# visualize_cartesian_path(cup_body, cup_path)
if cartesian_path_collision(cup_body, cup_path,
obstacles + [bowl_body]):
print('Attempt {}: Pour path collision!'.format(i))
continue
tool_waypoints = [
multiply(p, invert(grasp.grasp_pose)) for p in cup_path
]
grip_conf = solve_inverse_kinematics(world.robot,
arm,
tool_waypoints[0],
obstacles=obstacles)
if grip_conf is None:
continue
if water_robot_collision(world, bowl_body, bowl_pose, cup_body,
cup_path):
print('Attempt {}: Water robot collision'.format(i))
continue
if water_obstacle_collision(world, bowl_body, bowl_pose,
cup_body, cup_path):
print('Attempt {}: Water obstacle collision'.format(i))
continue
post_path = plan_workspace_motion(
world.robot,
arm,
tool_waypoints,
obstacles=obstacles + [bowl_body],
attachments=[attachment],
self_collisions=collisions,
disabled_collisions=disabled_collisions)
if post_path is None:
continue
pre_conf = Conf(post_path[-1])
pre_path = post_path[::-1]
post_conf = pre_conf
control = Control({
'action':
'pour',
'feature':
feature,
'parameter':
parameter,
'objects': [bowl_name, cup_name],
'times':
times_from_start,
'context':
Context(
savers=[BodySaver(world.robot)
], # TODO: robot might be at the wrong conf
attachments={cup_name: attachment}),
'commands': [
ArmTrajectory(arm, pre_path, dialation=2.),
Rest(duration=2.0),
ArmTrajectory(arm, post_path, dialation=2.),
],
})
yield (pre_conf, post_conf, control)
break
else:
yield None
return gen_fn
