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)