def time_step_position(O, qd, delta_t): w_body_frame, v_body_frame = matrix.col_list([qd.elems[:3], qd.elems[3:]]) qed = rbda_eq_4_13(q=O.unit_quaternion) * w_body_frame new_qe = (O.qe + qed * delta_t).normalize() qrd = O.e.transpose() * v_body_frame new_qr = O.qr + qrd * delta_t return six_dof(qe=new_qe, qr=new_qr)
def time_step_position(O, qd, delta_t):
w_body_frame, v_body_frame = matrix.col_list(
[qd.elems[:3], qd.elems[3:]])
qed = rbda_eq_4_13(q=O.unit_quaternion) * w_body_frame
new_qe = (O.qe + qed * delta_t).normalize()
qrd = O.e.transpose() * v_body_frame
new_qr = O.qr + qrd * delta_t
return six_dof(qe=new_qe, qr=new_qr)
def new_q(O, q): new_qe, new_qr = matrix.col_list((q[:4], q[4:])) return six_dof(qe=new_qe, qr=new_qr)
def tau_as_d_e_pot_d_q(O, tau): d = d_unit_quaternion_d_qe_matrix(q=O.qe) c = d * 4 * rbda_eq_4_13(q=O.unit_quaternion) n, f = matrix.col_list([tau.elems[:3], tau.elems[3:]]) return matrix.col((c * n, O.e.transpose() * f)).resolve_partitions()
def tau_as_d_e_pot_d_q(O, tau):
d = d_unit_quaternion_d_qe_matrix(q=O.qe)
c = d * 4 * rbda_eq_4_13(q=O.unit_quaternion)
n, f = matrix.col_list([tau.elems[:3], tau.elems[3:]])
return matrix.col((c * n, O.e.transpose() * f)).resolve_partitions()
def new_q(O, q):
new_qe, new_qr = matrix.col_list((q[:4], q[4:]))
return six_dof(qe=new_qe, qr=new_qr)