def test_no_constants(self): sys = models.multi_mass_spring_damper() constant_vals = {sm.Symbol('m0'): 1.0, sm.Symbol('c0'): 2.0, sm.Symbol('k0'): 3.0} sym_rhs = sys.eom_method.rhs() rhs = generate_ode_function(sym_rhs, sys.coordinates, sys.speeds, constant_vals) rhs2 = generate_ode_function(sym_rhs.subs(constant_vals), sys.coordinates, sys.speeds) assert np.array_equal(rhs(np.array([1.0, 2.0]), 0.0, constant_vals), rhs2(np.array([1.0, 2.0]), 0.0))
def generate_equation(self): """ New attributes: rhs : right hand side :return: """ print('symbolic dynamic: generating symbolic equations') kane = me.KanesMethod(self.frames[0], self.q, self.q_dot, self.kde) fr, frstar = kane.kanes_equations(self.loads, self.bodies) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full right_hand_side = generate_ode_function(forcing_vector, self.q, self.q_dot, [], mass_matrix=mass_matrix, specifieds=self.tau) self.kane = kane self.rhs = right_hand_side
def __construct_rhs(self): """Construct a callable function that can be used to integrate the mode. rhs = rhs(x, t, controller specifieds, parameters values) """ self.logger.debug('__construct_rhs') self.logger.debug('Use Gravity: ' + str(self.use_gravity)) self.logger.debug('Use Coordinate Limits: ' + str(self.use_coordinate_limits)) self.logger.debug('Use Viscous Joints: ' + str(self.use_viscosity)) # forces # Newton's 3rd law b = 0.05 tau = sp.Matrix(apply_generalized_force(self.Tau())) self.tau_l = sp.Matrix(apply_generalized_force(self.__tau_l)) self.tau_b = -b * sp.Matrix(apply_generalized_force(self.U())) # tau = sp.Matrix(self.Tau()) # self.tau_l = sp.Matrix(self.__tau_l) # self.tau_b = -b *sp.Matrix(self.U()) # M qdd + tau_c + tau_g = tau + tau_l + tau_b-> M qdd = forcing # f = tau_c + tau_g - tau_l - tau_b self.f = self.tau_c \ + self.use_gravity * self.tau_g \ - self.use_coordinate_limits * self.tau_l \ - self.use_viscosity * self.tau_b self.forcing = tau - self.f # substitute dq with u (required for code-gen) for i in range(0, self.forcing.shape[0]): self.forcing = self.forcing.subs(self.dq[i + 1], self.u[i + 1]) # rhs self.coordinates = sp.Matrix(self.Q()) self.speeds = sp.Matrix(self.U()) self.coordinates_derivatives = self.speeds self.specifieds = sp.Matrix(self.Tau()) self.rhs = generate_ode_function( self.forcing, self.coordinates, self.speeds, list(self.constants.keys()), mass_matrix=self.M, coordinate_derivatives=self.coordinates_derivatives, specifieds=self.specifieds)
def __init__(self): self.num_links = 5 # Number of links total_link_length = 1. total_link_mass = 1. self.ind_link_length = total_link_length / self.num_links ind_link_com_length = self.ind_link_length / 2. ind_link_mass = total_link_mass / self.num_links ind_link_inertia = ind_link_mass * (ind_link_com_length**2) # =======================# # Parameters for step() # # =======================# # Maximum number of steps before episode termination self.max_steps = 200 # For ODE integration self.dt = .0001 # Simultaion time step = 1ms self.sim_steps = 51 # Number of simulation steps in 1 learning step self.dt_step = np.linspace( 0., self.dt * self.sim_steps, num=self.sim_steps) # Learning time step = 50ms # Termination conditions for simulation self.num_steps = 0 # Step counter self.done = False # For visualisation self.viewer = None self.ax = False # Constraints for observation min_angle = -np.pi # Angle max_angle = np.pi min_omega = -10. # Angular velocity max_omega = 10. min_torque = -10. # Torque max_torque = 10. low_state_angle = np.full(self.num_links, min_angle) # Min angle low_state_omega = np.full(self.num_links, min_omega) # Min angular velocity low_state = np.append(low_state_angle, low_state_omega) high_state_angle = np.full(self.num_links, max_angle) # Max angle high_state_omega = np.full(self.num_links, max_omega) # Max angular velocity high_state = np.append(high_state_angle, high_state_omega) low_action = np.full(self.num_links, min_torque) # Min torque high_action = np.full(self.num_links, max_torque) # Max torque self.action_space = spaces.Box(low=low_action, high=high_action) self.observation_space = spaces.Box(low=low_state, high=high_state) # Minimum reward self.min_reward = -(max_angle**2 + .1 * max_omega**2 + .001 * max_torque**2) * self.num_links # Seeding self.seed() # ==============# # Orientations # # ==============# self.inertial_frame = ReferenceFrame('I') self.link_frame = [] self.theta = [] for i in range(self.num_links): temp_angle_name = "theta{}".format(i + 1) temp_link_name = "L{}".format(i + 1) self.theta.append(dynamicsymbols(temp_angle_name)) self.link_frame.append(ReferenceFrame(temp_link_name)) if i == 0: # First link self.link_frame[i].orient( self.inertial_frame, 'Axis', (self.theta[i], self.inertial_frame.z)) else: # Second link, third link... self.link_frame[i].orient( self.link_frame[i - 1], 'Axis', (self.theta[i], self.link_frame[i - 1].z)) # =================# # Point Locations # # =================# # --------# # Joints # # --------# self.link_length = [] self.link_joint = [] for i in range(self.num_links): temp_link_length_name = "l_L{}".format(i + 1) temp_link_joint_name = "A{}".format(i) self.link_length.append(symbols(temp_link_length_name)) self.link_joint.append(Point(temp_link_joint_name)) if i > 0: # Set position started from link2, then link3, link4... self.link_joint[i].set_pos( self.link_joint[i - 1], self.link_length[i - 1] * self.link_frame[i - 1].y) # --------------------------# # Centre of mass locations # # --------------------------# self.link_com_length = [] self.link_mass_centre = [] for i in range(self.num_links): temp_link_com_length_name = "d_L{}".format(i + 1) temp_link_mass_centre_name = "L{}_o".format(i + 1) self.link_com_length.append(symbols(temp_link_com_length_name)) self.link_mass_centre.append(Point(temp_link_mass_centre_name)) self.link_mass_centre[i].set_pos( self.link_joint[i], self.link_com_length[i] * self.link_frame[i].y) # ===========================================# # Define kinematical differential equations # # ===========================================# self.omega = [] self.kinematical_differential_equations = [] self.time = symbols('t') for i in range(self.num_links): temp_omega_name = "omega{}".format(i + 1) self.omega.append(dynamicsymbols(temp_omega_name)) self.kinematical_differential_equations.append( self.omega[i] - self.theta[i].diff(self.time)) # ====================# # Angular Velocities # # ====================# for i in range(self.num_links): if i == 0: # First link self.link_frame[i].set_ang_vel( self.inertial_frame, self.omega[i] * self.inertial_frame.z) else: # Second link, third link... self.link_frame[i].set_ang_vel( self.link_frame[i - 1], self.omega[i] * self.link_frame[i - 1].z) # ===================# # Linear Velocities # # ===================# for i in range(self.num_links): if i == 0: # First link self.link_joint[i].set_vel(self.inertial_frame, 0) else: # Second link, third link... self.link_joint[i].v2pt_theory(self.link_joint[i - 1], self.inertial_frame, self.link_frame[i - 1]) self.link_mass_centre[i].v2pt_theory(self.link_joint[i], self.inertial_frame, self.link_frame[i]) # ======# # Mass # # ======# self.link_mass = [] for i in range(self.num_links): temp_link_mass_name = "m_L{}".format(i + 1) self.link_mass.append(symbols(temp_link_mass_name)) # =========# # Inertia # # =========# self.link_inertia = [] self.link_inertia_dyadic = [] self.link_central_inertia = [] for i in range(self.num_links): temp_link_inertia_name = "I_L{}z".format(i + 1) self.link_inertia.append(symbols(temp_link_inertia_name)) self.link_inertia_dyadic.append( inertia(self.link_frame[i], 0, 0, self.link_inertia[i])) self.link_central_inertia.append( (self.link_inertia_dyadic[i], self.link_mass_centre[i])) # ==============# # Rigid Bodies # # ==============# self.link = [] for i in range(self.num_links): temp_link_name = "link{}".format(i + 1) self.link.append( RigidBody(temp_link_name, self.link_mass_centre[i], self.link_frame[i], self.link_mass[i], self.link_central_inertia[i])) # =========# # Gravity # # =========# self.g = symbols('g') self.link_grav_force = [] for i in range(self.num_links): self.link_grav_force.append( (self.link_mass_centre[i], -self.link_mass[i] * self.g * self.inertial_frame.y)) # ===============# # Joint Torques # # ===============# self.link_joint_torque = [] self.link_torque = [] for i in range(self.num_links): temp_link_joint_torque_name = "T_a{}".format(i + 1) self.link_joint_torque.append( dynamicsymbols(temp_link_joint_torque_name)) for i in range(self.num_links): if (i + 1) == self.num_links: # Last link self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z)) else: # Other links self.link_torque.append( (self.link_frame[i], self.link_joint_torque[i] * self.inertial_frame.z - self.link_joint_torque[i + 1] * self.inertial_frame.z)) # =====================# # Equations of Motion # # =====================# self.coordinates = [] self.speeds = [] self.loads = [] self.bodies = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.loads.append(self.link_grav_force[i]) self.loads.append(self.link_torque[i]) self.bodies.append(self.link[i]) self.kane = KanesMethod(self.inertial_frame, self.coordinates, self.speeds, self.kinematical_differential_equations) self.fr, self.frstar = self.kane.kanes_equations( self.bodies, self.loads) self.mass_matrix = self.kane.mass_matrix_full self.forcing_vector = self.kane.forcing_full # =============================# # List the symbolic arguments # # =============================# # -----------# # Constants # # -----------# self.constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.constants.append(self.link_length[i]) self.constants.append(self.link_com_length[i]) self.constants.append(self.link_mass[i]) self.constants.append(self.link_inertia[i]) self.constants.append(self.g) # --------------# # Time Varying # # --------------# self.coordinates = [] self.speeds = [] self.specified = [] for i in range(self.num_links): self.coordinates.append(self.theta[i]) self.speeds.append(self.omega[i]) self.specified.append(self.link_joint_torque[i]) # =======================# # Generate RHS Function # # =======================# self.right_hand_side = generate_ode_function( self.forcing_vector, self.coordinates, self.speeds, self.constants, mass_matrix=self.mass_matrix, specifieds=self.specified) # ==============================# # Specify Numerical Quantities # # ==============================# self.x = np.zeros(self.num_links * 2) self.x[:self.num_links] = deg2rad(2.0) self.numerical_constants = [] for i in range(self.num_links): if (i + 1) != self.num_links: self.numerical_constants.append(self.ind_link_length) self.numerical_constants.append(ind_link_com_length) self.numerical_constants.append(ind_link_mass) self.numerical_constants.append(ind_link_inertia) self.numerical_constants.append(9.81)
forcing_vector = trigsimp(kane.forcing_full) ### Transform symbolic equations of motion to Python functions and evaluate using numerical integration to solve the ordinary differential initial value problem # List all constants used by the EoM: lengths, angles, masses, inertias (i.e. anything defined as "symbols"). Order doesn't matter. constants = [s, mass_i, mass_j, inertia_i, inertia_j] # NOTE add in R, phi1, phi2 when make extra points. # A list called "specified" holds Exogenous inputs, i.e. inputs that don't rely on information of system (i.e. externally applied forces or torques). The Tutorial includes them, and defined them after Forces, including them in "loads" list above. I tried to neglect these and change remaining code accordingly, but without success. Instead we just create external loads as done in tutorial and set them to 0 for all time. specified = [x_F, y_F, i_T, j_T] # create the function right_hand_side = generate_ode_function(forcing_vector, coordinates, speeds, constants, mass_matrix=mass_matrix, specifieds=specified) ## Set initial conditions, parameter values and time array # Initialize q and u # righthandside function creates state vector x holding q and u x0 = zeros(8) x0[2] = deg2rad(90.0) # i.e. -pi/4 , assuming CCW+, so will be right cube. x0[3] = deg2rad(0.0) # i.e. +pi/4 , assuming CCW+, so will be left cube. # Assign numerical values to all constants (and any exogenous inputs) numerical_constants = array([ 0.05, # side length 50mm 0.25, # 250g
def closed_loop_ode_func(self, time, reference_noise, platform_acceleration): """Returns a function that evaluates the continous closed loop system first order ODEs. Parameters ---------- time : ndarray, shape(N,) The monotonically increasing time values. reference_noise : ndarray, shape(N, 4) The reference noise vector at each time. platform_acceleration : ndarray, shape(N,) The acceleration of the platform at each time. Returns ------- rhs : function A function that evaluates the right hand side of the first order ODEs in a form easily used with scipy.integrate.odeint. args : dictionary A dictionary containing the model constant values and the controller function. """ controls = np.empty(3, dtype=float) all_sigs = np.hstack((reference_noise, np.expand_dims(platform_acceleration, 1))) # NOTE : copy and assume_sorted do not seem to speed up the actual # interpolation call. assume_sorted was introduced in SciPy 0.14. interp_func = interp1d(time, all_sigs, axis=0, copy=False, assume_sorted=True) if self.unscaled_gain is None: s = 1.0 else: s = self.unscaled_gain def controller(x, t): """ x = [theta_a, theta_h, omega_a, omega_h] r = [a, T_a, T_h] """ # TODO : This interpolation call is the most expensive thing # when running odeint. Seems like InterpolatedUnivariateSpline # may be faster, but it doesn't supprt a multidimensional y. if t > time[-1]: result = interp_func(time[-1]) else: result = interp_func(t) controls[0] = result[-1] controls[1:] = np.dot(s * self.gain_scale_factors, result[:-1] - x) return controls rhs = generate_ode_function(self.forcing_vector_full, list(self.coordinates.values()), list(self.speeds.values()), list(self.parameters.values()), mass_matrix=self.mass_matrix_full, specifieds=list(self.specified.values())[-3:], generator='cython') return rhs, controller, np.array(list(self.open_loop_par_map.values()))
speeds = [u, v, w, phi1d, theta1d, psi1d] kane = me.KanesMethod(N, coordinates, speeds, kinematical_differential_equations) loads = [force, torque] bodies = [body] fr, frstar = kane.kanes_equations(bodies, loads) constants = [I_xx, I_yy, I_zz, mass] specified = [fx, fy, fz, mx, my, mz] # External force/torque right_hand_side = generate_ode_function(kane.forcing_full, coordinates, speeds, constants, mass_matrix=kane.mass_matrix_full, specifieds=specified) def simulate(t, force_torque, I_xx, I_yy, I_zz, mass, initial_coordinates=[0, 0, 0, 0, 0, 0], initial_speeds=[0, 0, 0, 0, 0, 0]) -> pd.DataFrame: """ Simulate a rigid body in 6 degrees of freedom under the influence of external torque and forces applied in the body reference frame
g] # Time Varying # ------------ coordinates = [theta1, theta2, theta3] speeds = [omega1, omega2, omega3] specified = [ankle_torque, knee_torque, hip_torque] # Generate RHS Function # ===================== right_hand_side = generate_ode_function(forcing_vector, coordinates, speeds, constants, mass_matrix=mass_matrix, specifieds=specified) # Specify Numerical Quantities # ============================ x0 = zeros(6) x0[:3] = deg2rad(2.0) # taken from male1.txt in yeadon (maybe I should use the values in Winters). numerical_constants = array([0.611, # lower_leg_length [m] 0.387, # lower_leg_com_length [m] 6.769, # lower_leg_mass [kg] 0.101, # lower_leg_inertia [kg*m^2] 0.424, # upper_leg_length [m]
# Create a list of the numerical values which have the same order as the # list of symbolic parameters. param_vals = [link_length for x in l] + \ [particle_mass for x in m_bob] + \ [link_mass for x in m_link] + \ [link_ixx for x in list(Ixx)] + \ [link_iyy for x in list(Iyy)] + \ [link_izz for x in list(Izz)] + \ [9.8] # A function that evaluates the right hand side of the set of first order # ODEs can be generated. print("Generating numeric right hand side.") right_hand_side = generate_ode_function(kane.forcing_full, q, u, param_syms, mass_matrix=kane.mass_matrix_full) # To simulate the system, a time vector and initial conditions for the # system's states is required. t = linspace(0.0, 60.0, num=600) x0 = hstack((ones(6) * radians(10.0), zeros(6))) print("Integrating equations of motion.") state_trajectories = odeint(right_hand_side, x0, t, args=({ 'constants': dict(zip(param_syms, param_vals))
def closed_loop_ode_func(system, time, set_point, gain_matrix, lateral_force): """Returns a function that evaluates the continous closed loop system first order ODEs. Parameters ---------- system : tuple, len(6) The output of the symbolic EoM generator. time : ndarray, shape(M,) The monotonically increasing time values that set_point : ndarray, shape(n,) The set point for the controller. gain_matrix : ndarray, shape((n - 1) / 2, n) The gain matrix that computes the optimal joint torques given the system state. lateral_force : ndarray, shape(M,) The applied lateral force at each time point. This will be linearly interpolated for time points other than those in time. Returns ------- rhs : function A function that evaluates the right hand side of the first order ODEs in a form easily used with odeint. args : dictionary A dictionary containing the model constant values and the controller function. """ # TODO : It will likely be useful to allow more inputs: noise on the # equilibrium point (i.e. sensor noise) and noise on the joint torques. # 10 cycles /sec * 2 pi rad / cycle interp_func = interp1d(time, lateral_force) def controller(x, t): joint_torques = np.dot(gain_matrix, set_point - x) if t > time[-1]: lateral_force = interp_func(time[-1]) else: lateral_force = interp_func(t) return np.hstack((joint_torques, lateral_force)) mass_matrix = system[0] forcing = system[1] constants = system[2] coordinates = system[3] speeds = system[4] specifieds = system[5] rhs = generate_ode_function(forcing, coordinates, speeds, constants, mass_matrix=mass_matrix, specifieds=specifieds, generator='cython') args = (controller, constants_dict(constants)) return rhs, args
kane = KanesMethod(N, val_coords, val_speeds, kd) print(kane.q) print(kane.u) # force list fr, frst = kane.kanes_equations(bodys, force_ls) mass_matrix = kane.mass_matrix_full forcing_vector = kane.forcing_full constants = [g, f] # todo add a constant mag multiplied by step print(forcing_vector) print(mass_matrix) # ##----------------- # solve # peicewise: solve mag sp = [f] const_val = [g_a, engine_obj.force] r_h_s = generate_ode_function(forcing_vector, val_coords, val_speeds, constants, mass_matrix=mass_matrix, specifieds=sp) # simulate frames_per_sec = 60 final_time = 5 t_range = np.linspace(0.0, final_time, final_time * frames_per_sec) y = odeint(r_h_s, np.array([0, 0, 0]), t_range, args=(np.array([0]), const_val))
# looks like some of the derivative terms were not substituted correctly forcing_vector = KM.forcing.subs(kd_map) derivative_generator = (expr.atoms(Derivative) for expr in find_dynamicsymbols(forcing_vector)) if frozenset().union(*(derivative_generator)): forcing_vector = forcing_vector.subs(kd_map) #List apprehension kdd = KM.kindiffdict() coord_derivs = Matrix([kdd[c.diff()] for c in KM.q]) #RHS = mass_matrix.LU_solve(forcing_vector) RHS = generate_ode_function(forcing_vector, KM.q, KM.u, constants, mass_matrix=KM.mass_matrix, coordinate_derivatives=coord_derivs, constants_arg_type='dictionary') # Initial conditions, time and constant values # q = [q1,theta,q2,q1dot,omega,q2dot] q0 = np.deg2rad((45, 0, 60, 0, 0.5, 0)) constants = { m: 7.5, g: 10, r: 0.2, bb: 0.5, ob: 0.4, ta: 0.18, d1: 0.62,
particle_mass = 5.0 # kg particle_radius = 1.0 # meters # Create a list of the numerical values which have the same order as the # list of symbolic parameters. param_vals = [link_length for x in l] + \ [particle_mass for x in m_bob] + \ [link_mass for x in m_link] + \ [link_ixx for x in list(Ixx)] + \ [link_iyy for x in list(Iyy)] + \ [link_izz for x in list(Izz)] + \ [9.8] # A function that evaluates the right hand side of the set of first order # ODEs can be generated. print("Generating numeric right hand side.") right_hand_side = generate_ode_function(kane.forcing_full, q, u, param_syms, mass_matrix=kane.mass_matrix_full) # To simulate the system, a time vector and initial conditions for the # system's states is required. t = linspace(0.0, 60.0, num=600) x0 = hstack((ones(6) * radians(10.0), zeros(6))) print("Integrating equations of motion.") state_trajectories = odeint(right_hand_side, x0, t, args=({'constants': dict(zip(param_syms, param_vals))},)) print("Integration done.")