コード例 #1
0
ファイル: core.py プロジェクト: mitkof6/arboris-python
    def __init__(self, name=None, mass=None, viscosity=None):
        NamedObject.__init__(self, name)
        Frame.__init__(self)
        if mass is None:
            mass = zeros((6, 6))
        else:
            assert(ismassmatrix(mass, True))
        if viscosity is None:
            viscosity = zeros((6, 6))
        else:
            pass #TODO: check the viscosity matrix

        # the object will have a frame, with the same name as the object itself
        self.parentjoint    = None
        self.childrenjoints = []
        self.mass           = mass
        self.viscosity      = viscosity
        self._pose          = None # updated by update_{geometric,kinematic,dynamic}
        self._jacobian      = None # updated by update_{geometric,kinematic,dynamic}
        self._djacobian     = None # updated by update_dynamic
        self._twist         = None # updated by update_dynamic
        self._nleffects     = None # updated by update_dynamic
コード例 #2
0
twist = np.array(rot_velocity + lin_velocity) # rotation velocity first
print("twist", twist)



##### About mass matrix ########################################################

import arboris.massmatrix as Mm


# mass matrix expressed in principal inertia frame, length in "m", mass in "kg"
M_com = Mm.sphere(radius=.1, mass=10)
M_com = Mm.ellipsoid(radii=(.1, .2, .3), mass=10)
M_com = Mm.cylinder(length=.1, radius=.2, mass=10)  # revolution axis along z
M_com = Mm.box(half_extents=(.1, .2, .3), mass=10)



isMm = Mm.ismassmatrix(M_com)       # check validity of mass matrix
print("is mass matrix?", isMm)


H_com_a= Hg.transl(.4,.5,.6)        # translation from {a} (new frame) to {com}
M_a = Mm.transport(M_com, H_com_a)


H_a_com = Mm.principalframe(M_a)    # return principal inertia frame from {a}
print("H_com_a * H_a_com\n", np.dot(H_com_a, H_a_com))