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.")
