def system_function(robot: Robot):
M = numpy.matrix([[
robot.I_wheel() + robot.I_platform() +
robot.I_flywheel(robot.r_min()) + robot.m_total() * robot.r_wheel**2,
robot.I_platform() + robot.I_flywheel(robot.r_min()),
robot.I_flywheel(robot.r_min())
],
[
robot.I_platform() + robot.I_flywheel(robot.r_min()),
robot.I_platform() + robot.I_flywheel(robot.r_min()),
robot.I_flywheel(robot.r_min())
],
[
robot.I_flywheel(robot.r_min()),
robot.I_flywheel(robot.r_min()),
robot.I_flywheel(robot.r_min())
]])
a = robot.m_cylinder() * (robot.r_min() - robot.r_max()) * robot.g
def aux_function(t, x):
q = x[0:3]
q_dot = x[3:6]
phi_ground_flywheel = q[0] + q[1] + q[2]
aux = (a * math.sin(phi_ground_flywheel) * numpy.ones(3) +
external_torque(robot, t, q, q_dot))
q_ddot = numpy.matmul(numpy.linalg.inv(M), numpy.transpose(aux))
q_ddot = numpy.transpose(q_ddot)
return [q_dot[0], q_dot[1], q_dot[2], q_ddot[0], q_ddot[1], q_ddot[2]]
return lambda t, x: aux_function(t, x)
