def create_inertia(self, inertia, color, name=""): H_body_com = principalframe(inertia) Mcom = transport(inertia, H_body_com) mass = Mcom[5, 5] m_o_inertia = Mcom[[0, 1, 2], [0, 1, 2]] position = H_body_com[0:3, 3] yaw, pitch, roll = np.degrees( rotzyx_angles(H_body_com)) # in degrees for collada specification self.physics_model.append( E.rigid_body(E.technique_common( E.dynamic("true"), E.mass(str(mass)), E.mass_frame( E.translate(" ".join(map(str, position)), sid="location"), E.rotate(" ".join(map(str, [0, 0, 1, yaw])), sid="rotationZ"), E.rotate(" ".join(map(str, [0, 1, 0, pitch])), sid="rotationY"), E.rotate(" ".join(map(str, [1, 0, 0, roll])), sid="rotationX"), ), E.inertia(" ".join(map(str, m_o_inertia))), ), sid=name))
def rotation_matrix_to_roll_pitch_yaw(R): """ """ H = np.eye(4) H[0:3,0:3] = R yaw, pitch, roll = rotzyx_angles(H) return np.array([roll, pitch, yaw])
def create_transform(self, pose, is_constant, name=None): """Generate the node corresponding to the pose.""" if name: node = Element(QN("node"), {"id":name, "name":name}) else: node = Element(QN("node")) if not is_constant: rotz, roty, rotx = rotzyx_angles(pose) coeff = 180./pi matrix = SubElement(node, QN("matrix"), {'sid':"matrix"}) matrix.text = str(pose.reshape(16))[1:-1] elif not (pose == eye(4)).all(): matrix = SubElement(node, QN("matrix"), {'sid':'matrix'}) matrix.text = str(pose.reshape(-1)).strip('[]') return node
def create_transform(self, pose, is_constant, name=None): """Generate the node corresponding to the pose.""" if name: node = Element(QN("node"), {"id": name, "name": name}) else: node = Element(QN("node")) if not is_constant: rotz, roty, rotx = rotzyx_angles(pose) coeff = 180. / pi matrix = SubElement(node, QN("matrix"), {'sid': "matrix"}) matrix.text = str(pose.reshape(16))[1:-1] elif not (pose == eye(4)).all(): matrix = SubElement(node, QN("matrix"), {'sid': 'matrix'}) matrix.text = str(pose.reshape(-1)).strip('[]') return node
def create_inertia(self, inertia, color, name=""): H_body_com = principalframe(inertia) Mcom = transport(inertia, H_body_com) mass = Mcom[5, 5] m_o_inertia = Mcom[[0, 1, 2], [0, 1, 2]] position = H_body_com[0:3, 3] yaw, pitch, roll = np.degrees(rotzyx_angles(H_body_com)) # in degrees for collada specification self.physics_model.append( E.rigid_body( E.technique_common( E.dynamic("true"), E.mass(str(mass)), E.mass_frame( E.translate(" ".join(map(str, position)), sid="location"), E.rotate(" ".join(map(str, [0, 0, 1, yaw])), sid="rotationZ"), E.rotate(" ".join(map(str, [0, 1, 0, pitch])), sid="rotationY"), E.rotate(" ".join(map(str, [1, 0, 0, roll])), sid="rotationX"), ), E.inertia(" ".join(map(str, m_o_inertia))), ), sid=name, ) )