INNER CODE UNIT · Python
sp
MOLAorg/mola · mola_demos/demos/fake_sensor_publisher.py:108
cp, sp = math.cos(pitch), math.sin(pitch)
cr, sr = math.cos(roll), math.sin(roll)
R = np.array([
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
[-sp, cp * sr, cp * cr],
])
return R.T @ sf
# ---------------------------------------------------------------------------
# Ground-truth trajectories
# ---------------------------------------------------------------------------
def _gt_static(_t):
return _STATIC_X, _STATIC_Y, 0.0, _STATIC_YAW, 0.0, 0.0