def init(self):
self.time = []
self.kinetic_energy = []
self.potential_energy = []
self.mechanichal_energy = []
self._com_pos = {}
self._bodies = self._world.ground.iter_descendant_bodies
for body in self._bodies():
self._com_pos[body] = principalframe(body.mass)[:,3]
def _register(self, obj):
"""Add the given arboris object ``obj`` to the scene.
For a single arboris object, several OSG nodes may be added,
belonging to different categories. For instance,
registering an object of the :class:`Body` class returns nodes
belonging to the ``frame``, ``name`` and ``inertia ellispoid``
categories.
The categories a node belongs to is specified using node masks.
"""
if obj in self.registered:
pass
else:
self.registered.append(obj)
opts = self._options
if isinstance(obj, core.Frame):
# create a transform for the frames (instances of
# the Body and Subframe classes)
t = osg.MatrixTransform()
self.frames[obj] = t
# draw the frame name:
name = draw_text(obj.name, opts["text size"])
name.setNodeMask(_MASK["name"])
t.addChild(name)
# draw the frame basis:
t.addChild(self._generic_frame)
if isinstance(obj, core.Body):
self.root.addChild(t)
if obj.mass[5, 5] != 0:
# draw the body inertia ellipsoid.
#
# MatrixTransform() # position
# |
# PositionAttitudeTransform() # ellispoid axis scale
# |
# Geode()
# |
# Sphere()
#
Mb = obj.mass
color = self._choose_color(obj)
bHg = massmatrix.principalframe(Mb)
Mg = massmatrix.transport(Mb, bHg)
shape = osg.ShapeDrawable(osg.Sphere(osg.Vec3(0.0, 0.0, 0.0), 1.0))
shape.setColor(color)
shape_geo = osg.Geode()
shape_geo.addDrawable(shape)
scale_node = osg.PositionAttitudeTransform()
scale_node.setScale(osg.Vec3d(Mg[0, 0], Mg[1, 1], Mg[2, 2]))
scale_node.addChild(shape_geo)
gen_scale_node = osg.PositionAttitudeTransform()
gen_scale_node.addChild(scale_node)
pos_node = osg.MatrixTransform()
pos_node.setMatrix(_pose2mat(bHg))
pos_node.addChild(gen_scale_node)
pos_node.setNodeMask(_MASK["inertia ellipsoid"])
self.frames[obj].addChild(pos_node)
elif isinstance(obj, core.SubFrame) or isinstance(obj, core.MovingSubFrame):
self.frames[obj.body].addChild(t)
# draw a line between the subframe and its parent:
color = self._choose_color(obj.body)
nl = draw_line(
(0, 0, 0), -dot(obj.bpose[0:3, 0:3].T, obj.bpose[0:3, 3]), opts["link radius"], color
)
nl.setNodeMask(_MASK["link"])
self.frames[obj].addChild(nl)
elif isinstance(obj, constraints.SoftFingerContact) or isinstance(obj, constraints.BallAndSocketConstraint):
t = osg.PositionAttitudeTransform()
self.constraint_forces[obj] = t
t.setNodeMask(_MASK["constraint force"])
self.frames[obj._frames[0]].addChild(t)
t.addChild(self._generic_force)
elif isinstance(obj, core.Shape):
color = self._choose_color(obj.frame.body)
if isinstance(obj, shapes.Sphere):
shape = osg.ShapeDrawable(osg.Sphere(osg.Vec3(0.0, 0.0, 0.0), obj.radius))
elif isinstance(obj, shapes.Point):
shape = osg.ShapeDrawable(osg.Sphere(osg.Vec3(0.0, 0.0, 0.0), self._options["point radius"]))
elif isinstance(obj, shapes.Box):
shape = osg.ShapeDrawable(
osg.Box(
osg.Vec3(0.0, 0.0, 0.0),
obj.half_extents[0] * 2.0,
obj.half_extents[1] * 2,
obj.half_extents[2] * 2,
)
)
elif isinstance(obj, shapes.Cylinder):
shape = osg.ShapeDrawable(osg.Cylinder(osg.Vec3(0.0, 0.0, 0.0), obj.radius, obj.length))
else:
raise NotImplemented("Cannot draw this shape")
shape.setColor(color)
switch = osg.Switch()
geode = osg.Geode()
geode.addDrawable(shape)
geode.setNodeMask(_MASK["shape"])
self.frames[obj.frame].addChild(geode)
elif isinstance(obj, core.Joint) or isinstance(obj, core.Constraint) or isinstance(obj, core.Controller):
pass
else:
raise NotImplemented(obj)
def _register(self, obj):
"""Given an arboris obect ``obj`` as input, this method
return a dictionnary of OSG nodes for the rendering of
``obj``. I also returns a dictionary of osg switches, in
order to make it possible to disable the rendering of some nodes.
Each node is the child of its corresponding switch.
If the optional ``parent`` argument is given, the switches are its
children
for instance, if ``obj`` is an instance of
:class:`arboris.core.SubFrame`, and ``parent`` is given, it will
result in the following tree::
parent
|
+-----------------+------------------+
| | |
switches['name'] switches['frame'] switches['link']
| | |
nodes['name'] nodes['frame'] nodes['link']
- ``name`` for any instance of the :class:`arboris.core.NamedObject` class
- ``frame`` for objects of the :class:`arboris.core.Frame` class
- ``link`` lines for skeletton-like view
- ``shapes`` the basic shapes (:class:`arboris.core.Shape`)
used in the simulation for contact detection
- ``geometry``
- ``cog``
"""
if obj in self.registered:
pass
else:
self.registered.append(obj)
opts = self._options
def _add_switch(parent, child, name):
switch = osg.Switch()
parent.addChild(switch)
switch.addChild(child)
switch.setName(name)
if isinstance(obj, core.Frame):
# create a transform for the frames (instances of
# the Body and Subframe classes)
t = osg.MatrixTransform()
self.frames[obj] = t
# draw the frame name:
_add_switch(t, draw_text(obj.name, opts['text size']), 'name')
# draw the frame basis:
_add_switch(t, self._generic_frame, 'frame')
if isinstance(obj, core.Body):
self.root.addChild(t)
if obj.mass[5,5] != 0:
# draw the body inertia ellipsoid.
#
# MatrixTransform() # position
# |
# PositionAttitudeTransform() # ellispoid axis scale
# |
# Geode()
# |
# Sphere()
#
Mb = obj.mass
color = self._choose_color(obj)
bHg = massmatrix.principalframe(Mb)
Mg = massmatrix.transport(Mb, bHg)
shape = osg.ShapeDrawable(
osg.Sphere(osg.Vec3(0.,0.,0.), 1.))
shape.setColor(color)
shape_geo = osg.Geode()
shape_geo.addDrawable(shape)
scale_node = osg.PositionAttitudeTransform()
scale_node.setScale(
osg.Vec3d(Mg[0,0], Mg[1,1], Mg[2,2]))
scale_node.addChild(shape_geo)
gen_scale_node = osg.PositionAttitudeTransform()
gen_scale_node.addChild(scale_node)
pos_node = osg.MatrixTransform()
pos_node.setMatrix(pose2mat(bHg))
pos_node.addChild(gen_scale_node)
_add_switch(self.frames[obj], pos_node,
'inertia ellipsoid')
elif isinstance(obj, core.SubFrame):
self.frames[obj.body].addChild(t)
# draw a line between the subframe and its parent:
color = self._choose_color(obj.body)
nl = draw_line((0,0,0),
-dot(obj.bpose[0:3,0:3].T, obj.bpose[0:3,3]),
opts['link radius'], color)
_add_switch(self.frames[obj], nl, 'link')
elif isinstance(obj, constraints.SoftFingerContact):
t = osg.PositionAttitudeTransform()
self.constraint_forces[obj] = t
_add_switch(self.frames[obj._frames[0]],t,"constraint force")
t.addChild(self._generic_force)
elif isinstance(obj, core.Shape):
color = self._choose_color(obj.frame.body)
if isinstance(obj, shapes.Sphere):
shape = osg.ShapeDrawable(
osg.Sphere(osg.Vec3(0.,0.,0.), obj.radius))
elif isinstance(obj, shapes.Point):
shape = osg.ShapeDrawable(
osg.Sphere(osg.Vec3(0.,0.,0.),
self._options['point radius']))
elif isinstance(obj, shapes.Box):
shape = osg.ShapeDrawable(
osg.Box(osg.Vec3(0.,0.,0.), obj.lengths[0],
obj.lengths[1], obj.lengths[2]))
elif isinstance(obj, shapes.Cylinder):
shape = osg.ShapeDrawable(
osg.Cylinder(osg.Vec3(0.,0.,0.),
obj.radius, obj.length))
else:
raise NotImplemented("Cannot draw this shape")
shape.setColor(color)
switch = osg.Switch()
self.frames[obj.frame].addChild(switch)
geode = osg.Geode()
geode.addDrawable(shape)
switch.addChild(geode)
switch.setName('shape')
elif isinstance(obj, core.Joint) or \
isinstance(obj, core.Constraint) or \
isinstance(obj, core.Controller):
pass
else:
raise NotImplemented(obj)