f_gravity_world = matrix([0, -m * g, 0]).T f_external_world = f_gravity_world construct_sympy_expressions = False if construct_sympy_expressions: print "Constructing sympy symbols..." d_expr = sympy.Symbol("d") alpha_expr = sympy.Symbol("alpha") beta_expr = sympy.Symbol("beta") gamma_expr = sympy.Symbol("gamma") m_expr = sympy.Symbol("m") I_expr, I_expr_entries = sympyutils.construct_matrix_and_entries( "I", (3, 3)) f_external_expr, f_external_expr_entries = sympyutils.construct_matrix_and_entries( "f_e", (3, 1)) t_expr = sympy.Symbol("t") p_z_expr = sympy.physics.mechanics.dynamicsymbols("p_z") p_y_expr = sympy.physics.mechanics.dynamicsymbols("p_y") p_x_expr = sympy.physics.mechanics.dynamicsymbols("p_x") theta_expr = sympy.physics.mechanics.dynamicsymbols("theta") psi_expr = sympy.physics.mechanics.dynamicsymbols("psi") phi_expr = sympy.physics.mechanics.dynamicsymbols("phi") q_expr = sympy.Matrix( [p_z_expr, p_y_expr, p_x_expr, theta_expr, psi_expr, phi_expr]) q_dot_expr = q_expr.diff(t_expr)
f_external_world = f_gravity_world construct_sympy_expressions = False if construct_sympy_expressions: print "Constructing sympy symbols..." d_expr = sympy.Symbol("d") alpha_expr = sympy.Symbol("alpha") beta_expr = sympy.Symbol("beta") gamma_expr = sympy.Symbol("gamma") m_expr = sympy.Symbol("m") I_expr, I_expr_entries = sympyutils.construct_matrix_and_entries("I",(3,3)) f_external_expr, f_external_expr_entries = sympyutils.construct_matrix_and_entries("f_e",(3,1)) t_expr = sympy.Symbol("t") p_z_expr = sympy.physics.mechanics.dynamicsymbols("p_z") p_y_expr = sympy.physics.mechanics.dynamicsymbols("p_y") p_x_expr = sympy.physics.mechanics.dynamicsymbols("p_x") theta_expr = sympy.physics.mechanics.dynamicsymbols("theta") psi_expr = sympy.physics.mechanics.dynamicsymbols("psi") phi_expr = sympy.physics.mechanics.dynamicsymbols("phi") q_expr = sympy.Matrix([p_z_expr,p_y_expr,p_x_expr,theta_expr,psi_expr,phi_expr]) q_dot_expr = q_expr.diff(t_expr) u_expr, u_expr_entries = sympyutils.construct_matrix_and_entries("u",(4,1))
I = m * d**2.0 # moment of intertia f_gravity_world = matrix([-m * g, 0]).T f_external_world = f_gravity_world construct_sympy_expressions = False if construct_sympy_expressions: print "Constructing sympy symbols..." d_expr = sympy.Symbol("d") m_expr = sympy.Symbol("m") I_expr = sympy.Symbol("I") f_external_expr, f_external_expr_entries = sympyutils.construct_matrix_and_entries( "f_e", (2, 1)) t_expr = sympy.Symbol("t") p_y_expr = sympy.physics.mechanics.dynamicsymbols("p_y") p_x_expr = sympy.physics.mechanics.dynamicsymbols("p_x") theta_expr = sympy.physics.mechanics.dynamicsymbols("theta") q_expr = sympy.Matrix([p_y_expr, p_x_expr, theta_expr]) q_dot_expr = q_expr.diff(t_expr) u_expr, u_expr_entries = sympyutils.construct_matrix_and_entries( "u", (2, 1)) print "Constructing sympy expressions..."