Esempio n. 1
0
 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]
Esempio n. 2
0
    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)
Esempio n. 3
0
    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)