def exercise3():
""" Main function to run for Exercise 3.
Parameters
----------
None
Returns
-------
None
"""
# Create system
sim = system_init()
# Add external inputs to neural network
sim.add_external_inputs_to_network(0.1 * np.ones((len(sim.time), 4)))
# Integrate the system for the above initialized state and time
sim.simulate()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# In order to obtain internal states of the muscle
# you can access the results attribute in the muscle class
muscle_1_results = sim.sys.muscle_sys.muscle_1.results
muscle_2_results = sim.sys.muscle_sys.muscle_2.results
# Plotting the results
plt.figure('Pendulum')
plt.title('Pendulum Phase')
plt.plot(res[:, 1], res[:, 2])
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad.s]')
plt.grid()
# To animate the model, use the SystemAnimation class
# Pass the res(states) and systems you wish to animate
simulation = SystemAnimation(res, sim.sys.pendulum_sys, sim.sys.muscle_sys,
sim.sys.neural_sys)
if DEFAULT["save_figures"] is False:
# To start the animation
simulation.animate()
plt.show()
else:
figures = plt.get_figlabels()
pylog.debug("Saving figures:\n{}".format(figures))
for fig in figures:
plt.figure(fig)
save_figure(fig)
plt.close(fig)
def L(self, value):
""" Keyword Arguments:
value -- Set the value of pendulum's length [m] """
self.parameters['L'] = value
# ReCompute inertia
# Inertia = m*l**2
self.I = self.m * self.parameters['L']**2
pylog.debug('Changed pendulum length to {} [m]'.format(self.L))
def m(self, value):
"""
Set the mass of the pendulum.
Setting/Changing mass will automatically recompute the inertia.
"""
self.parameters['m'] = value
# ReCompute inertia
# Inertia = m*l**2
self.I = self.parameters['m'] * self.L**2
pylog.debug('Changed pendulum mass to {} [kg]'.format(self.m))
def error(method_state, analytical_state, method):
""" Compute error of an ode method based on analytical solution """
err = np.sum(np.abs(analytical_state - method_state))/len(method_state)
err_max = max(np.abs(analytical_state - method_state))
pylog.debug(
"Obtained an error of {} using {} method (max={}, len={})".format(
err, method, err_max, len(method_state)
)
)
return err
def exercise1(clargs):
""" Exercise 1 """
# Setup
pylog.info("Running exercise 1")
# Setup
time_max = 5 # Maximum simulation time
time_step = 0.2 # Time step for ODE integration in simulation
x0 = np.array([1.]) # Initial state
# Integration methods (Exercises 1.a - 1.d)
pylog.info("Running function integration using different methods")
# Example
pylog.debug("Running example plot for integration (remove)")
example = example_integrate(x0, time_max, time_step)
example.plot_state(figure="Example", label="Example", marker=".")
# Analytical (1.a)
time = np.arange(0, time_max, time_step) # Time vector
x_a = analytic_function(time)
analytical = Result(x_a, time) if x_a is not None else None
# Euler (1.b)
euler = euler_integrate(function, x0, time_max, time_step)
# ODE (1.c)
ode = ode_integrate(function, x0, time_max, time_step)
# ODE Runge-Kutta (1.c)
ode_rk = ode_integrate_rk(function_rk, x0, time_max, time_step)
# Euler with lower time step (1.d)
pylog.warning("Euler with smaller ts must be implemented")
euler_time_step = None
euler_ts_small = (euler_integrate(function, x0, time_max, euler_time_step)
if euler_time_step is not None else None)
# Plot integration results
plot_integration_methods(analytical=analytical,
euler=euler,
ode=ode,
ode_rk=ode_rk,
euler_ts_small=euler_ts_small,
euler_timestep=time_step,
euler_timestep_small=euler_time_step)
# Error analysis (Exercise 1.e)
pylog.warning("Error analysis must be implemented")
# Show plots of all results
if not clargs.save_figures:
plt.show()
return
def main():
"""Main function testing"""
dae = DaeGenerator()
#: Add x
x = dae.add_x('x', 10.0)
z = dae.add_x('z', 100.0)
#: Set x as an output
dae.add_y(x)
dae.add_y(z)
biolog.debug(dae.y)
x.val = 123
biolog.debug([p**2 for p in dae.y])
def save_figure(figure, name=None, **kwargs):
""" Save figure """
for extension in kwargs.pop("extensions", ["pdf"]):
fig = figure.replace(" ", "_").replace(".", "dot")
if name is None:
name = "{}.{}".format(fig, extension)
else:
name = "{}.{}".format(name, extension)
fig = plt.figure(figure)
size = plt.rcParams.get('figure.figsize')
fig.set_size_inches(0.7 * size[0], 0.7 * size[1], forward=True)
plt.savefig(name, bbox_inches='tight')
pylog.debug("Saving figure {}...".format(name))
fig.set_size_inches(size[0], size[1], forward=True)
def exercise1():
exercise1a()
exercise1d()
if DEFAULT["save_figures"] is False:
plt.show()
else:
figures = plt.get_figlabels()
print(figures)
pylog.debug("Saving figures:\n{}".format(figures))
for fig in figures:
plt.figure(fig)
save_figure(fig)
plt.close(fig)
def pendulum_set_position(x0, time=0.0, *args):
""" Function to analyse the pendulum spring damper system"""
pendulum = args[0]
pendulum.parameters.b1 = 1.
pendulum.parameters.b2 = 1.
pendulum.parameters.k1 = 50.0
pendulum.parameters.k2 = 50.0
pendulum.parameters.s_theta_ref1 = np.deg2rad(0.0)
pendulum.parameters.s_theta_ref2 = np.deg2rad(65.6)
pylog.info("1b. Running pendulum_system to set fixed position")
pylog.info(pendulum.parameters.showParameters())
title = "{} Pendulum Fixed Position (x0 = {})"
res = integrate(pendulum_integration, x0, time, args=args)
pylog.debug('Position : {}'.format(np.rad2deg(res.state[-1])))
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
def exercise3(clargs):
""" Exercise 3 """
parameters = PendulumParameters() # Checkout pendulum.py for more info
pylog.info(parameters)
# Simulation parameters
time = np.arange(0, 30, 0.01) # Simulation time
x0 = [0.1, 0.0] # Initial state
# To use/modify pendulum parameters (See PendulumParameters documentation):
# parameters.g = 9.81 # Gravity constant
# parameters.m = 1. # Mass
# parameters.L = 1. # Length
# parameters.I = 1. # Inertia (Automatically computed!)
# parameters.d = 0.3 # damping
# parameters.sin = np.sin # Sine function
# parameters.dry = False # Use dry friction (True or False)
# Example of system integration (Similar to lab1)
# (NOTE: pendulum_equation must be imlpemented first)
pylog.debug("Running integration example")
res = integrate(pendulum_system, x0, time, args=(parameters,))
res.plot_state("State")
res.plot_phase("Phase")
# Evolutions
# Write code here (You can add functions for the different cases)
pylog.warning(
"Evolution of pendulum in normal conditions must be implemented"
)
pylog.warning(
"Evolution of pendulum without damping must be implemented"
)
pylog.warning(
"Evolution of pendulum with perturbations must be implemented"
)
pylog.warning(
"Evolution of pendulum with dry friction must be implemented"
)
# Show plots of all results
if not clargs.save_figures:
plt.show()
def animat_interface(self, iteration):
"""Animat interface"""
animat = self.animat()
# Camera zoom
if self.interface.user_params.zoom().changed:
self.interface.camera.set_zoom(
self.interface.user_params.zoom().value)
# Drives
if self.interface.user_params.drive_left().changed:
pylog.debug(
'Switch of behaviour can be implemented here (OPTIONAL)')
# Example: animat.data.network.drives...
self.interface.user_params.drive_left().changed = False
if self.interface.user_params.drive_right().changed:
pylog.debug(
'Switch of behaviour can be implemented here (OPTIONAL)')
# Example: animat.data.network.drives...
self.interface.user_params.drive_right().changed = False
def set_body_properties(self, verbose=False):
"""Set body properties"""
# Masses
n_links = pybullet.getNumJoints(self.identity()) + 1
self.masses = np.zeros(n_links)
for i in range(n_links):
self.masses[i] = pybullet.getDynamicsInfo(self.identity(),
i - 1)[0]
if verbose:
pylog.debug('Body mass: {} [kg]'.format(np.sum(self.masses)))
# Deactivate collisions
if self.options.morphology.links_no_collisions is not None:
self.set_collisions(self.options.morphology.links_no_collisions,
group=0,
mask=0)
# Deactivate damping
small = 0
self.set_links_dynamics(self._links.keys(),
linearDamping=small,
angularDamping=small,
jointDamping=small)
# Friction
self.set_links_dynamics(
self._links.keys(),
lateralFriction=0.5,
spinningFriction=small,
rollingFriction=small,
)
if self.options.morphology.feet is not None:
self.set_links_dynamics(
self.options.morphology.feet,
lateralFriction=0.9,
spinningFriction=small,
rollingFriction=small,
# contactStiffness=1e3,
# contactDamping=1e6
)
def spawn_sdf(self, verbose=False):
"""Spawn sdf"""
if verbose:
pylog.debug(self.sdf)
self._identity = pybullet.loadSDF(self.sdf,
useMaximalCoordinates=0,
globalScaling=1)[0]
initial_pose(self._identity, self.options.spawn, self.units)
for joint_i in range(pybullet.getNumJoints(self.identity())):
joint_info = pybullet.getJointInfo(self.identity(), joint_i)
self._links[joint_info[12].decode('UTF-8')] = joint_i
self._joints[joint_info[1].decode('UTF-8')] = joint_i
if self.options.morphology.links is not None:
for link in self.options.morphology.links:
if link not in self._links:
self._links[link] = -1
break
for link in self.options.morphology.links:
assert link in self._links, 'Link {} not in {}'.format(
link,
self._links,
)
if verbose:
self.print_information()
def run_network(network_path):
""" Run the network.
Parameters
----------
network_path : <Path>
Path to the network config file
"""
# Initialize network
dt = 1e-3 #: Time step (1ms)
duration = 2
time_vec = np.arange(0, duration, dt) #: Time
container = Container(duration / dt)
net = NeuralSystem(network_path, container)
# initialize network parameters
container.initialize()
net.setup_integrator()
#: Integrate the network
pylog.debug('Begin Integration!')
for t in time_vec:
net.step(dt=dt)
container.update_log()
#: Results
container.dump(overwrite=True)
# Plot results
neural_data = container.neural
neural_outputs = neural_data.outputs.log
neural_outputs_names = neural_data.outputs.names
neural_outputs_name_id = neural_data.outputs.name_index
# Plot Intra-limb activations
for leg in ("RF", "RM", "RH", "LH", "LM", "LH"):
leg_data = np.asarray([
neural_outputs[:, neural_outputs_name_id[name]]
for name in neural_outputs_names if leg in name
]).T
leg_names = [name for name in neural_outputs_names if leg in name]
fig, axs = plt.subplots(nrows=3, ncols=1)
axs[0].plot(time_vec, 1 + np.sin(leg_data[:, :2]))
axs[1].plot(time_vec, 1 + np.sin(leg_data[:, 2:4]))
axs[2].plot(time_vec, 1 + np.sin(leg_data[:, 4:]))
axs[0].axes.xaxis.set_visible(False)
axs[1].axes.xaxis.set_visible(False)
axs[0].set_title(leg_names[0].split('_')[2])
axs[1].set_title(leg_names[2].split('_')[2])
axs[2].set_title(leg_names[4].split('_')[2])
axs[2].set_xlabel("Time[s]")
# Plot Inter-limb activations
leg_data = np.asarray([
neural_outputs[:, neural_outputs_name_id[name]]
for name in neural_outputs_names
if "Coxa" in name and "flexion" in name
]).T
leg_names = [name for name in neural_outputs_names if "Coxa" in name]
fig, ax = plt.subplots(nrows=1, ncols=1)
ax.plot(time_vec, 1 + np.sin(leg_data[:, :]))
ax.set_title("Coxa")
ax.set_xlabel("Time[s]")
#: Show network
net.visualize_network(edge_labels=False)
plt.show()
def exercise2():
""" Main function to run for Exercise 2.
Parameters
----------
None
Returns
-------
None
"""
'''
sim = system_init()
# Add muscle activations to the simulation
# Here you can define your muscle activation vectors
# that are time dependent
act1 = np.ones((len(sim.time), 1)) * 0.05
act2 = np.ones((len(sim.time), 1)) * 0.05
activations = np.hstack((act1, act2))
# Method to add the muscle activations to the simulation
sim.add_muscle_stimulations(activations)
#: If you would like to perturb the pedulum model then you could do
# so by
sim.sys.pendulum_sys.parameters.PERTURBATION = True
# The above line sets the state of the pendulum model to zeros between
# time interval 1.2 < t < 1.25. You can change this and the type of
# perturbation in
# pendulum_system.py::pendulum_system function
# Integrate the system for the above initialized state and time
sim.simulate()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# In order to obtain internal states of the muscle
# you can access the results attribute in the muscle class
muscle_1_results = sim.sys.muscle_sys.muscle_1.results
muscle_2_results = sim.sys.muscle_sys.muscle_2.results
# Plotting the results
plt.figure('Pendulum')
plt.title('Pendulum Phase')
plt.plot(res[:, 1], res[:, 2])
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad.s]')
plt.grid()
'''
###########################################################
###########################################################
###########################################################
###########################################################
###########################################################
### code for 2a
pylog.info("2a")
theta = np.arange(np.pi / 4, np.pi * 3 / 4, 0.001)
temp_a1 = 0.35
ratios = [
0.2,
0.5,
1.,
2.,
5.,
]
L2_s = []
h2_s = []
for temp_ratio in ratios:
temp_a2 = temp_a1 * temp_ratio
temp_L2 = np.sqrt(temp_a1 * temp_a1 + temp_a2 * temp_a2 +
2 * temp_a1 * temp_a2 * np.cos(theta))
temp_h2 = (temp_a1 * temp_a2 * np.sin(theta)) / temp_L2
L2_s = L2_s + [temp_L2]
h2_s = h2_s + [temp_h2]
plt.figure(
'2a. Relationship between muscle length and pendulum angular position')
plt.title(
'Relationship between muscle length and pendulum angular position')
for i in range(len(ratios)):
plt.plot(theta, L2_s[i])
plt.xlabel('Angular Position [rad]')
plt.ylabel('Muscle Length [m]')
temp_legends = [
'ratio of a2/a1 = ' + format((temp_ratio), '.2f')
for temp_ratio in ratios
]
plt.legend(temp_legends)
plt.grid()
plt.show()
plt.figure(
'2a. Relationship between moment arm and pendulum angular position')
plt.title('Relationship between moment arm and pendulum angular position')
for i in range(len(ratios)):
plt.plot(theta, h2_s[i])
plt.xlabel('Angular Position [rad]')
plt.ylabel('Moment Arm [m]')
temp_legends = [
'ratio of a2/a1 = ' + format((temp_ratio), '.2f')
for temp_ratio in ratios
]
plt.legend(temp_legends)
plt.grid()
plt.show()
###########################################################
###########################################################
###########################################################
###########################################################
###########################################################
### code for 2b
pylog.info("2b")
#initialization
P_params = PendulumParameters() # Instantiate pendulum parameters
P_params.L = 1.0 # To change the default length of the pendulum
P_params.m = 0.25 # To change the default mass of the pendulum
pendulum = PendulumSystem(P_params) # Instantiate Pendulum object
#### CHECK OUT Pendulum.py to ADD PERTURBATIONS TO THE MODEL #####
pylog.info('Pendulum model initialized \n {}'.format(
pendulum.parameters.showParameters()))
########## MUSCLES ##########
# Define and Setup your muscle model here
# Check MuscleSystem.py for more details on MuscleSystem class
m1_param = MuscleParameters() # Instantiate Muscle 1 parameters
m1_param.f_max = 200. # To change Muscle 1 max force
m1_param.l_opt = 0.4
m1_param.l_slack = 0.45
m2_param = MuscleParameters() # Instantiate Muscle 2 parameters
m2_param.f_max = 200. # To change Muscle 2 max force
m2_param.l_opt = 0.4
m2_param.l_slack = 0.45
m1 = Muscle('m1', m1_param) # Instantiate Muscle 1 object
m2 = Muscle('m2', m2_param) # Instantiate Muscle 2 object
# Use the MuscleSystem Class to define your muscles in the system
# Instantiate Muscle System with two muscles
muscles = MuscleSystem(m1, m2)
pylog.info('Muscle system initialized \n {} \n {}'.format(
m1.parameters.showParameters(), m2.parameters.showParameters()))
# Define Muscle Attachment points
m1_origin = np.asarray([0.0, 0.9]) # Origin of Muscle 1
m1_insertion = np.asarray([0.0, 0.15]) # Insertion of Muscle 1
m2_origin = np.asarray([0.0, 0.8]) # Origin of Muscle 2
m2_insertion = np.asarray([0.0, -0.3]) # Insertion of Muscle 2
# Attach the muscles
muscles.attach(np.asarray([m1_origin, m1_insertion]),
np.asarray([m2_origin, m2_insertion]))
########## ADD SYSTEMS ##########
# Create a system with Pendulum and Muscles using the System Class
# Check System.py for more details on System class
sys = System() # Instantiate a new system
sys.add_pendulum_system(pendulum) # Add the pendulum model to the system
sys.add_muscle_system(muscles) # Add the muscle model to the system
########## INITIALIZATION ##########
t_max = 2 # Maximum simulation time
time = np.arange(0., t_max, 0.001) # Time vector
##### Model Initial Conditions #####
x0_P = np.asarray([np.pi / 2, 0.0]) # Pendulum initial condition
# Muscle Model initial condition
l_ce_0 = sys.muscle_sys.initialize_muscle_length(np.pi / 2)
x0_M = np.asarray([0.05, l_ce_0[0], 0.05, l_ce_0[1]])
x0 = np.concatenate((x0_P, x0_M)) # System initial conditions
########## System Simulation ##########
sim = SystemSimulation(sys) # Instantiate Simulation object
# Simulate the system for given time
sim.initalize_system(x0, time) # Initialize the system state
omega = 1.5
sin_act_1 = np.sin(2 * np.pi * omega * time).reshape(len(time), 1)
sin_act_1[sin_act_1 < 0] = 0
#sin_act_2=np.sin(2*np.pi*omega*time+np.pi/2).reshape(len(time),1)
sin_act_2 = -np.sin(2 * np.pi * omega * time).reshape(len(time), 1)
sin_act_2[sin_act_2 < 0] = 0
activations = np.hstack((sin_act_1, sin_act_2))
plt.figure('2b. Activation wave')
plt.title('Activation wave')
plt.plot(time, sin_act_1, label='Activation 1')
plt.plot(time, sin_act_2, label='Activation 2')
plt.xlabel('Time [s]')
plt.ylabel('Activation')
plt.grid()
plt.legend()
# without pertubation
sim.add_muscle_stimulations(activations)
sim.initalize_system(x0, time)
sim.sys.pendulum_sys.parameters.PERTURBATION = False
sim.simulate()
res = sim.results()
muscle1_results = sim.sys.muscle_sys.muscle_1.results
muscle2_results = sim.sys.muscle_sys.muscle_2.results
plt.figure('2b. Limit cycle without pertubation')
plt.title('Pendulum Phase without pertubation')
plt.plot(
res[:, 1],
res[:, 2],
)
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad/s]')
plt.grid()
plt.legend()
# with pertubation
sim.add_muscle_stimulations(activations)
sim.initalize_system(x0, time)
sim.sys.pendulum_sys.parameters.PERTURBATION = True
sim.simulate()
res = sim.results()
muscle1_results = sim.sys.muscle_sys.muscle_1.results
muscle2_results = sim.sys.muscle_sys.muscle_2.results
plt.figure('2b. Limit cycle with pertubation')
plt.title('Pendulum Phase with pertubation')
plt.plot(
res[:, 1],
res[:, 2],
)
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad/s]')
plt.grid()
plt.legend()
###########################################################
###########################################################
###########################################################
###########################################################
###########################################################
### code for 2c
pylog.info("2c")
# different frequencies
omegas = 1.5 * np.array([0.5, 2.])
positions = []
vels = []
for temp_omega in omegas:
sin_act_1 = np.sin(2 * np.pi * temp_omega * time).reshape(len(time), 1)
sin_act_1[sin_act_1 < 0] = 0
sin_act_2 = -np.sin(2 * np.pi * temp_omega * time).reshape(
len(time), 1)
sin_act_2[sin_act_2 < 0] = 0
activations = np.hstack((sin_act_1, sin_act_2))
sim.add_muscle_stimulations(activations)
sim.initalize_system(x0, time)
sim.sys.pendulum_sys.parameters.PERTURBATION = False
sim.simulate()
res = sim.results()
muscle1_results = sim.sys.muscle_sys.muscle_1.results
muscle2_results = sim.sys.muscle_sys.muscle_2.results
positions = positions + [res[:, 1]]
vels = vels + [res[:, 2]]
plt.figure('2c.Pendulum phase plane with stimulation frequencies')
plt.title('Pendulum phase plane with stimulation frequencies')
for i in range(len(omegas)):
plt.plot(positions[i], vels[i])
plt.xlabel('Angular Position [rad]')
plt.ylabel('Velocity [rad/s]')
temp_legends = [
'ratio of frequency = ' + format((temp_omega / 1.5), '.2f')
for temp_omega in omegas
]
plt.legend(temp_legends)
plt.grid()
plt.show()
'''
# different frequencies
omegas=1.5*np.array([0.2,0.5,1.,2.,5.])
positions=[]
vels=[]
for temp_omega in omegas:
sin_act_1=np.sin(2*np.pi*temp_omega*time).reshape(len(time),1)
sin_act_1[sin_act_1<0]=0
sin_act_2=np.sin(2*np.pi*temp_omega*(np.pi/6+time)).reshape(len(time),1)
sin_act_2[sin_act_2<0]=0
activations = np.hstack((sin_act_1,sin_act_2))
sim.add_muscle_stimulations(activations)
sim.initalize_system(x0, time)
sim.sys.pendulum_sys.parameters.PERTURBATION = False
sim.simulate()
res = sim.results()
muscle1_results = sim.sys.muscle_sys.muscle_1.results
muscle2_results = sim.sys.muscle_sys.muscle_2.results
positions=positions+[res[:, 1]]
vels=vels+[res[:,2]]
plt.figure('2c.Pendulum phase plane with stimulation frequencies')
plt.title('Pendulum phase plane with stimulation frequencies')
for i in range(len(ratios)):
plt.plot(positions[i], vels[i])
plt.xlabel('Angular Position [rad]')
plt.ylabel('Muscle Length [m]')
temp_legends=['ratio of frequency = '+ format((temp_omega/1.5),'.2f') for temp_omega in omegas]
plt.legend(temp_legends)
plt.grid()
plt.show()
'''
# To animate the model, use the SystemAnimation class
# Pass the res(states) and systems you wish to animate
simulation = SystemAnimation(res, sim.sys.pendulum_sys, sim.sys.muscle_sys)
if not DEFAULT["save_figures"]:
# To start the animation
simulation.animate()
plt.show()
else:
figures = plt.get_figlabels()
pylog.debug("Saving figures:\n{}".format(figures))
for fig in figures:
plt.figure(fig)
save_figure(fig)
plt.close(fig)
def save_figures(**kwargs):
"""Save_figures"""
figures = [str(figure) for figure in plt.get_figlabels()]
pylog.debug("Other files:\n - " + "\n - ".join(figures))
for name in figures:
save_figure(name, extensions=kwargs.pop("extensions", ["pdf"]))
def exercise3():
""" Main function to run for Exercise 3.
Parameters
----------
None
Returns
-------
None
"""
'''
# Create system
sim = system_init()
# Add external inputs to neural network
sim.add_external_inputs_to_network(np.ones((len(sim.time), 4)))
# Integrate the system for the above initialized state and time
sim.simulate()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# In order to obtain internal states of the muscle
# you can access the results attribute in the muscle class
muscle_1_results = sim.sys.muscle_sys.muscle_1.results
muscle_2_results = sim.sys.muscle_sys.muscle_2.results
# Plotting the results
plt.figure('Pendulum')
plt.title('Pendulum Phase')
plt.plot(res[:, 1], res[:, 2])
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad.s]')
plt.grid()
'''
######################################################
# initialization
########## PENDULUM ##########
P_params = PendulumParameters()
P_params.L = 1.0
P_params.m = 0.25
pendulum = PendulumSystem(P_params)
pylog.info('Pendulum model initialized \n {}'.format(
pendulum.parameters.showParameters()))
########## MUSCLES ##########
m1_param = MuscleParameters()
m1_param.f_max = 200.
m1_param.l_opt = 0.4
m1_param.l_slack = 0.45
m2_param = MuscleParameters()
m2_param.f_max = 200.
m2_param.l_opt = 0.4
m2_param.l_slack = 0.45
m1 = Muscle('m1', m1_param)
m2 = Muscle('m2', m2_param)
muscles = MuscleSystem(m1, m2)
pylog.info('Muscle system initialized \n {} \n {}'.format(
m1.parameters.showParameters(), m2.parameters.showParameters()))
######## Define Muscle Attachment points
m1_origin = np.asarray([0.0, 0.9])
m1_insertion = np.asarray([0.0, 0.15])
m2_origin = np.asarray([0.0, 0.8])
m2_insertion = np.asarray([0.0, -0.3])
muscles.attach(np.asarray([m1_origin, m1_insertion]),
np.asarray([m2_origin, m2_insertion]))
##### Time #####
t_max = 2.5
time = np.arange(0., t_max, 0.001)
###########################################################
###########################################################
###########################################################
###########################################################
###########################################################
### code for 3a
pylog.info("3a")
d = 1.
tau = np.array([0.02, 0.02, 0.1, 0.1])
b = np.array([3., 3., -3., -3.])
w = np.zeros((4, 4))
w[0, 1] = w[0, 3] = w[1, 0] = w[1, 2] = -5
w[0, 2] = w[1, 3] = 5
w[2, 0] = w[3, 1] = -5
w = w.T
N_params = NetworkParameters()
N_params.D = d
N_params.tau = tau
N_params.b = b
N_params.w = w
neural_network = NeuralSystem(N_params)
sys = System()
sys.add_pendulum_system(pendulum)
sys.add_muscle_system(muscles)
sys.add_neural_system(neural_network)
x0_P = np.asarray([np.pi / 2, 0.])
l_ce_0 = sys.muscle_sys.initialize_muscle_length(np.pi / 2)
x0_M = np.asarray([0.05, l_ce_0[0], 0.05, l_ce_0[1]])
x0_N = np.asarray([-0.5, 1, 0.5, 1])
x0 = np.concatenate((x0_P, x0_M, x0_N))
sim = SystemSimulation(sys)
sim.initalize_system(x0, time)
sim.simulate()
res = sim.results()
positions = res[:, 1]
vels = res[:, 2]
plt.figure('3a. Activation with time ')
plt.title('Activation with time')
plt.plot(res[:, 0], res[:, 3], label="Activation 1")
plt.plot(res[:, 0], res[:, 5], label="Activation 2")
plt.xlabel('Time [s]')
plt.ylabel('Activation')
plt.legend()
plt.grid()
plt.show()
# Plotting the results
plt.figure('3a. Pendulum state with time')
plt.title('Pendulum state with time')
plt.plot(res[:, 0], positions)
plt.xlabel('Time [s]')
plt.ylabel('Position [rad]')
plt.grid()
plt.show()
# Plotting the results
plt.figure('3a. Pendulum phase plot')
plt.title('Pendulum phase plot')
plt.plot(positions, vels)
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad/s]')
plt.grid()
plt.show()
###########################################################
###########################################################
###########################################################
###########################################################
###########################################################
### code for 3b
pylog.info("3b")
all_positions = []
all_vels = []
all_time = []
all_act_1 = []
all_act_2 = []
external_drives = np.array([0, 0.2, 0.5, 1., 2., 5.])
for temp_drive in external_drives:
sim = SystemSimulation(sys)
sim.initalize_system(x0, time)
sim.add_external_inputs_to_network(
np.ones((len(sim.time), 4)) * temp_drive)
sim.simulate()
res = sim.results()
all_time = all_time + [res[:, 0]]
all_positions = all_positions + [res[:, 1]]
all_vels = all_vels + [res[:, 2]]
all_act_1 = all_act_1 + [res[:, 3]]
all_act_2 = all_act_2 + [res[:, 5]]
plt.figure('3a. Activation with time by different external drives')
plt.title('Activation with time by different external drives')
for i in range(len(external_drives)):
plt.plot(all_time[i], all_act_1[i])
plt.plot(all_time[i], all_act_2[i])
plt.xlabel('Time [s]')
plt.ylabel('Activation')
temp_legends = [
'external drive: ' + format((temp_drive), '.2f')
for temp_drive in external_drives
]
plt.legend(temp_legends)
plt.grid()
plt.show()
plt.figure('3b. Pendulum state with time by different external drives')
plt.title('Pendulum state with time by different external drives')
for i in range(len(external_drives)):
plt.plot(all_time[i], all_positions[i])
plt.xlabel('Time [s]')
plt.ylabel('Position [rad]')
temp_legends = [
'external drive: ' + format((temp_drive), '.2f')
for temp_drive in external_drives
]
plt.legend(temp_legends)
plt.grid()
plt.show()
plt.figure('3a. Pendulum phase plot by different external drives')
plt.title('Pendulum phase plot by different external drives')
for i in range(len(external_drives)):
plt.plot(all_positions[i], all_vels[i])
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad/s]')
temp_legends = [
'external drive: ' + format((temp_drive), '.2f')
for temp_drive in external_drives
]
plt.legend(temp_legends)
plt.grid()
plt.show()
# To animate the model, use the SystemAnimation class
# Pass the res(states) and systems you wish to animate
simulation = SystemAnimation(res, sim.sys.pendulum_sys, sim.sys.muscle_sys,
sim.sys.neural_sys)
if DEFAULT["save_figures"] is False:
# To start the animation
simulation.animate()
plt.show()
else:
figures = plt.get_figlabels()
pylog.debug("Saving figures:\n{}".format(figures))
for fig in figures:
plt.figure(fig)
save_figure(fig)
plt.close(fig)
self.parameters['b1'] = value
pylog.info(
'Changed damping constant for damper 1 to {} [N-s/rad]'.format(
self.parameters['b1']))
@property
def b2(self):
""" Get the value of damping constant for damper 2. [N-s/rad]
Default is 0.5"""
return self.parameters['b2']
@b2.setter
def b2(self, value):
""" Keyword Arguments:
value -- Set the value of damping constant for damper 2. [N-s/rad] """
if (value < 0.0):
pylog.warning('Setting bad damping values. Should be positive!')
else:
self.parameters['b2'] = value
pylog.info(
'Changed damping constant for damper 2 to {} [N-s/rad]'.format(
self.parameters['b2']))
def showParameters(self):
return self.msg(self.parameters, self.units)
if __name__ == '__main__':
P = PendulumParameters(g=9.81, L=1.)
pylog.debug(P.showParameters())
def exercise2():
""" Main function to run for Exercise 2.
Parameters
----------
None
Returns
-------
None
"""
sim = system_init()
# Add muscle activations to the simulation
# Here you can define your muscle activation vectors
# that are time dependent
# act1 = np.ones((len(sim.time), 1)) * 0.05
# act2 = np.ones((len(sim.time), 1)) * 0.05
A= 0.95
act1 = 0.05+ np.absolute(np.expand_dims((np.sin(2.*np.pi*sim.time)),axis=1))*A
act2 = 0.05+ np.absolute(np.expand_dims((np.cos(2.*np.pi*sim.time)),axis=1))*A
#Frequency Variation
act11 = 0.05 + np.absolute(np.expand_dims((np.sin(2.*np.pi*sim.time*0.25)),axis=1))*A
act21 = 0.05 + np.absolute(np.expand_dims((np.cos(2.*np.pi*sim.time*0.25)),axis=1))*A
act12 = 0.05 + np.absolute(np.expand_dims((np.sin(2.*np.pi*sim.time*0.5)),axis=1))*A
act22 = 0.05 + np.absolute(np.expand_dims((np.cos(2.*np.pi*sim.time*0.5)),axis=1))*A
act13 = 0.05 + np.absolute(np.expand_dims((np.sin(2.*np.pi*sim.time*2)),axis=1))*A
act23 = 0.05 + np.absolute(np.expand_dims((np.cos(2.*np.pi*sim.time*2)),axis=1))*A
act14 = 0.05 + np.absolute(np.expand_dims((np.sin(2.*np.pi*sim.time*4)),axis=1))*A
act24 = 0.05 + np.absolute(np.expand_dims((np.cos(2.*np.pi*sim.time*4)),axis=1))*A
activations = np.hstack((act1, act2))
activations1 = np.hstack((act11, act21))
activations2 = np.hstack((act12, act22))
activations3 = np.hstack((act13, act23))
activations4 = np.hstack((act14, act24))
plt.figure('Activations forms')
plt.plot(sim.time, act1, label = "Activation 1 - Sinus")
plt.plot(sim.time, act2, label = "Activation 2 - Cosinus")
plt.legend(loc="best")
plt.xlabel('Time [s]')
plt.ylabel('Amplitude')
# Method to add the muscle activations to the simulation
sim.add_muscle_stimulations(activations)
#: If you would like to perturb the pedulum model then you could do
# so by
sim.sys.pendulum_sys.parameters.PERTURBATION = True
# The above line sets the state of the pendulum model to zeros between
# time interval 1.2 < t < 1.25. You can change this and the type of
# perturbation in
# pendulum_system.py::pendulum_system function
# Integrate the system for the above initialized state and time
sim.simulate()
# Obtain the states of the system after integration
# res is np.asarray [time, states]
# states vector is in the same order as x0
res = sim.results()
# In order to obtain internal states of the muscle
# you can access the results attribute in the muscle class
muscle_1_results = sim.sys.muscle_sys.muscle_1.results
muscle_2_results = sim.sys.muscle_sys.muscle_2.results
#PARTIE 2A
theta = np.linspace(np.pi/4, 3*np.pi/4,50)
# plot muscle 1 length
plt.figure('Muscle Length')
L1 = np.sqrt(sim.sys.muscle_sys.a1_m1**2 + sim.sys.muscle_sys.a2_m1**2 - 2*sim.sys.muscle_sys.a1_m1*sim.sys.muscle_sys.a2_m1*np.cos(theta))
plt.plot(theta, L1, label = 'Muscle 1')
print(sim.sys.muscle_sys.a1_m1)
print(sim.sys.muscle_sys.a2_m1)
print(sim.sys.muscle_sys.a1_m2)
print(sim.sys.muscle_sys.a2_m2)
# plot muscle 2 length
L2 = np.sqrt(sim.sys.muscle_sys.a1_m2**2 + sim.sys.muscle_sys.a2_m2**2 + 2*sim.sys.muscle_sys.a1_m2*sim.sys.muscle_sys.a2_m2*np.cos(theta))
plt.plot(theta, L2, label = 'Muscle 2')
plt.xlabel('Theta [rad]')
plt.ylabel('Muscle Length [m]')
plt.legend(loc="upper left")
plt.show()
plt.figure('Muscle moment arm')
h1 = sim.sys.muscle_sys.a1_m1*sim.sys.muscle_sys.a2_m1*np.sin(theta)/L1
plt.plot(theta, h1, label = 'Muscle 1')
h2 = sim.sys.muscle_sys.a1_m2*sim.sys.muscle_sys.a2_m2*np.sin(theta)/L2
plt.plot(theta, h2, label = 'Muscle 2')
plt.xlabel('Theta [rad]')
plt.ylabel('Moment arm of muscle [m]')
plt.legend(loc="upper left")
plt.show()
#PARTIE 2B
plt.figure('Pendulum')
plt.figure('Pendulum Phase')
plt.plot(res[:, 1], res[:, 2])
#PARTIE2C
# Plotting the results
plt.figure('Pendulum')
plt.title('Pendulum Phase')
plt.plot(res[:, 1], res[:, 2], label ="Initial f=1")
sim.add_muscle_stimulations(activations1)
sim.sys.pendulum_sys.parameters.PERTURBATION = True
sim.simulate()
res1 = sim.results()
plt.plot(res1[:, 1], res1[:, 2], label="f=0.25")
sim.add_muscle_stimulations(activations2)
sim.sys.pendulum_sys.parameters.PERTURBATION = True
sim.simulate()
res2 = sim.results()
plt.plot(res2[:, 1], res2[:, 2], label="f=0.5")
sim.add_muscle_stimulations(activations3)
sim.sys.pendulum_sys.parameters.PERTURBATION = True
sim.simulate()
res3 = sim.results()
plt.plot(res3[:, 1], res3[:, 2], label="f=2")
sim.add_muscle_stimulations(activations4)
sim.sys.pendulum_sys.parameters.PERTURBATION = True
sim.simulate()
res4 = sim.results()
plt.plot(res4[:, 1], res4[:, 2], label="f=4")
plt.xlabel('Position [rad]')
plt.ylabel('Velocity [rad.s]')
plt.grid()
plt.legend(loc="best")
# To animate the model, use the SystemAnimation class
# Pass the res(states) and systems you wish to animate
simulation = SystemAnimation(
res, sim.sys.pendulum_sys, sim.sys.muscle_sys
)
if not DEFAULT["save_figures"]:
# To start the animation
simulation.animate()
plt.show()
else:
figures = plt.get_figlabels()
pylog.debug("Saving figures:\n{}".format(figures))
for fig in figures:
plt.figure(fig)
save_figure(fig)
plt.close(fig)
