コード例 #1
0
ファイル: joint_lib.py プロジェクト: cctbx/cctbx-playground
 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)
コード例 #2
0
 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)
コード例 #3
0
ファイル: joint_lib.py プロジェクト: cctbx/cctbx-playground
 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)
コード例 #4
0
ファイル: joint_lib.py プロジェクト: cctbx/cctbx-playground
 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()
コード例 #5
0
 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()
コード例 #6
0
 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)