def body_collision(body1, body2=None):
if body2 is not None:
if is_cylinder(body1) and is_cylinder(body2): # TODO - ode cylinder collision checks are having issues
return aabb_collision(body1, body2)
return get_env().CheckCollision(body1, body2)
for body2 in get_env().GetBodies():
if body2 != body1 and body_collision(body1, body2):
return True
return False
def update_viewer(): get_env().UpdatePublishedBodies()
def manip_point(q):
robot = get_env().GetRobots()[0]
with robot:
robot.SetActiveDOFValues(q.value)
return point_from_trans(get_trans(robot.GetActiveManipulator()))
def place_body(env, body, (x, y, theta), base_name): # TODO - make it place on the relative table
body_aabb = aabb_from_body(body)
base_aabb = aabb_from_body(env.GetKinBody(base_name))
z = get_point(body)[2] - (body_aabb.pos()[2] - body_aabb.extents()[2]) + \
(base_aabb.pos()[2] + base_aabb.extents()[2]) + BODY_PLACEMENT_Z_OFFSET
set_point(body, (x, y, z))
set_quat(body, quat_from_axis_angle(0, 0, theta))
env.Add(body)
def place_body_on_floor(body, (x, y, theta)):
aabb = aabb_from_body(body)
z = get_point(body)[2] - (aabb.pos()[2] - aabb.extents()[2]) + BODY_PLACEMENT_Z_OFFSET
set_point(body, (x, y, z))
set_quat(body, quat_from_axis_angle(0, 0, theta))
get_env().Add(body)
def randomly_place_body(env, body, table_names):
# import pdb; pdb.set_trace()
if env.GetKinBody(get_name(body)) is None:
env.Add(body)
while True:
set_quat(body, quat_from_z_rot(uniform(0, 2*PI)))
aabb = aabb_from_body(body)
region = AARegion.create_on_body(env.GetKinBody(choice(table_names)))
cspace = region.cspace(aabb)
if cspace is None: continue
set_point(body, np.array([uniform(*range) for range in cspace] +
[region.z + aabb.extents()[2] + BODY_PLACEMENT_Z_OFFSET]) - aabb.pos() + get_point(body))
if not body_collision(body): return
def load_fixed_problem(problem_name):
if hasattr(fixed, problem_name):
return getattr(fixed, problem_name)(get_env()) # dir(fixed)