kinematics package

Submodules

kinematics.equations module

kinematics.equations.B_acc_BW_exp = SX(@1=(((R_WC_0*R_BC_0)+(R_WC_3*R_BC_3))+(R_WC_6*R_BC_6)), @2=(((R_WC_0*R_BC_1)+(R_WC_3*R_BC_4))+(R_WC_6*R_BC_7)), @3=(((R_WC_0*R_BC_2)+(R_WC_3*R_BC_5))+(R_WC_6*R_BC_8)), @4=2, @5=(((R_WC_1*R_BC_0)+(R_WC_4*R_BC_3))+(R_WC_7*R_BC_6)), @6=(((R_WC_1*R_BC_1)+(R_WC_4*R_BC_4))+(R_WC_7*R_BC_7)), @7=(((R_WC_1*R_BC_2)+(R_WC_4*R_BC_5))+(R_WC_7*R_BC_8)), @8=(W_om_CW_1-(((@5*B_om_CB_0)+(@6*B_om_CB_1))+(@7*B_om_CB_2))), @9=(((R_WC_2*R_BC_0)+(R_WC_5*R_BC_3))+(R_WC_8*R_BC_6)), @10=(((R_WC_2*R_BC_1)+(R_WC_5*R_BC_4))+(R_WC_8*R_BC_7)), @11=(((R_WC_2*R_BC_2)+(R_WC_5*R_BC_5))+(R_WC_8*R_BC_8)), @12=(((@9*BB_v_CB_0)+(@10*BB_v_CB_1))+(@11*BB_v_CB_2)), @13=(W_om_CW_2-(((@9*B_om_CB_0)+(@10*B_om_CB_1))+(@11*B_om_CB_2))), @14=(((@5*BB_v_CB_0)+(@6*BB_v_CB_1))+(@7*BB_v_CB_2)), @15=(((@1*B_om_CB_0)+(@2*B_om_CB_1))+(@3*B_om_CB_2)), @16=(W_om_CW_0-(((@1*B_om_CB_0)+(@2*B_om_CB_1))+(@3*B_om_CB_2))), @17=(((@9*B_om_CB_0)+(@10*B_om_CB_1))+(@11*B_om_CB_2)), @18=((W_alp_CW_1-(((@5*B_alp_CB_0)+(@6*B_alp_CB_1))+(@7*B_alp_CB_2)))-((@13*@15)-(@16*@17))), @19=(((@9*B_p_CB_0)+(@10*B_p_CB_1))+(@11*B_p_CB_2)), @20=(((@5*B_om_CB_0)+(@6*B_om_CB_1))+(@7*B_om_CB_2)), @21=((W_alp_CW_2-(((@9*B_alp_CB_0)+(@10*B_alp_CB_1))+(@11*B_alp_CB_2)))-((@16*@20)-(@8*@15))), @22=(((@5*B_p_CB_0)+(@6*B_p_CB_1))+(@7*B_p_CB_2)), @23=(((@5*B_p_CB_0)+(@6*B_p_CB_1))+(@7*B_p_CB_2)), @24=(((@1*B_p_CB_0)+(@2*B_p_CB_1))+(@3*B_p_CB_2)), @25=((@16*@23)-(@8*@24)), @26=(((@9*B_p_CB_0)+(@10*B_p_CB_1))+(@11*B_p_CB_2)), @27=((@13*@24)-(@16*@26)), @28=((((W_acc_CW_0-(((@1*B_acc_CB_0)+(@2*B_acc_CB_1))+(@3*B_acc_CB_2)))-(@4*((@8*@12)-(@13*@14))))-((@18*@19)-(@21*@22)))-((@8*@25)-(@13*@27))), @29=(((@1*BB_v_CB_0)+(@2*BB_v_CB_1))+(@3*BB_v_CB_2)), @30=(((@1*B_p_CB_0)+(@2*B_p_CB_1))+(@3*B_p_CB_2)), @31=((W_alp_CW_0-(((@1*B_alp_CB_0)+(@2*B_alp_CB_1))+(@3*B_alp_CB_2)))-((@8*@17)-(@13*@20))), @32=((@8*@26)-(@13*@23)), @33=((((W_acc_CW_1-(((@5*B_acc_CB_0)+(@6*B_acc_CB_1))+(@7*B_acc_CB_2)))-(@4*((@13*@29)-(@16*@12))))-((@21*@30)-(@31*@19)))-((@13*@32)-(@16*@25))), @34=((((W_acc_CW_2-(((@9*B_acc_CB_0)+(@10*B_acc_CB_1))+(@11*B_acc_CB_2)))-(@4*((@16*@14)-(@8*@29))))-((@31*@22)-(@18*@30)))-((@16*@27)-(@8*@32))), [((((((R_BC_0*R_WC_0)+(R_BC_3*R_WC_3))+(R_BC_6*R_WC_6))*@28)+((((R_BC_0*R_WC_1)+(R_BC_3*R_WC_4))+(R_BC_6*R_WC_7))*@33))+((((R_BC_0*R_WC_2)+(R_BC_3*R_WC_5))+(R_BC_6*R_WC_8))*@34)), ((((((R_BC_1*R_WC_0)+(R_BC_4*R_WC_3))+(R_BC_7*R_WC_6))*@28)+((((R_BC_1*R_WC_1)+(R_BC_4*R_WC_4))+(R_BC_7*R_WC_7))*@33))+((((R_BC_1*R_WC_2)+(R_BC_4*R_WC_5))+(R_BC_7*R_WC_8))*@34)), ((((((R_BC_2*R_WC_0)+(R_BC_5*R_WC_3))+(R_BC_8*R_WC_6))*@28)+((((R_BC_2*R_WC_1)+(R_BC_5*R_WC_4))+(R_BC_8*R_WC_7))*@33))+((((R_BC_2*R_WC_2)+(R_BC_5*R_WC_5))+(R_BC_8*R_WC_8))*@34))]): Averaged IMU measurements

