def _build_analyzer_body(self, model, body_parts, connections): """ Builds a model from body parts in analyzer mode - i.e. one link per body part to allow collision detection. :param model: :param body_parts: :type body_parts: list[BodyPart] :return: """ part_map = {} for body_part in body_parts: link = Link("link_"+body_part.id, self_collide=True) link.set_position(body_part.get_position()) link.set_rotation(body_part.get_rotation()) body_part.set_position(Vector3()) body_part.set_rotation(Quaternion()) link.add_element(body_part) model.add_element(link) part_map[body_part.id] = link # Create fake joints for all connections so they will # not trigger collisions. for a, b in connections: joint = FixedJoint(part_map[a], part_map[b]) model.add_element(joint)
def gen_boxes(model_name, dimensions=4, spacing=0.5, size=0.05, height=0.015, center_sq=1): model = Model(model_name, static=True) for x in range(-dimensions, dimensions + 1): for y in range(-dimensions, dimensions + 1): l = Link("box") if (abs(x) >= center_sq or abs(y) >= center_sq): l.make_box(1, size, size, height) pos = Vector3(spacing * x, spacing * y, height / 2) l.set_position(pos) #l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False) model.add_element(l) return model
def gen_boxes(dimensions=4, spacing=0.5, size=0.05): for x in range(-dimensions,dimensions+1): for y in range(-dimensions,dimensions+1): l = Link("box") #box_geom = Box(1.0, 1.0, 1.0, mass=0.5) #b = StructureCombination("box", box_geom) #l.add_element(b) if (x!=0 or y!=0): l.make_box(1, size, size, 0.01*max(abs(x),abs(y))) pos = Vector3(spacing*x,spacing*y,0) l.set_position(pos) l.rotate_around(Vector3(0, 0, 1), math.radians(x*y), relative_to_child=False) model.add_element(l)
def gen_steps(model_name, num_steps=6, offset=0.4, height=0.02, width=3.0, depth=0.2, incline=0): model = Model(model_name, static=True) steps = PosableGroup() for x in range(0, num_steps): l = Link("box") l.make_box(1, depth, width, height) pos = Vector3(offset + depth * x, 0, height / 2 + x * height) l.set_position(pos) steps.add_element(l) for x in range(0, 4): steps.set_rotation( Quaternion.from_rpy(0, math.radians(-incline), math.radians(90 * x))) model.add_element(steps.copy()) return model