def create_door(width=0.08,
length=1,
height=2,
mass=1,
handle=True,
frame=True,
**kwargs):
# TODO: hinge, cylinder on end, sliding door, knob
# TODO: explicitly disable self collisions (happens automatically)
geometry = get_box_geometry(width, length, height)
hinge = 0 # -width/2
com_point = Point(x=hinge, y=-length / 2., z=height / 2.)
door_collision, door_visual = create_shape(geometry,
pose=Pose(com_point),
**kwargs)
door_link = LinkInfo(
mass=mass,
collision_id=door_collision,
visual_id=door_visual,
#point=com_point,
inertial_point=com_point, # TODO: be careful about the COM
parent=0,
joint_type=p.JOINT_REVOLUTE,
joint_axis=[0, 0, 1])
links = [door_link]
if handle:
links.append(create_handle(width, length, height))
if frame:
links.append(create_frame(width, length, height))
body = create_multi_body(base_link=LinkInfo(), links=links)
#draw_circle(center=unit_point(), diameter=width/2., parent=body, parent_link=0)
#set_joint_limits(body, link=0, lower=-PI, upper=PI)
return body
def parse_object(obj, mesh_directory):
name = obj.find('name').text
mesh_filename = obj.find('geom').text
geom = get_mesh_geometry(os.path.join(mesh_directory, mesh_filename))
pose = parse_pose(obj.find('pose'))
movable = parse_boolean(obj.find('moveable'))
color = (.75, .75, .75, 1)
if 'red' in name:
color = (1, 0, 0, 1)
elif 'green' in name:
color = (0, 1, 0, 1)
elif 'blue' in name:
color = (0, 0, 1, 1)
elif movable: # TODO: assign new color
#size = 2 * MAX_INT
#size = 255
#n = id(obj) % size
#n = hash(obj) % size
#h = float(n) / size
h = random.random()
r, g, b = colorsys.hsv_to_rgb(h, .75, .75)
color = (r, g, b, 1)
print(name, mesh_filename, movable, color)
collision_id, visual_id = create_shape(geom, color=color)
body_id = p.createMultiBody(baseMass=STATIC_MASS,
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id,
physicsClientId=CLIENT)
set_pose(body_id, pose)
return body_id
def main(group=SE3):
connect(use_gui=True)
set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.]))
# TODO: can also create all links and fix some joints
# TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed
obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED)
#robot = create_cylinder(radius=0.025, height=0.1, color=BLUE)
obstacles = [obstacle]
collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE)
robot = create_flying_body(group, collision_id, visual_id)
body_link = get_links(robot)[-1]
joints = get_movable_joints(robot)
joint_from_group = dict(zip(group, joints))
print(joint_from_group)
#print(get_aabb(robot, body_link))
dump_body(robot, fixed=False)
custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()}
# sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits)
# for i in range(10):
# conf = sample_fn()
# set_joint_positions(robot, joints, conf)
# wait_for_user('Iteration: {}'.format(i))
# return
initial_point = SIZE*np.array([-1., -1., 0])
#initial_point = -1.*np.ones(3)
final_point = -initial_point
initial_euler = np.zeros(3)
add_line(initial_point, final_point, color=GREEN)
initial_conf = np.concatenate([initial_point, initial_euler])
final_conf = np.concatenate([final_point, initial_euler])
set_joint_positions(robot, joints, initial_conf)
#print(initial_point, get_link_pose(robot, body_link))
#set_pose(robot, Pose(point=-1.*np.ones(3)))
path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles,
self_collisions=False, custom_limits=custom_limits)
if path is None:
disconnect()
return
for i, conf in enumerate(path):
set_joint_positions(robot, joints, conf)
point, quat = get_link_pose(robot, body_link)
#euler = euler_from_quat(quat)
euler = intrinsic_euler_from_quat(quat)
print(conf)
print(point, euler)
wait_for_user('Step: {}/{}'.format(i, len(path)))
wait_for_user('Finish?')
disconnect()
def parse_region(region):
lower = np.min(region['hull'], axis=0)
upper = np.max(region['hull'], axis=0)
x, y = (lower + upper) / 2.
w, h = (upper - lower) # / 2.
geom = get_box_geometry(w, h, 1e-3)
collision_id, visual_id = create_shape(geom,
pose=Pose(Point(x, y)),
color=parse_color(region['color']))
#region_id = create_body(NULL_ID, visual_id)
region_id = create_body(collision_id, visual_id)
set_pose(region_id, parse_pose(region))
return region_id
def parse_body(body, important=False):
[link] = body['links']
# for geometry in link['geometries']:
geoms = []
poses = []
colors = []
skipped = False
for geometry in link:
geom, pose, color = parse_geometry(geometry)
if geom == None:
skipped = True
else:
geoms.append(geom)
poses.append(pose)
colors.append(color)
if skipped:
if important:
center = body['aabb']['center']
extents = 2 * np.array(body['aabb']['extents'])
geoms = [get_box_geometry(*extents)]
poses = [Pose(center)]
colors = [(.5, .5, .5, 1)]
else:
return None
if not geoms:
return None
if len(geoms) == 1:
collision_id, visual_id = create_shape(geoms[0],
pose=poses[0],
color=colors[0])
else:
collision_id, visual_id = create_shape_array(geoms, poses, colors)
body_id = create_body(collision_id, visual_id)
set_pose(body_id, parse_pose(body))
return body_id