kinematics.equations.C_om_CW_exp = SX(@1=(B_om_BW_0+B_om_CB_0), @2=(B_om_BW_1+B_om_CB_1), @3=(B_om_BW_2+B_om_CB_2), [(((R_BC_0*@1)+(R_BC_1*@2))+(R_BC_2*@3)), (((R_BC_3*@1)+(R_BC_4*@2))+(R_BC_5*@3)), (((R_BC_6*@1)+(R_BC_7*@2))+(R_BC_8*@3))]): IMU reference trajectory (analytical)

kinematics.equations.R_acc_avg_exp = SX(@1=2, @2=(dt*((B_om_BW_2+B_om_BW_n_2)/@1)), @3=(dt*((B_om_BW_1+B_om_BW_n_1)/@1)), @4=(dt*((B_om_BW_0+B_om_BW_n_0)/@1)), [(((((R_WB_0*B_acc_BW_0)+(R_WB_3*B_acc_BW_1))+(R_WB_6*B_acc_BW_2))+((((R_WB_0+((R_WB_3*@2)-(R_WB_6*@3)))*B_acc_BW_n_0)+((R_WB_3+((R_WB_6*@4)-(R_WB_0*@2)))*B_acc_BW_n_1))+((R_WB_6+((R_WB_0*@3)-(R_WB_3*@4)))*B_acc_BW_n_2)))/@1), (((((R_WB_1*B_acc_BW_0)+(R_WB_4*B_acc_BW_1))+(R_WB_7*B_acc_BW_2))+((((R_WB_1+((R_WB_4*@2)-(R_WB_7*@3)))*B_acc_BW_n_0)+((R_WB_4+((R_WB_7*@4)-(R_WB_1*@2)))*B_acc_BW_n_1))+((R_WB_7+((R_WB_1*@3)-(R_WB_4*@4)))*B_acc_BW_n_2)))/@1), (((((R_WB_2*B_acc_BW_0)+(R_WB_5*B_acc_BW_1))+(R_WB_8*B_acc_BW_2))+((((R_WB_2+((R_WB_5*@2)-(R_WB_8*@3)))*B_acc_BW_n_0)+((R_WB_5+((R_WB_8*@4)-(R_WB_2*@2)))*B_acc_BW_n_1))+((R_WB_8+((R_WB_2*@3)-(R_WB_5*@4)))*B_acc_BW_n_2)))/@1)]): Camera

