def color_structure(element_bodies, printed, next_element):
# TODO: could also do this with debug segments
element_colors = {}
for element in printed:
element_colors[element] = apply_alpha(BLUE, alpha=1)
element_colors[next_element] = apply_alpha(GREEN, alpha=1)
remaining = set(element_bodies) - printed - {next_element}
for element in remaining:
element_colors[element] = apply_alpha(RED, alpha=0.5)
for element, color in element_colors.items():
body = element_bodies[element]
[shape] = get_visual_data(body)
if color != shape.rgbaColor:
set_color(body, color=color)
def randomize_body(name,
width_range=(1., 1.),
height_range=(1., 1.),
mass_range=(1., 1.)):
average_mass = MODEL_MASSES[get_type(name)]
obj_path, color = load_cup_bowl_obj(get_type(name))
if obj_path is None:
obj_path, color = get_body_obj(name), apply_alpha(get_color(name))
width_scale = np.random.uniform(*width_range)
height_scale = np.random.uniform(*height_range)
#if (width_scale == 1.) and (height_scale == 1.):
# return load_pybullet(obj_path)
transform = np.diag([width_scale, width_scale, height_scale])
transformed_obj_file = transform_obj_file(read(obj_path), transform)
transformed_dir = os.path.join(os.getcwd(), 'temp_models/')
ensure_dir(transformed_dir)
global RANDOMIZED_OBJS
transformed_obj_path = os.path.join(
transformed_dir,
'transformed_{}_{}'.format(len(RANDOMIZED_OBJS),
os.path.basename(obj_path)))
#RANDOMIZED_OBJS[name].append(transformed_obj_path) # pybullet caches obj files
write(transformed_obj_path, transformed_obj_file)
# TODO: could scale mass proportionally
mass_scale = np.random.uniform(*mass_range)
mass = mass_scale * average_mass
body = create_obj(transformed_obj_path, scale=1, mass=mass, color=color)
RANDOMIZED_OBJS[body] = ObjInfo(width_scale, height_scale, mass_scale)
return body
def create_bounding_mesh(printed_elements, element_bodies=None, node_points=None, buffer=0.):
# TODO: use bounding boxes instead of points
# TODO: connected components
assert printed_elements
assert element_bodies or node_points
printed_points = []
if node_points is not None:
printed_points.extend(node_points[n] for element in printed_elements for n in element)
if element_bodies is not None:
for element in printed_elements:
body = element_bodies[element]
printed_points.extend(apply_affine(get_pose(body), vertices_from_link(body, BASE_LINK)))
if buffer != 0.:
half_extents = buffer*np.ones(3)
for point in list(printed_points):
printed_points.extend(np.array(point) + np.array(corner)
for corner in get_aabb_vertices(AABB(-half_extents, half_extents)))
rgb = colorsys.hsv_to_rgb(h=random.random(), s=1, v=1)
#rgb = RED
try:
mesh = convex_hull(printed_points)
# handles = draw_mesh(mesh)
return create_mesh(mesh, under=True, color=apply_alpha(rgb, 0.5))
except QhullError as e:
print(printed_elements)
raise e
def main():
# The URDF loader seems robust to package:// and slightly wrong relative paths?
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with LockRenderer():
with HideOutput():
robot = load_model(MOVO_URDF, fixed_base=True)
for link in get_links(robot):
set_color(robot,
color=apply_alpha(0.25 * np.ones(3), 1),
link=link)
base_joints = joints_from_names(robot, BASE_JOINTS)
draw_base_limits((get_min_limits(
robot, base_joints), get_max_limits(robot, base_joints)),
z=1e-2)
dump_body(robot)
wait_for_user('Start?')
#for arm in ARMS:
# test_close_gripper(robot, arm)
arm = 'right'
tool_link = link_from_name(robot, TOOL_LINK.format(arm))
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = base_joints + get_arm_joints(robot, arm)
#joints = get_movable_joints(robot)
print('Joints:', get_joint_names(robot, joints))
ik_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link)
#fixed_joints = []
fixed_joints = ik_joints[:1]
#fixed_joints = ik_joints
sample_fn = get_sample_fn(robot, joints)
handles = []
for i in range(10):
print('Iteration:', i)
conf = sample_fn()
print(conf)
set_joint_positions(robot, joints, conf)
tool_pose = get_link_pose(robot, tool_link)
remove_handles(handles)
handles = draw_pose(tool_pose)
wait_for_user()
#conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose,
# fixed_joints=fixed_joints, max_time=0.1), None)
#if conf is not None:
# set_joint_positions(robot, ik_joints, conf)
#wait_for_user()
test_retraction(robot,
MOVO_INFOS[arm],
tool_link,
fixed_joints=fixed_joints,
max_time=0.1)
disconnect()
def add_table(world, use_surface=False):
# Use the table in simulation to ensure no penetration
if use_surface:
# TODO: note whether the convex hull image was clipped (only partial)
table_mesh = rectangular_mesh(width=0.6, length=1.2)
color = apply_alpha(COLORS['tan'])
table_body = create_mesh(table_mesh, under=True, color=color)
set_pose(table_body, TABLE_POSE)
world.perception.sim_bodies[TABLE_NAME] = table_body
world.perception.sim_surfaces[TABLE_NAME] = None
else:
table_body = world.perception.add_surface(TABLE_NAME, TABLE_POSE)
return table_body
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 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 add_camera(self,
name,
pose,
camera_matrix,
max_depth=KINECT_DEPTH,
display=False):
cone = get_viewcone(depth=max_depth,
camera_matrix=camera_matrix,
color=apply_alpha(RED, 0.1),
mass=0,
collision=False)
set_pose(cone, pose)
if display:
kinect = load_pybullet(KINECT_URDF, fixed_base=True)
set_pose(kinect, pose)
set_color(kinect, BLACK)
self.add(name, kinect)
self.cameras[name] = Camera(cone, camera_matrix, max_depth)
draw_pose(pose)
step_simulation()
return name
def validate_trajectories(element_bodies, fixed_obstacles, trajectories):
if trajectories is None:
return False
# TODO: combine all validation procedures
remove_all_debug()
for body in element_bodies.values():
set_color(body, np.zeros(4))
print('Trajectories:', len(trajectories))
obstacles = list(fixed_obstacles)
for i, trajectory in enumerate(trajectories):
for _ in trajectory.iterate():
#wait_for_user()
if any(pairwise_collision(trajectory.robot, body) for body in obstacles):
if has_gui():
print('Collision on trajectory {}'.format(i))
wait_for_user()
return False
if isinstance(trajectory, PrintTrajectory):
body = element_bodies[trajectory.element]
set_color(body, apply_alpha(RED))
obstacles.append(body)
return True
def load_body(name):
average_mass = MODEL_MASSES[get_type(name)]
obj_path, color = load_cup_bowl_obj(get_type(name))
if obj_path is None:
obj_path, color = get_body_obj(name), apply_alpha(get_color(name))
return create_obj(obj_path, scale=1, mass=average_mass, color=color)
def visualize_collected_pours(paths, num=6, save=True):
result_from_bowl = {}
for path in randomize(paths):
results = read_data(path)
print(path, len(results))
for result in results:
result_from_bowl.setdefault(result['feature']['bowl_type'], []).append(result)
world = create_world()
environment = get_bodies()
#collector = SKILL_COLLECTORS['pour']
print(get_camera())
for bowl_type in sorted(result_from_bowl):
# TODO: visualize same
for body in get_bodies():
if body not in environment:
remove_body(body)
print('Bowl type:', bowl_type)
bowl_body = load_cup_bowl(bowl_type)
bowl_pose = get_pose(bowl_body)
results = result_from_bowl[bowl_type]
results = randomize(results)
score_cup_pose = []
for i, result in enumerate(results):
if num <= len(score_cup_pose):
break
feature = result['feature']
parameter = result['parameter']
score = result['score']
if (score is None) or not result['execution'] or result['annotation']:
continue
cup_body = load_cup_bowl(feature['cup_type'])
world.bodies[feature['bowl_name']] = bowl_body
world.bodies[feature['cup_name']] = cup_body
fraction = compute_fraction_filled(score)
#value = collector.score_fn(feature, parameter, score)
value = pour_score(feature, parameter, score)
print(i, feature['cup_type'], fraction, value)
path, _ = pour_path_from_parameter(world, feature, parameter)
sort = fraction
#sort = parameter['pitch']
#sort = parameter['axis_in_bowl_z']
score_cup_pose.append((sort, fraction, value, cup_body, path[0]))
#order = score_cup_pose
order = randomize(score_cup_pose)
#order = sorted(score_cup_pose)
angles = np.linspace(0, 2*np.pi, num=len(score_cup_pose), endpoint=False) # Halton
for angle, (_, fraction, value, cup_body, pose) in zip(angles, order):
#fraction = clip(fraction, min_value=0, max_value=1)
value = clip(value, *DEFAULT_INTERVAL)
fraction = rescale(value, DEFAULT_INTERVAL, new_interval=(0, 1))
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN)
color = interpolate_hue(fraction)
set_color(cup_body, apply_alpha(color, alpha=0.25))
#angle = random.uniform(-np.pi, np.pi)
#angle = 0
rotate_bowl = Pose(euler=Euler(yaw=angle))
cup_pose = multiply(bowl_pose, invert(rotate_bowl), pose)
set_pose(cup_body, cup_pose)
#wait_for_user()
#remove_body(cup_body)
if save:
#filename = os.path.join('workspace', '{}.png'.format(bowl_type))
#save_image(filename, take_picture()) # [0, 255]
#wait_for_duration(duration=0.5)
# TODO: get window location
#os.system("screencapture -R {},{},{},{} {}".format(
# 275, 275, 500, 500, filename)) # -R<x,y,w,h> capture screen rect
wait_for_user()
remove_body(bowl_body)
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 plan_extrusion(args_list, viewer=False, verify=False, verbose=False, watch=False):
results = []
if not args_list:
return results
# TODO: setCollisionFilterGroupMask
if not verbose:
sys.stdout = open(os.devnull, 'w')
problems = {args.problem for args in args_list}
assert len(problems) == 1
[problem] = problems
# TODO: change dir for pddlstream
extrusion_path = get_extrusion_path(problem)
#extrusion_path = rotate_problem(extrusion_path)
element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path, verbose=True)
elements = sorted(element_from_id.values())
#node_points = transform_model(problem, elements, node_points, ground_nodes)
connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR)
with LockRenderer(lock=True):
#draw_pose(unit_pose(), length=1.)
obstacles, robot = load_world()
#draw_model(elements, node_points, ground_nodes)
#wait_for_user()
color = apply_alpha(BLACK, alpha=0) # 0, 1
#color = None
element_bodies = dict(zip(elements, create_elements_bodies(node_points, elements, color=color)))
set_extrusion_camera(node_points)
#if viewer:
# label_nodes(node_points)
saver = WorldSaver()
#visualize_stiffness(extrusion_path)
#debug_elements(robot, node_points, node_order, elements)
initial_position = point_from_pose(get_link_pose(robot, link_from_name(robot, TOOL_LINK)))
checker = None
plan = None
for args in args_list:
if args.stiffness and (checker is None):
checker = create_stiffness_checker(extrusion_path, verbose=False)
saver.restore()
#initial_conf = get_joint_positions(robot, get_movable_joints(robot))
with LockRenderer(lock=not viewer):
start_time = time.time()
plan, data = None, {}
with timeout(args.max_time):
with Profiler(num=10, cumulative=False):
plan, data = solve_extrusion(robot, obstacles, element_from_id, node_points, element_bodies,
extrusion_path, ground_nodes, args, checker=checker)
runtime = elapsed_time(start_time)
sequence = recover_directed_sequence(plan)
if verify:
max_translation, max_rotation = compute_plan_deformation(extrusion_path, recover_sequence(plan))
valid = check_plan(extrusion_path, sequence)
print('Valid:', valid)
safe = validate_trajectories(element_bodies, obstacles, plan)
print('Safe:', safe)
data.update({
'safe': safe,
'valid': valid,
'max_translation': max_translation,
'max_rotation': max_rotation,
})
data.update({
'runtime': runtime,
'num_elements': len(elements),
'ee_distance': compute_sequence_distance(node_points, sequence, start=initial_position, end=initial_position),
'print_distance': get_print_distance(plan, teleport=True),
'distance': get_print_distance(plan, teleport=False),
'sequence': sequence,
'parameters': get_global_parameters(),
'problem': args.problem,
'algorithm': args.algorithm,
'heuristic': args.bias,
'plan_extrusions': not args.disable,
'use_collisions': not args.cfree,
'use_stiffness': args.stiffness,
})
print(data)
#plan_path = '{}_solution.json'.format(args.problem)
#with open(plan_path, 'w') as f:
# json.dump(plan_data, f, indent=2, sort_keys=True)
results.append((args, data))
reset_simulation()
disconnect()
if watch and (plan is not None):
args = args_list[-1]
animate = not (args.disable or args.ee_only)
connect(use_gui=True, shadows=SHADOWS, color=BACKGROUND_COLOR) # TODO: avoid reconnecting
obstacles, robot = load_world()
display_trajectories(node_points, ground_nodes, plan, #time_step=None, video=True,
animate=animate)
reset_simulation()
disconnect()
if not verbose:
sys.stdout.close()
return results
def normalize_rgb(color, **kwargs):
return apply_alpha(np.array(color, dtype=float) / 255.0, **kwargs)
