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