def test_expm(self):
# Test for eye
correct_res = diag(exp(DM.ones(3)))
a = expm(DM.eye(3))
self.assertAlmostEqual(norm_fro(a - correct_res), 0, 3)
# Test for -magic(3) (compared with MATLAB solution)
a = DM([[-8, -1, -6], [-3, -5, -7], [-4, -9, -2]])
correct_res = DM(
[[3.646628887990924, 32.404567030885005, -36.051195612973601],
[5.022261973341555, 44.720086474306093, -49.742348141745325],
[-8.668890555430160, -77.124653199288772, 85.793544060621244]])
self.assertAlmostEqual(norm_fro(expm(a) - correct_res), 0, 2)
def __eq__(self, other):
if not isinstance(other, type(self)):
return NotImplemented
for p in [*self.points, *self.params]:
if cas.norm_fro(getattr(self, p) - getattr(other, p)) > self._eps:
return False
return True
def dxdt(h, states, controls, params):
u = controls
x = CX(states.shape[0], n_step + 1)
p = params
x[:, 0] = states
nb_dof = 0
quat_idx = []
quat_number = 0
for j in range(model.nbSegment()):
if model.segment(j).isRotationAQuaternion():
quat_idx.append([nb_dof, nb_dof + 1, nb_dof + 2, model.nbDof() + quat_number])
quat_number += 1
nb_dof += model.segment(j).nbDof()
for i in range(1, n_step + 1):
t_norm_init = (i - 1) / n_step # normalized time
k1 = fun(x[:, i - 1], get_u(u, t_norm_init), p)[:, idx]
k2 = fun(x[:, i - 1] + h / 2 * k1, get_u(u, t_norm_init + h_norm / 2), p)[:, idx]
k3 = fun(x[:, i - 1] + h / 2 * k2, get_u(u, t_norm_init + h_norm / 2), p)[:, idx]
k4 = fun(x[:, i - 1] + h * k3, get_u(u, t_norm_init + h_norm), p)[:, idx]
x[:, i] = x[:, i - 1] + h / 6 * (k1 + 2 * k2 + 2 * k3 + k4)
for j in range(model.nbQuat()):
quaternion = vertcat(
x[quat_idx[j][3], i], x[quat_idx[j][0], i], x[quat_idx[j][1], i], x[quat_idx[j][2], i]
)
quaternion /= norm_fro(quaternion)
x[quat_idx[j][0] : quat_idx[j][2] + 1, i] = quaternion[1:4]
x[quat_idx[j][3], i] = quaternion[0]
return x[:, -1], x
def states_to_euler_rate(states):
# maximizing Lagrange twist velocity (indeterminate of quaternion to Euler of 2*pi*n)
def body_vel_to_euler_rate(w, e):
# xyz convention
_ = e[0]
th = e[1]
ps = e[2]
wx = w[0]
wy = w[1]
wz = w[2]
dph = cas.cos(ps) / cas.cos(th) * wx - cas.sin(ps) / cas.cos(th) * wy
dth = cas.sin(ps) * wx + cas.cos(ps) * wy
dps = -cas.cos(ps) * cas.sin(th) / cas.cos(th) * wx + cas.sin(
th) * cas.sin(ps) / cas.cos(th) * wy + wz
return cas.vertcat(dph, dth, dps)
quaternion_cas = cas.vertcat(states[8], states[3], states[4], states[5])
quaternion_cas /= cas.norm_fro(quaternion_cas)
quaternion = biorbd.Quaternion(quaternion_cas[0], quaternion_cas[1],
quaternion_cas[2], quaternion_cas[3])
omega = cas.vertcat(states[12:15])
euler = biorbd.Rotation.toEulerAngles(
biorbd.Quaternion.toMatrix(quaternion), "xyz").to_mx()
eul_rate = body_vel_to_euler_rate(omega, euler)
return cas.Function("max_twist", [states], [eul_rate]).expand()
def states_to_euler(states):
quaternion_cas = cas.vertcat(states[8], states[3], states[4], states[5])
quaternion_cas /= cas.norm_fro(quaternion_cas)
quaternion = biorbd.Quaternion(quaternion_cas[0], quaternion_cas[1],
quaternion_cas[2], quaternion_cas[3])
euler = biorbd.Rotation.toEulerAngles(
biorbd.Quaternion.toMatrix(quaternion), "xyz").to_mx()
return cas.Function("states_to_euler", [states], [euler])
def norm(x, ord=None, axis=None):
"""
Matrix or vector norm.
See syntax here: https://numpy.org/doc/stable/reference/generated/numpy.linalg.norm.html
"""
if not is_casadi_type(x):
return _onp.linalg.norm(x, ord=ord, axis=axis)
else:
# Figure out which axis, if any, to take a vector norm about.
if axis is not None:
if not (
axis==0 or
axis==1 or
axis == -1
):
raise ValueError("`axis` must be -1, 0, or 1 for CasADi types.")
elif x.shape[0] == 1:
axis = 1
elif x.shape[1] == 1:
axis = 0
if ord is None:
if axis is not None:
ord = 2
else:
ord = 'fro'
if ord == 1:
# return _cas.norm_1(x)
return sum(
abs(x),
axis=axis
)
elif ord == 2:
# return _cas.norm_2(x)
return sum(
x ** 2,
axis=axis
) ** 0.5
elif ord == 'fro':
return _cas.norm_fro(x)
elif np.isinf(ord):
return _cas.norm_inf()
else:
try:
return sum(
abs(x) ** ord,
axis=axis
) ** (1 / ord)
except Exception as e:
print(e)
raise ValueError("Couldn't interpret `ord` sensibly! Tried to interpret it as a floating-point order "
"as a last-ditch effort, but that didn't work.")
def test_so3():
r = ca.DM([0.1, 0.2, 0.3, 0])
v = ca.DM([0.1, 0.2, 0.3])
R = Dcm.from_euler(ca.DM([0.1, 0.2, 0.3]))
q1 = Quat.from_euler(ca.DM([0.1, 0.2, 0.3]))
assert ca.norm_2(Dcm.log(Dcm.exp(v)) - v) < eps
assert ca.norm_2(Quat.log(Quat.exp(v)) - v) < eps
assert ca.norm_2(Mrp.log(Mrp.exp(v)) - v) < eps
assert ca.norm_2(so3.vee(so3.wedge(v)) - v) < eps
assert ca.norm_2(Quat.from_dcm(Dcm.from_quat(q1)) - q1) < eps
assert ca.norm_2(Quat.from_mrp(Mrp.from_quat(q1)) - q1) < eps
assert ca.norm_2(Quat.from_euler(Euler.from_quat(q1)) - q1) < eps
assert ca.norm_2(Mrp.from_dcm(Dcm.from_mrp(r)) - r) < eps
assert ca.norm_2(Mrp.from_quat(Quat.from_mrp(r)) - r) < eps
assert ca.norm_2(Mrp.from_euler(Euler.from_mrp(r)) - r) < eps
assert ca.norm_fro(Dcm.from_quat(Quat.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_mrp(Mrp.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_euler(Euler.from_dcm(R)) - R) < eps
R = ca.DM([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
assert ca.norm_fro(Dcm.from_quat(Quat.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_mrp(Mrp.from_dcm(R)) - R) < eps
assert ca.norm_fro(Dcm.from_euler(Euler.from_dcm(R)) - R) < eps
def dxdt(self, h: float, states: Union[MX, SX], controls: Union[MX, SX],
params: Union[MX, SX]) -> tuple:
"""
The dynamics of the system
Parameters
----------
h: float
The time step
states: Union[MX, SX]
The states of the system
controls: Union[MX, SX]
The controls of the system
params: Union[MX, SX]
The parameters of the system
Returns
-------
The derivative of the states
"""
u = controls
x = self.CX(states.shape[0], self.n_step + 1)
p = params
x[:, 0] = states
n_dof = 0
quat_idx = []
quat_number = 0
for j in range(self.model.nbSegment()):
if self.model.segment(j).isRotationAQuaternion():
quat_idx.append([
n_dof, n_dof + 1, n_dof + 2,
self.model.nbDof() + quat_number
])
quat_number += 1
n_dof += self.model.segment(j).nbDof()
for i in range(1, self.n_step + 1):
t_norm_init = (i - 1) / self.n_step # normalized time
x[:, i] = self.next_x(h, t_norm_init, x[:, i - 1], u, p)
for j in range(self.model.nbQuat()):
quaternion = vertcat(x[quat_idx[j][3], i], x[quat_idx[j][0],
i],
x[quat_idx[j][1], i], x[quat_idx[j][2],
i])
quaternion /= norm_fro(quaternion)
x[quat_idx[j][0]:quat_idx[j][2] + 1, i] = quaternion[1:4]
x[quat_idx[j][3], i] = quaternion[0]
return x[:, -1], x
def euler_to_quaternion(angle):
quaternion = biorbd.Quaternion.fromMatrix(
biorbd.Rotation.fromEulerAngles(angle, "xyz")).to_mx()
quaternion /= cas.norm_fro(quaternion)
return cas.Function("euler_to_quaternion", [angle], [quaternion])
