def test_accelerations():
(mass_matrix, forcing_vector, kane, constants, coordinates, speeds,
specified, visualization_frames, ground, origin) = \
derive.derive_equations_of_motion()
pydy_rhs = generate_ode_function(mass_matrix, forcing_vector, constants,
coordinates, speeds,
specified=specified)
coordinate_values = np.random.random(9)
speed_values = np.random.random(9)
specified_values = np.random.random(9)
constant_map = simulate.load_constants(constants,
'data/example_constants.yml')
args = {'constants': np.array(constant_map.values()),
'specified': specified_values}
x = np.hstack((coordinate_values, speed_values))
pydy_xdot = pydy_rhs(x, 0.0, args)
with open('data/example_constants.yml', 'r') as f:
constants_dict = yaml.load(f)
constants_dict = simulate.map_values_to_autolev_symbols(constants_dict)
accelerations, grfs, animation_data = \
autolev_rhs(coordinate_values, speed_values, specified_values,
constants_dict)
testing.assert_allclose(pydy_xdot[9:], accelerations)
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))
rhs = generate_ode_function(*system, generator='cython')
args = {
'constants': np.array(constants_dict(system[2]).values()),
'specified': controller
}
return rhs, args
def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling,
q_ind=[position],
u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
constants = (mass, stiffness, damping, gravity)
coordinates = (position, )
speeds = (speed, )
system = (mass_matrix, forcing_vector, constants, coordinates, speeds)
# integrate eoms
evaluate_ode = generate_ode_function(*system)
x0 = array([0.1, -1.0])
args = {'constants': array([1.0, 1.0, 0.2, 9.8])}
t = linspace(0.0, 10.0, 100)
y = odeint(evaluate_ode, x0, t, args=(args, ))
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json([position, speed], list(constants), y,
args['constants'])
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
def test_create_static_html():
# derive simple system
mass, stiffness, damping, gravity = symbols('m, k, c, g')
position, speed = me.dynamicsymbols('x v')
positiond = me.dynamicsymbols('x', 1)
ceiling = me.ReferenceFrame('N')
origin = me.Point('origin')
origin.set_vel(ceiling, 0)
center = origin.locatenew('center', position * ceiling.x)
center.set_vel(ceiling, speed * ceiling.x)
block = me.Particle('block', center, mass)
kinematic_equations = [speed - positiond]
total_force = mass * gravity - stiffness * position - damping * speed
forces = [(center, total_force * ceiling.x)]
particles = [block]
kane = me.KanesMethod(ceiling, q_ind=[position], u_ind=[speed],
kd_eqs=kinematic_equations)
kane.kanes_equations(forces, particles)
mass_matrix = kane.mass_matrix_full
forcing_vector = kane.forcing_full
constants = (mass, stiffness, damping, gravity)
coordinates = (position,)
speeds = (speed,)
system = (mass_matrix, forcing_vector, constants, coordinates, speeds)
# integrate eoms
evaluate_ode = generate_ode_function(*system)
x0 = array([0.1, -1.0])
args = {'constants': array([1.0, 1.0, 0.2, 9.8])}
t = linspace(0.0, 10.0, 100)
y = odeint(evaluate_ode, x0, t, args=(args,))
# create visualization
sphere = Sphere()
viz_frame = VisualizationFrame(ceiling, block, sphere)
scene = Scene(ceiling, origin, viz_frame)
scene.generate_visualization_json([position, speed], list(constants), y,
args['constants'])
# test static dir creation
scene.create_static_html(overwrite=True)
assert os.path.exists('static')
assert os.path.exists('static/index.html')
assert os.path.exists('static/data.json')
# test static dir deletion
scene.remove_static_html(force=True)
assert not os.path.exists('static')
from sympy.physics.mechanics import kinetic_energy
from numpy import array, linspace, deg2rad, ones, concatenate
from sympy import lambdify, atan, atan2, Matrix, simplify
from sympy.mpmath import norm
from pydy.codegen.code import generate_ode_function
from scipy.integrate import odeint
import math
import numpy as np
import single_pendulum_setup as sp
import double_pendulum_particle_setup as dp
import triple_pendulum_setup as tp
# Specify Numerical Quantities
# ============================
right_hand_side = generate_ode_function(tp.mass_matrix_full, tp.forcing_vector_full,
tp.constants, tp.coordinates,
tp.speeds, tp.specified)
initial_coordinates = [0.01, -0.1, 0.01]
initial_speeds = [-0.0,-0.0, -0.0]
x0 = concatenate((initial_coordinates, initial_speeds), axis=1)
# taken from male1.txt in yeadon (maybe I should use the values in Winters).
numerical_constants = array([0.5,
0.5,
0.75,
0.75,
1.0,
1.0,
9.81])
# Constants
# ---------
constants = {l: 10.0, m: 10.0, g: 9.81}
# Time-varying
# ------------
coordinates = [q1, q2]
speeds = [u1, u2]
# Generate function that returns state derivatives
# ================================================
xdot_function = generate_ode_function(mass_matrix, forcing_vector,
constants.keys(), coordinates, speeds)
# Specify numerical quantities
# ============================
initial_coordinates = [1.0, 0.0]
initial_speeds = [0.0, 0.0]
x0 = concatenate((initial_coordinates, initial_speeds), axis=1)
args = {'constants': constants.values()}
# Simulate
# ========
frames_per_sec = 60
final_time = 5.0
def run_benchmark(max_num_links, num_time_steps=1000):
"""Runs the n link pendulum derivation, code generation, and integration
for each n up to the max number provided and generates a plot of the
results."""
methods = ['lambdify', 'theano', 'cython']
link_numbers = range(1, max_num_links + 1)
derivation_times = zeros(len(link_numbers))
integration_times = zeros((max_num_links, len(methods)))
code_generation_times = zeros_like(integration_times)
for j, n in enumerate(link_numbers):
title = "Pendulum with {} links.".format(n)
print(title)
print('=' * len(title))
start = time.time()
results = \
generate_n_link_pendulum_on_cart_equations_of_motion(n, cart_force=False)
derivation_times[j] = time.time() - start
print('The derivation took {:1.5f} seconds.\n'.format(derivation_times[j]))
# Define the numerical values: parameters, time, and initial conditions
arm_length = 1. / n
bob_mass = 0.01 / n
parameter_vals = [9.81, 0.01 / n]
for i in range(n):
parameter_vals += [arm_length, bob_mass]
# odeint arguments
x0 = hstack((0, pi / 2 * ones(len(results[3]) - 1), 1e-3 *
ones(len(results[4]))))
args = {'constants': dict(zip(results[2], array(parameter_vals)))}
t = linspace(0, 10, num_time_steps)
for k, method in enumerate(methods):
subtitle = "Generating with {} method.".format(method)
print(subtitle)
print('-' * len(subtitle))
start = time.time()
evaluate_ode = generate_ode_function(*results,
generator=method)
code_generation_times[j, k] = time.time() - start
print('The code generation took {:1.5f} seconds.'.format(
code_generation_times[j, k]))
start = time.time()
odeint(evaluate_ode, x0, t, args=(args,))
integration_times[j, k] = time.time() - start
print('ODE integration took {:1.5f} seconds.\n'.format(
integration_times[j, k]))
del results, evaluate_ode
# clean up the cython crud
files = glob.glob('multibody_system*')
for f in files:
os.remove(f)
shutil.rmtree('build')
# plot the results
fig, ax = plt.subplots(3, 1, sharex=True)
ax[0].plot(link_numbers, derivation_times)
ax[0].set_title('Symbolic Derivation Time')
ax[1].plot(link_numbers, code_generation_times)
ax[1].set_title('Code Generation Time')
ax[1].legend(methods, loc=2)
ax[2].plot(link_numbers, integration_times)
ax[2].set_title('Integration Time')
ax[2].legend(methods, loc=2)
for a in ax.flatten():
a.set_ylabel('Time [s]')
ax[-1].set_xlabel('Number of links')
plt.tight_layout()
fig.savefig('benchmark-results.png')
def test_sim_discrete_equate():
"""This ensures that the rhs function evaluates the same as the symbolic
closed loop form."""
num_links = 1
system = model.n_link_pendulum_on_cart(num_links,
cart_force=True,
joint_torques=True,
spring_damper=True)
mass_matrix = system[0]
forcing_vector = system[1]
constants_syms = system[2]
coordinates_syms = system[3]
speeds_syms = system[4]
specified_inputs_syms = system[5] # last entry is lateral force
states_syms = coordinates_syms + speeds_syms
state_derivs_syms = model.state_derivatives(states_syms)
gains = simulate.compute_controller_gains(num_links)
equilibrium_point = np.zeros(len(states_syms))
lateral_force = np.random.random(1)
def specified(x, t):
joint_torques = np.dot(gains, equilibrium_point - x)
return np.hstack((joint_torques, lateral_force))
rhs = generate_ode_function(*system)
args = {
'constants': simulate.constants_dict(constants_syms).values(),
'specified': specified
}
state_values = np.random.random(len(states_syms))
state_deriv_values = rhs(state_values, 0.0, args)
control_dict, gain_syms, equil_syms = \
model.create_symbolic_controller(states_syms,
specified_inputs_syms[:-1])
eq_dict = dict(zip(equil_syms, len(states_syms) * [0]))
closed = model.symbolic_constraints(mass_matrix, forcing_vector,
states_syms, control_dict, eq_dict)
xdot_expr = sym.solve(closed, state_derivs_syms)
xdot_expr = sym.Matrix([xdot_expr[xd] for xd in state_derivs_syms])
val_map = dict(zip(states_syms, state_values))
val_map.update(simulate.constants_dict(constants_syms))
val_map.update(dict(zip(gain_syms, gains.flatten())))
val_map[specified_inputs_syms[-1]] = lateral_force
sym_sol = np.array([x for x in xdot_expr.subs(val_map).evalf()],
dtype=float)
np.testing.assert_allclose(state_deriv_values, sym_sol)
# Now let's see if the discretized version gives a simliar answer if h
# is small enough.
collocator = dc.ConstraintCollocator(closed, states_syms, 10, 0.01)
dclosed = collocator.discrete_eom
xi = collocator.current_discrete_state_symbols
xp = collocator.previous_discrete_state_symbols
si = collocator.current_discrete_specified_symbols
h = collocator.time_interval_symbol
euler_formula = [(i - p) / h for i, p in zip(xi, xp)]
xdot_expr = sym.solve(dclosed, euler_formula)
xdot_expr = sym.Matrix([xdot_expr[xd] for xd in euler_formula])
val_map = dict(zip(xi, state_values))
val_map.update(simulate.constants_dict(constants_syms))
val_map.update(dict(zip(gain_syms, gains.flatten())))
val_map[si[0]] = lateral_force
sym_sol = np.array([x for x in xdot_expr.subs(val_map).evalf()],
dtype=float)
np.testing.assert_allclose(state_deriv_values, sym_sol)
def run_benchmark(max_num_links, num_time_steps=1000):
"""Runs the n link pendulum derivation, code generation, and integration
for each n up to the max number provided and generates a plot of the
results."""
methods = ['lambdify', 'theano', 'cython']
link_numbers = range(1, max_num_links + 1)
derivation_times = zeros(len(link_numbers))
integration_times = zeros((max_num_links, len(methods)))
code_generation_times = zeros_like(integration_times)
for j, n in enumerate(link_numbers):
title = "Pendulum with {} links.".format(n)
print(title)
print('=' * len(title))
start = time.time()
results = \
generate_n_link_pendulum_on_cart_equations_of_motion(n, cart_force=False)
derivation_times[j] = time.time() - start
print('The derivation took {:1.5f} seconds.\n'.format(
derivation_times[j]))
# Define the numerical values: parameters, time, and initial conditions
arm_length = 1. / n
bob_mass = 0.01 / n
parameter_vals = [9.81, 0.01 / n]
for i in range(n):
parameter_vals += [arm_length, bob_mass]
# odeint arguments
x0 = hstack((0, pi / 2 * ones(len(results[3]) - 1),
1e-3 * ones(len(results[4]))))
args = {'constants': array(parameter_vals)}
t = linspace(0, 10, num_time_steps)
for k, method in enumerate(methods):
subtitle = "Generating with {} method.".format(method)
print(subtitle)
print('-' * len(subtitle))
start = time.time()
evaluate_ode = generate_ode_function(*results, generator=method)
code_generation_times[j, k] = time.time() - start
print('The code generation took {:1.5f} seconds.'.format(
code_generation_times[j, k]))
start = time.time()
odeint(evaluate_ode, x0, t, args=(args, ))
integration_times[j, k] = time.time() - start
print('ODE integration took {:1.5f} seconds.\n'.format(
integration_times[j, k]))
del results, evaluate_ode
# clean up the cython crud
files = glob.glob('multibody_system*')
for f in files:
os.remove(f)
shutil.rmtree('build')
# plot the results
fig, ax = plt.subplots(3, 1, sharex=True)
ax[0].plot(link_numbers, derivation_times)
ax[0].set_title('Symbolic Derivation Time')
ax[1].plot(link_numbers, code_generation_times)
ax[1].set_title('Code Generation Time')
ax[1].legend(methods, loc=2)
ax[2].plot(link_numbers, integration_times)
ax[2].set_title('Integration Time')
ax[2].legend(methods, loc=2)
for a in ax.flatten():
a.set_ylabel('Time [s]')
ax[-1].set_xlabel('Number of links')
plt.tight_layout()
fig.savefig('benchmark-results.png')
import matplotlib.pyplot as plt
import scipy.integrate as integrate
from scipy.integrate import odeint
from scipy.linalg import solve_continuous_are
from pydy.codegen.code import generate_ode_function
import pickle
import matplotlib.animation as animation
from double_pendulum_setup import theta1, theta2, ankle, leg_length, waist, omega1, omega2, ankle_torque, waist_torque, coordinates, speeds, kane, mass_matrix, forcing_vector, specified, parameter_dict, constants, numerical_constants, numerical_specified
#from utils import controllable
init_vprinting()
rcParams['figure.figsize'] = (14.0, 6.0)
right_hand_side = generate_ode_function(mass_matrix, forcing_vector,
constants,
coordinates, speeds, specified)
#Initial Conditions for speeds and positions
x0 = zeros(4)
x0[0] = deg2rad(2.0)
x0[1] = deg2rad(-2.0)
args = {'constants': numerical_constants,
'specified': numerical_specified}
frames_per_sec = 60
final_time = 5.0
t = linspace(0.0, final_time, final_time*frames_per_sec)
# ---------
constants = {l: 10.0, m: 10.0, g: 9.81}
# Time-varying
# ------------
coordinates = [q1, q2]
speeds = [u1, u2]
# Generate function that returns state derivatives
# ================================================
xdot_function = generate_ode_function(mass_matrix, forcing_vector,
constants.keys(), coordinates, speeds)
# Specify numerical quantities
# ============================
initial_coordinates = [1.0, 0.0]
initial_speeds = [0.0, 0.0]
x0 = concatenate((initial_coordinates, initial_speeds), axis=1)
args = {'constants': constants.values()}
# Simulate
# ========
torso_com_length, torso_mass, torso_inertia, 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(mass_matrix, forcing_vector, constants,
coordinates, speeds, specified)
# Specify Numerical Quantities
# ============================
initial_coordinates = deg2rad(5.0) * array([-1, 1, -1])
initial_speeds = deg2rad(-5.0) * ones(len(speeds))
x0 = concatenate((initial_coordinates, initial_speeds), axis=1)
# 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]
current_gain = f_gains(current_percent_gait_cycle) # shape(6, 12)
x = state_sign * x
s0 = state_sign * s0
joint_torques = np.squeeze(np.dot(current_gain, (s0[state_indices] - x[state_indices])))
return m0 + specified_sign * np.hstack(([0.0, 0.0, 0.0], joint_torques[control_indices]))
# Generate the system.
(mass_matrix, forcing_vector, kane, constants, coordinates, speeds,
specified, visualization_frames, ground, origin) = derive.derive_equations_of_motion()
rhs = generate_ode_function(mass_matrix, forcing_vector,
constants, coordinates, speeds,
specified=specified, generator='cython')
# Get all simulation and model parameters.
model_constants_path = os.path.join(os.path.split(pygait2d.__file__)[0], '../data/example_constants.yml')
constant_values = simulate.load_constants(model_constants_path)
args = {'constants': np.array([constant_values[c] for c in constants]),
'specified': combined_controller}
time_vector = np.linspace(0.0, 0.5, num=1000)
mean_of_gait_cycles = gait_data.gait_cycles.mean(axis='items')
initial_conditions = np.zeros(18)
def test_sim_discrete_equate():
"""This ensures that the rhs function evaluates the same as the symbolic
closed loop form."""
num_links = 1
system = model.n_link_pendulum_on_cart(num_links, cart_force=True,
joint_torques=True,
spring_damper=True)
mass_matrix = system[0]
forcing_vector = system[1]
constants_syms = system[2]
coordinates_syms = system[3]
speeds_syms = system[4]
specified_inputs_syms = system[5] # last entry is lateral force
states_syms = coordinates_syms + speeds_syms
state_derivs_syms = model.state_derivatives(states_syms)
gains = simulate.compute_controller_gains(num_links)
equilibrium_point = np.zeros(len(states_syms))
lateral_force = np.random.random(1)
def specified(x, t):
joint_torques = np.dot(gains, equilibrium_point - x)
return np.hstack((joint_torques, lateral_force))
rhs = generate_ode_function(*system)
args = {'constants': simulate.constants_dict(constants_syms).values(),
'specified': specified}
state_values = np.random.random(len(states_syms))
state_deriv_values = rhs(state_values, 0.0, args)
control_dict, gain_syms, equil_syms = \
model.create_symbolic_controller(states_syms,
specified_inputs_syms[:-1])
eq_dict = dict(zip(equil_syms, len(states_syms) * [0]))
closed = model.symbolic_constraints(mass_matrix, forcing_vector,
states_syms, control_dict, eq_dict)
xdot_expr = sym.solve(closed, state_derivs_syms)
xdot_expr = sym.Matrix([xdot_expr[xd] for xd in state_derivs_syms])
val_map = dict(zip(states_syms, state_values))
val_map.update(simulate.constants_dict(constants_syms))
val_map.update(dict(zip(gain_syms, gains.flatten())))
val_map[specified_inputs_syms[-1]] = lateral_force
sym_sol = np.array([x for x in xdot_expr.subs(val_map).evalf()], dtype=float)
np.testing.assert_allclose(state_deriv_values, sym_sol)
# Now let's see if the discretized version gives a simliar answer if h
# is small enough.
collocator = dc.ConstraintCollocator(closed, states_syms, 10, 0.01)
dclosed = collocator.discrete_eom
xi = collocator.current_discrete_state_symbols
xp = collocator.previous_discrete_state_symbols
si = collocator.current_discrete_specified_symbols
h = collocator.time_interval_symbol
euler_formula = [(i - p) / h for i, p in zip(xi, xp)]
xdot_expr = sym.solve(dclosed, euler_formula)
xdot_expr = sym.Matrix([xdot_expr[xd] for xd in euler_formula])
val_map = dict(zip(xi, state_values))
val_map.update(simulate.constants_dict(constants_syms))
val_map.update(dict(zip(gain_syms, gains.flatten())))
val_map[si[0]] = lateral_force
sym_sol = np.array([x for x in xdot_expr.subs(val_map).evalf()], dtype=float)
np.testing.assert_allclose(state_deriv_values, sym_sol)
"""This example simply simulates and visualizes the uncontrolled motion and
the model "falls down"."""
import numpy as np
from scipy.integrate import odeint
from pydy.codegen.code import generate_ode_function
from pydy.viz import Scene
from pygait2d import derive, simulate
(mass_matrix, forcing_vector, kane, constants, coordinates, speeds,
specified, visualization_frames, ground, origin) = derive.derive_equations_of_motion()
rhs = generate_ode_function(mass_matrix, forcing_vector,
constants, coordinates, speeds,
specified=specified, generator='cython')
constant_values = simulate.load_constants(constants,
'data/example_constants.yml')
args = {'constants': np.array(constant_values.values()),
'specified': np.zeros(9)}
time_vector = np.linspace(0.0, 10.0, num=1000)
initial_conditions = np.zeros(18)
initial_conditions[1] = 1.0 # set hip above ground
initial_conditions[3] = np.deg2rad(5.0) # right hip angle
initial_conditions[6] = -np.deg2rad(5.0) # left hip angle
trajectories = odeint(rhs, initial_conditions, time_vector, args=(args,))
# 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.mass_matrix_full,
kane.forcing_full, param_syms, kane._q,
kane._u)
# 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': param_vals
}, ))
print("Integration done.")
from pydy.codegen.code import generate_ode_function
from scipy.integrate import odeint
import math
import numpy as np
import single_pendulum_setup as sp
import double_pendulum_particle_setup as pp
import double_pendulum_rb_setup as rb
import pickle
print("done importing!")
# Specify Numerical Quantities
# ============================
file = open('args_ode_funcs.pkl', 'rb')
args_ode_funcs = pickle.load(file)
file.close()
args_ode_funcs = sympify(args_ode_funcs)
right_hand_side = generate_ode_function(args_ode_funcs[0],args_ode_funcs[1],args_ode_funcs[2],args_ode_funcs[3],args_ode_funcs[4],args_ode_funcs[5])
initial_coordinates = [0.01, 0.01, 0.01]
initial_speeds = [0.0, 0.0, 0.0]
x0 = concatenate((initial_coordinates, initial_speeds), axis=1)
# taken from male1.txt in yeadon (maybe I should use the values in Winters).
numerical_constants = array([0.5,
0.5,
0.75,
0.75,
1.0,
1.0,
9.81])
parameter_dict = dict(zip(args_ode_funcs[2], numerical_constants))
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.mass_matrix_full,
kane.forcing_full,
param_syms, kane._q, kane._u)
# 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.")
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 an 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.mass_matrix_full,
self.forcing_vector_full,
self.parameters.values(),
self.coordinates.values(),
self.speeds.values(),
self.specified.values()[-3:],
generator='cython')
args = {'constants': np.array(self.open_loop_par_map.values()),
'specified': controller}
return rhs, args
