def create_meshes(ty, draw=False, visualize=False):
assert not (visualize and draw) # Incompatible?
suffix = SUFFIX_TEMPLATE.format(ty)
models_path = os.path.join(get_models_path(), MODELS_TEMPLATE.format(ty))
ensure_dir(models_path)
for prefix, properties in OBJECT_PROPERTIES[ty].items():
color = normalize_rgb(properties.color)
side = approximate_bowl(properties, d=2) # 1 doesn't seem to really work
name = prefix + suffix
print(name, color)
print(side)
if draw:
draw_curvature(side, name=name)
chunks = make_revolute_chunks(side, n_theta=60, n_chunks=10,
in_off=properties.thickness/4.,
scale=SCALE)
obj_path = os.path.join(models_path, OBJ_TEMPLATE.format(name))
write_obj(chunks, obj_path)
if visualize:
body = create_obj(obj_path, color=color)
_, dims = approximate_as_cylinder(body)
print(dims)
wait_for_user()
remove_body(body)
pcd_path = os.path.join(models_path, PCD_TEMPLATE.format(name))
pcd_from_mesh(obj_path, pcd_path)
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 load_ycb(ycb_name, **kwargs):
# TODO: simplify geometry
ycb_type = type_from_name(ycb_name)
ycb_obj_path = get_ycb_obj_path(ycb_type)
assert ycb_obj_path is not None
# TODO: set color (as average) or texture
return create_obj(ycb_obj_path, color=None, **kwargs)
def test_ycb(world):
name = 'mustard_bottle' # potted_meat_can | cracker_box | sugar_box | mustard_bottle | bowl
path_from_name = get_ycb_objects()
print(path_from_name)
ycb_directory = os.path.join(YCB_DIRECTORY, path_from_name[name], SENSOR)
obj_path = os.path.join(ycb_directory, OBJ_PATH)
image_path = os.path.join(ycb_directory, PNG_PATH)
print(obj_path)
#body = load_pybullet(obj_path) #, fixed_base=True)
body = create_obj(obj_path, color=WHITE)
set_texture(body, image_path)
for link in get_all_links(body):
set_color(body, link=link, color=WHITE)
wait_if_gui()
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 load_pick_and_place(extrusion_name, scale=MILLIMETER, max_bricks=6):
assert extrusion_name == 'choreo_brick_demo'
root_directory = os.path.dirname(os.path.abspath(__file__))
bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY,
'bricks')
print('Name: {}'.format(extrusion_name))
with open(
os.path.join(bricks_directory,
PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f:
json_data = json.loads(f.read())
kuka_urdf = '../conrob_pybullet/models/kuka_kr6_r900/urdf/kuka_kr6_r900_mit_suction_gripper.urdf'
obj_directory = os.path.join(bricks_directory, 'meshes', 'collision')
with HideOutput():
#world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf'))
robot = load_pybullet(os.path.join(root_directory, kuka_urdf),
fixed_base=True)
#set_point(robot, (0.14, 0, 0))
#dump_body(robot)
pick_base_point = parse_point(json_data['pick_base_center_point'])
draw_pose((pick_base_point, unit_quat()))
place_base_point = parse_point(json_data['place_base_center_point'])
draw_pose((place_base_point, unit_quat()))
# workspace_geo_place_support_object_11 = pick_left_over_bricks_11
obstacle_from_name = {}
for filename in json_data['pick_support_surface_file_names']:
obstacle_from_name[strip_extension(filename)] = \
create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1))
for filename in json_data['place_support_surface_file_names']:
obstacle_from_name[strip_extension(filename)] = \
create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1))
brick_from_index = {}
for json_element in json_data['sequenced_elements']:
index = json_element['order_id']
pick_body = create_obj(os.path.join(
obj_directory, json_element['pick_element_geometry_file_name']),
scale=scale,
color=(0, 0, 1, 1))
add_body_name(pick_body, index)
#place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']),
# scale=scale, color=(0, 1, 0, 1))
#add_body_name(place_body, name)
world_from_obj_pick = get_pose(pick_body)
# [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane',
# u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane']
ee_grasp_poses = [{
name: pose_from_tform(parse_transform(approach))
for name, approach in json_grasp.items()
} for json_grasp in json_element['grasps']]
# pick_grasp_plane is at the top of the object with z facing downwards
grasp_index = 0
world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane']
world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane']
#draw_pose(world_from_ee_pick, length=0.04)
#draw_pose(world_from_ee_place, length=0.04)
ee_from_obj = multiply(invert(world_from_ee_pick),
world_from_obj_pick) # Using pick frame
world_from_obj_place = multiply(world_from_ee_place, ee_from_obj)
#set_pose(pick_body, world_from_obj_place)
grasps = [
Grasp(
index, num, *[
multiply(invert(world_from_ee_pick[name]),
world_from_obj_pick) for name in GRASP_NAMES
]) for num, world_from_ee_pick in enumerate(ee_grasp_poses)
]
brick_from_index[index] = Brick(
index=index,
body=pick_body,
initial_pose=WorldPose(index, world_from_obj_pick),
goal_pose=WorldPose(index, world_from_obj_place),
grasps=grasps,
goal_supports=json_element.get('place_contact_ngh_ids', []))
# pick_contact_ngh_ids are movable element contact partial orders
# pick_support_surface_file_names are fixed element contact partial orders
return robot, brick_from_index, obstacle_from_name
def load_cup_bowl(bowl, **kwargs):
obj_path, color = load_cup_bowl_obj(bowl)
if obj_path is None:
return None
from plan_tools.common import MODEL_MASSES
return create_obj(obj_path, mass=MODEL_MASSES[bowl], color=color, **kwargs)
