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 _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
