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])