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