Решение системы линейных уравнений типа AutoDiffXd ⇐ Python

[Anonymous]

У меня есть уравнение в форме M(q, dq)*ddq + C(q, dq) + G(q) = f, где мне нужно найти ddq. Я столкнулся с ошибкой, когда np.linalg.solve (и вариант scipy) несовместим с типом Drake AutoDiffXd. Как я могу решить эту проблему?

Код: Выделить всё

from pydrake.systems.framework import LeafSystem_, Context, VectorBase, ContinuousState
from pydrake.systems.scalar_conversion import TemplateSystem

import numpy as np

from BallbotMPC.ballbot_params import BallbotParams

@TemplateSystem.define("BallbotSystem_")
def BallbotSystem_(T):
class Impl(LeafSystem_[T]):
def _construct(self, converter=None, params=BallbotParams()):
LeafSystem_[T].__init__(self, converter=converter)

self._params = params

self.DeclareContinuousState(4) # q: 4 states [ball angle, ball velocity, lean angle, lean velocity]

self._command_port = self.DeclareVectorInputPort("u", 4)

self._state_port = self.DeclareVectorOutputPort("state", 4, self.CopyStateOut)

def _construct_copy(self, other, converter=None):
Impl._construct(self, converter=converter, params=other._params)

def DoCalcTimeDerivatives(self, context: Context, derivatives: ContinuousState):
# constants
mass = self._params.mass_k + self._params.mass_a + self._params.mass_w
radius_total = self._params.radius_k + self._params.radius_w
gam = self._params.l * self._params.mass_a + radius_total * self._params.mass_w

state = self._state_port.Eval(context)
u = self._command_port.Eval(context)

# Dtype, necessary for numpy to understand the type of the drake types (AutoDiffXd, etc.)
dtype = type(state[0])
print(dtype)

# Mass term in Langrangian formulation (Eqn. 2.22)
M_x = np.zeros((2, 2), dtype=dtype)
M_x[0, 0] = mass * self._params.radius_k ** 2 + self._params.Theta_k + (self._params.radius_k / self._params.radius_w) ** 2 * self._params.Theta_w
M_x[0, 1] = -(self._params.radius_k / self._params.radius_w ** 2) * radius_total * self._params.Theta_w + gam * self._params.radius_k * np.cos(state[2])
M_x[1, 0] = M_x[0, 1]
M_x[1, 1] = (radius_total / self._params.radius_w) ** 2 * self._params.Theta_w + self._params.Theta_a + self._params.mass_a * self._params.l ** 2 + self._params.mass_w * radius_total ** 2

# Coriolis term in Langrangian formulation (Eqn. 2.23)
C_x = np.zeros(2, dtype=dtype)
C_x[0] = -self._params.radius_k * gam * np.sin(state[2]) * state[3] ** 2

# Gravitational term in Lagrangian formulation (Eqn. 2.24)
G_x = np.zeros(2, dtype=dtype)
G_x[1] = -self._params.g * np.sin(state[2]) * gam

# Non-potential force term in Lagrangian formulation (Eqn. 2.17 + Eqn. 2.18)
f_np = np.zeros(2, dtype=dtype)
f_np[0] = self._params.radius_k / self._params.radius_w * u
f_np[1] = -f_np[0]

# Solve for ddq in M(q, dq)*ddq + C(q, dq) + G(q) = f_np
ddq = np.linalg.solve(M_x, f_np - C_x - G_x)

derivatives.get_mutable_vector().SetAtIndex(0, state[1])
derivatives.get_mutable_vector().SetAtIndex(1, ddq[0])
derivatives.get_mutable_vector().SetAtIndex(2, state[3])
derivatives.get_mutable_vector().SetAtIndex(3, ddq[1])

def CopyStateOut(self, context: Context, output: VectorBase):
state = context.get_continuous_state_vector().CopyToVector()
output.SetFromVector(state)

return Impl

BallbotSystem = BallbotSystem_[None]

Использование Ubuntu 24.04 с Drake 1.34.0 и numpy 2.1.3 в виртуальной среде Python 3.12.

Подробнее здесь: https://stackoverflow.com/questions/791 ... iffxd-type