kinematics.equations.W_acc_BW_exp = SX(@1=(((R_WC_0*R_BC_0)+(R_WC_3*R_BC_3))+(R_WC_6*R_BC_6)), @2=(((R_WC_0*R_BC_1)+(R_WC_3*R_BC_4))+(R_WC_6*R_BC_7)), @3=(((R_WC_0*R_BC_2)+(R_WC_3*R_BC_5))+(R_WC_6*R_BC_8)), @4=2, @5=(((R_WC_1*R_BC_0)+(R_WC_4*R_BC_3))+(R_WC_7*R_BC_6)), @6=(((R_WC_1*R_BC_1)+(R_WC_4*R_BC_4))+(R_WC_7*R_BC_7)), @7=(((R_WC_1*R_BC_2)+(R_WC_4*R_BC_5))+(R_WC_7*R_BC_8)), @8=(W_om_CW_1-(((@5*B_om_CB_0)+(@6*B_om_CB_1))+(@7*B_om_CB_2))), @9=(((R_WC_2*R_BC_0)+(R_WC_5*R_BC_3))+(R_WC_8*R_BC_6)), @10=(((R_WC_2*R_BC_1)+(R_WC_5*R_BC_4))+(R_WC_8*R_BC_7)), @11=(((R_WC_2*R_BC_2)+(R_WC_5*R_BC_5))+(R_WC_8*R_BC_8)), @12=(((@9*BB_v_CB_0)+(@10*BB_v_CB_1))+(@11*BB_v_CB_2)), @13=(W_om_CW_2-(((@9*B_om_CB_0)+(@10*B_om_CB_1))+(@11*B_om_CB_2))), @14=(((@5*BB_v_CB_0)+(@6*BB_v_CB_1))+(@7*BB_v_CB_2)), @15=(((@1*B_om_CB_0)+(@2*B_om_CB_1))+(@3*B_om_CB_2)), @16=(W_om_CW_0-(((@1*B_om_CB_0)+(@2*B_om_CB_1))+(@3*B_om_CB_2))), @17=(((@9*B_om_CB_0)+(@10*B_om_CB_1))+(@11*B_om_CB_2)), @18=((W_alp_CW_1-(((@5*B_alp_CB_0)+(@6*B_alp_CB_1))+(@7*B_alp_CB_2)))-((@13*@15)-(@16*@17))), @19=(((@9*B_p_CB_0)+(@10*B_p_CB_1))+(@11*B_p_CB_2)), @20=(((@5*B_om_CB_0)+(@6*B_om_CB_1))+(@7*B_om_CB_2)), @21=((W_alp_CW_2-(((@9*B_alp_CB_0)+(@10*B_alp_CB_1))+(@11*B_alp_CB_2)))-((@16*@20)-(@8*@15))), @22=(((@5*B_p_CB_0)+(@6*B_p_CB_1))+(@7*B_p_CB_2)), @23=(((@5*B_p_CB_0)+(@6*B_p_CB_1))+(@7*B_p_CB_2)), @24=(((@1*B_p_CB_0)+(@2*B_p_CB_1))+(@3*B_p_CB_2)), @25=((@16*@23)-(@8*@24)), @26=(((@9*B_p_CB_0)+(@10*B_p_CB_1))+(@11*B_p_CB_2)), @27=((@13*@24)-(@16*@26)), @28=(((@1*BB_v_CB_0)+(@2*BB_v_CB_1))+(@3*BB_v_CB_2)), @29=(((@1*B_p_CB_0)+(@2*B_p_CB_1))+(@3*B_p_CB_2)), @30=((W_alp_CW_0-(((@1*B_alp_CB_0)+(@2*B_alp_CB_1))+(@3*B_alp_CB_2)))-((@8*@17)-(@13*@20))), @31=((@8*@26)-(@13*@23)), [((((W_acc_CW_0-(((@1*B_acc_CB_0)+(@2*B_acc_CB_1))+(@3*B_acc_CB_2)))-(@4*((@8*@12)-(@13*@14))))-((@18*@19)-(@21*@22)))-((@8*@25)-(@13*@27))), ((((W_acc_CW_1-(((@5*B_acc_CB_0)+(@6*B_acc_CB_1))+(@7*B_acc_CB_2)))-(@4*((@13*@28)-(@16*@12))))-((@21*@29)-(@30*@19)))-((@13*@31)-(@16*@25))), ((((W_acc_CW_2-(((@9*B_acc_CB_0)+(@10*B_acc_CB_1))+(@11*B_acc_CB_2)))-(@4*((@16*@14)-(@8*@28))))-((@30*@22)-(@18*@29)))-((@16*@27)-(@8*@31)))]): IMU meas = fcn(cam, probe) - in B frame

kinematics.equations.f_imu = Function(f_imu:(W_p_CW[3],R_WC[3x3],WW_v_CW[3],W_om_CW[3],W_acc_CW[3],W_alp_CW[3],notchdofs[3],B_p_CB[3],R_BC[3x3],BB_v_CB[3],B_om_CB[3],B_acc_CB[3],B_alp_CB[3])->(W_p_BW[3],R_WB[3x3],WW_v_BW[3]) SXFunction): IMU measurements u = func(cam, dofs)

kinematics.equations.f_imu_meas = Function(f_imu_meas:(B_p_CB[3],R_BC[3x3],BB_v_CB[3],B_om_CB[3],B_acc_CB[3],B_alp_CB[3])->(B_om_BW[3],B_acc_BW[3]) SXFunction): Kalman filter stuff

kinematics.functions module

kinematics.functions.dummify_array(expr)

kinematics.functions.dummify_undefined_functions(expr)

kinematics.functions.sympy2casadi(sympy_expr, sympy_var, casadi_var)

kinematics.functions.to_casadi(var)

kinematics.symbols module

kinematics.symbols.get_err_pc_dot(probe)

Example derivation of err_p_B:

[In continuous time] p_B_tr_dot = p_B_dot + err_p_B_dot v_B_tr = v_B + err_v_B

err_p_B_dot = v_B_tr - v_B: = v_B + err_v_B - v_B = err_v_B

[Discretised] err_p_B_next = err_p_B + dt * err_v_B

kinematics.symbols.get_err_theta_c_dot(probe)

kinematics.symbols.quat_matrix(wxyz, dir)

kinematics.symbols.rvec_to_quat(v): returns wxyz

kinematics.transformations module

kinematics.transformations.to_world_coords(R_WB: ndarray, vec: ndarray) → ndarray