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)