def add_muscle(self, muscle):
"""Add the muscle model to the system.
Parameters
----------
muscle: <Muscle>
Instance of muscle model
Example:
--------
>>> from muscle import Muscle
>>> from system_parameters import MuscleParameters
>>> muscle = Muscle(MuscleParameters()) #: Default muscle
>>> isometric_system = IsometricMuscleSystem()
>>> isometric_system.add_muscle(muscle)
"""
if self.muscle is not None:
pylog.warning(
'You have already added the muscle model to the system.')
return
else:
if muscle.__class__ is not Muscle:
pylog.error('Trying to set of type {} to muscle'.format(
muscle.__class__))
raise TypeError()
else:
pylog.info('Added new muscle model to the system')
self.muscle = muscle
def main():
"""Main function that runs all the exercises."""
pylog.info('Implementing Lab 6 : Exercise 2')
exercise2()
pylog.info('Implementing Lab 6 : Exercise 3')
exercise3()
return
def calc(a, b, op='add'):
if op == 'add':
return a + b
elif op == 'sub':
return a - b
else:
pylog.info('valid operations are add and sub')
def postprocess(self,
iteration,
log_path='',
plot=False,
video='',
**kwargs):
"""Plot after simulation"""
times = np.arange(0, self.options.timestep * self.options.n_iterations,
self.options.timestep)[:iteration]
animat = self.animat()
# Log
if log_path:
pylog.info('Saving data to {}'.format(log_path))
animat.data.to_file(
os.path.join(log_path, 'simulation.hdf5'),
iteration,
)
self.options.save(os.path.join(log_path,
'simulation_options.yaml'))
animat.options.save(os.path.join(log_path, 'animat_options.yaml'))
# Plot
if plot:
animat.data.plot(times)
# Record video
if video:
self.interface.video.save(video,
iteration=iteration,
writer=kwargs.pop('writer', 'ffmpeg'))
def __init__(self, **kwargs):
super(PendulumParameters, self).__init__('Pendulum')
self.parameters = {}
self.units = {}
self.units['g'] = 'N-m/s2'
self.units['m'] = 'kg'
self.units['L'] = 'm'
self.units['I'] = 'kg-m**2'
self.units['sin'] = ''
self.units['PERTURBATION'] = 'bool'
# Initialize parameters
self.parameters = {
'g': 9.81, 'm': 0.25, 'L': 1., 'I': 0.0, 'sin': np.sin,
'PERTURBATION': False}
# Pendulum parameters
self.g = kwargs.pop("g", 9.81) # Gravity constant
self.m = kwargs.pop("m", 0.25) # Mass
self.L = kwargs.pop("L", 1.) # Length
self.sin = kwargs.pop("sin", np.sin) # Sine function
self.PERTURBATION = kwargs.pop("PERTURBATION", False) # Perturbation
pylog.info(self)
return
def plot_integration_methods(**kwargs):
""" Plot integration methods results """
pylog.info("Plotting integration results")
# Results
analytical = kwargs.pop("analytical", None)
euler = kwargs.pop("euler", None)
ode = kwargs.pop("ode", None)
ode_rk = kwargs.pop("ode_rk", None)
euler_ts_small = kwargs.pop("euler_ts_small", None)
# Figures
fig_all = kwargs.pop(
"figure",
"Integration methods".replace(" ", "_")
)
fig_ts = "Integration methods smaller ts".replace(" ", "_")
fig_euler = "Euler integration".replace(" ", "_")
d = "."
# Time steps information
et_val = kwargs.pop("euler_timestep", None)
ets_val = kwargs.pop("euler_timestep_small", None)
et = " (ts={})".format(et_val) if et_val is not None else ""
ets = " (ts={})".format(ets_val) if ets_val is not None else ""
# Analytical
if analytical is not None:
analytical.plot_state(figure=fig_euler, label="Analytical", marker=d)
# Euler
if euler is not None:
euler.plot_state(figure=fig_euler, label="Euler"+et, marker=d)
# ODE
ls = " "
ode_plot = False
if ode is not None:
if ode_plot is False:
analytical.plot_state(figure=fig_all, label="Analytical", marker=d)
euler.plot_state(figure=fig_all, label="Euler"+et, marker=d)
ode_plot = True
ode.plot_state(figure=fig_all, label="LSODA", linestyle=ls, marker="x")
# ODE Runge-Kutta
if ode_rk is not None:
if ode_plot is False:
analytical.plot_state(figure=fig_all, label="Analytical", marker=d)
euler.plot_state(figure=fig_all, label="Euler"+et, marker=d)
ode_rk.plot_state(figure=fig_all, label="RK", linestyle=ls, marker=d)
# Euler with lower time step
if euler_ts_small is not None:
euler_ts_small.plot_state(
figure=fig_ts,
label="Euler"+ets
)
analytical.plot_state(figure=fig_ts, label="Analytical", marker=d)
def evolution_perturbation(x0, time):
""" Perturbation and no damping simulation """
pylog.info("Evolution with perturbations")
parameters = PendulumParameters(d=0.0)
pylog.info(parameters)
title = "{} with perturbation (x0={})"
res = integrate(pendulum_perturbation, x0, time, args=(parameters, ))
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
def evolution_no_damping(x0, time):
""" No damping simulation """
pylog.info("Evolution with no damping")
parameters = PendulumParameters(d=0.0)
pylog.info(parameters)
title = "{} without damping (x0={})"
res = integrate(pendulum_system, x0, time, args=(parameters, ))
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
def evolution_dry(x0, time):
""" Dry friction simulation """
pylog.info("Evolution with dry friction")
parameters = PendulumParameters(d=0.03, dry=True)
pylog.info(parameters)
title = "{} with dry friction (x0={})"
res = integrate(pendulum_system, x0, time, args=(parameters, ))
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
def k2(self, value):
""" Keyword Arguments:
value -- Set the value of spring constant for spring 2[N/rad] """
if (value < 0.0):
pylog.warning('Setting bad spring constant. Should be positive!')
else:
self.parameters['k2'] = value
pylog.info('Changed spring constant of spring 1 to {} [N/rad]'.format(
self.parameters['k2']))
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 pendulum_perturbation(state, time, *args):
""" Function for system integration with perturbations.
Setup your time based system pertubations within this function.
The pertubation can either be applied to the states or as an
external torque.
"""
pendulum = args[0]
if time > 5 and time < 5.1:
pylog.info("Applying state perturbation to pendulum_system")
state[1] = 2.
return pendulum.pendulum_system(state[0], state[1], time, torque=0.0)[:, 0]
def evolution_cases(time):
""" Normal simulation """
pylog.info("Evolution with basic paramater")
x0_cases = [["Normal", [0.1, 0]], ["Stable", [0.0, 0.0]],
["Unstable", [np.pi, 0.0]], ["Multiple loops", [0.1, 10.0]]]
title = "{} case {} (x0={})"
parameters = PendulumParameters()
for name, x0 in x0_cases:
res = integrate(pendulum_system, x0, time, args=(parameters, ))
res.plot_state(title.format(name, "state", x0))
res.plot_phase(title.format(name, "phase", x0))
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 generate_config_file(log: dict) -> None:
""" Generates a config file of the weights used in the optimization. """
output_file_directory = os.path.join(neuromechfly_path,
'scripts/neuromuscular_optimization',
'optimization_results', 'CONFIG.yaml')
pylog.info('Output config file : ' + output_file_directory)
with open(output_file_directory, 'w') as of:
yaml.dump(log, of, default_flow_style=False)
def load_config_file(self):
"""Load the animal configuration file"""
try:
stream = open(os.path.realpath(self.muscle_joint_config_path), 'r')
self.muscle_joint_config = yaml.load(stream)
biolog.info('Successfully loaded the file : {}'.format(
os.path.split(self.muscle_joint_config_path)[-1]))
return
except ValueError:
biolog.error('Unable to read the file {}'.format(
self.muscle_joint_config_path))
raise ValueError()
def __init__(self, **kwargs):
super(PendulumParameters, self).__init__()
self._mass = 1.0 # Internal parameter : Do NOT use it!
self._length = 1.0 # Internal parameter : Do NOT use it!
self.g = kwargs.pop("g", 9.81) # Gravity constant
self.I = None # Inertia be set automatically when mass and length are set
self.m = kwargs.pop("m", 1.) # Mass
self.L = kwargs.pop("L", 1.) # Length
self.d = kwargs.pop("d", 0.3) # damping
self.sin = kwargs.pop("sin", np.sin) # Sine function
self.dry = kwargs.pop("dry", False) # Use dry friction (True or False)
pylog.info(self)
def __init__(self, **kwargs):
super(PendulumParameters, self).__init__('Pendulum')
self.parameters = {}
self.units = {}
self.units['g'] = 'N-m/s2'
self.units['m'] = 'kg'
self.units['L'] = 'm'
self.units['I'] = 'kg-m**2'
self.units['sin'] = ''
self.units['k1'] = 'N/rad'
self.units['k2'] = 'N/rad'
self.units['s_theta_ref1'] = 'rad'
self.units['s_theta_ref2'] = 'rad'
self.units['b1'] = 'N-s/rad'
self.units['b2'] = 'N-s/rad'
# Initialize parameters
self.parameters = {
'g': 9.81,
'm': 1.,
'L': 1.,
'I': 0.0,
'sin': np.sin,
'k1': 0.,
'k2': 0.,
's_theta_ref1': 0.,
's_theta_ref2': 0.,
'b1': 0.,
'b2': 0.
}
# Pendulum parameters
self.g = kwargs.pop("g", 9.81) # Gravity constant
self.m = kwargs.pop("m", 1.) # Mass
self.L = kwargs.pop("L", 1.) # Length
self.sin = kwargs.pop("sin", np.sin) # Sine function
# Spring parameters
self.k1 = kwargs.pop("k1", 10.) # Spring constant of Spring 1
self.k2 = kwargs.pop("k2", 10.) # Spring constant of Spring 2
self.s_theta_ref1 = kwargs.pop("s_theta_ref1",
0.0) # Spring 1 reference angle
self.s_theta_ref2 = kwargs.pop("s_theta_ref2",
0.0) # Spring 2 reference angle
# Damping parameters
self.b1 = kwargs.pop("b1", 0.5) # Damping constant of Damper 1
self.b2 = kwargs.pop("b2", 0.5) # Damping constant of Damper 2
pylog.info(self)
return
def exercise2(clargs):
""" Exercise 2 """
pylog.info("Running exercise 2")
# System definition
A = np.array([[1, 4], [-4, -2]])
system_analysis(A) # Optional
time_total = 10
time_step = 0.01
x0, time = [0, 1], np.arange(0, time_total, time_step)
# Normal run
pylog.info("Running system integration")
integration(x0, time, A, "system_integration")
# Stable point (Optional)
pylog.info("Running stable point integration")
x0 = [0, 0]
integration(x0, time, A, "stable")
# Periodic
pylog.info("Running periodic system integration")
A = np.array([[2, 4], [-4, -2]])
x0 = [1, 0]
integration(x0, time, A, "periodic")
# Plot
if not clargs.save_figures:
plt.show()
def pendulum_limit_cycle(x0, time=0.0, *args):
# Initialize the parameters without damping
pendulum = args[0]
pendulum.parameters.b1 = 0.0
pendulum.parameters.b2 = 0.0
pylog.info(pendulum.parameters.showParameters())
pylog.info(
"1a. Running pendulum_system with springs to study limit cycle behavior"
)
title = "{} Limit Cycle(x0 = {})"
res = integrate(pendulum_perturbation, x0, time, args=args)
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
def add_pendulum_system(self, system):
"""Add the pendulum system.
Parameters
----------
system: Pendulum
Pendulum system
"""
if self.systems_list.count('pendulum') == 1:
pylog.warning(
'You have already added the pendulum model to the system.')
return
else:
pylog.info('Added pedulum model to the system')
self.systems_list.append('pendulum')
self.pendulum_sys = system
def add_muscle_system(self, system):
"""Add the muscle system.
Parameters
----------
system: MuscleSystem
Muscle system
"""
if self.systems_list.count('muscle') == 1:
pylog.warning(
'You have already added the muscle model to the system.')
return
else:
pylog.info('Added muscle model to the system')
self.systems_list.append('muscle')
self.muscle_sys = system
def add_neural_system(self, system):
"""Add the neural network system.
Parameters
----------
system: NeuralSystem
Neural Network system
"""
if self.systems_list.count('neural') == 1:
pylog.warning(
'You have already added the neural model to the system.')
return
else:
pylog.info('Added neural network to the system')
self.systems_list.append('neural')
self.neural_sys = system
def simulation_setup(
animat_sdf,
arena,
animat_options,
simulation_options,
network,
):
"""Simulation setup"""
# Animat data
animat_data = SalamandraData.from_options(
np.arange(
0,
simulation_options.duration(),
simulation_options.timestep,
),
network.state,
animat_options.morphology,
simulation_options.n_iterations,
)
# Animat controller
animat_controller = SalamandraController(
joints=animat_options.morphology.joints,
animat_data=animat_data,
network=network,
)
# Creating animat
animat = Salamandra(
sdf=animat_sdf,
options=animat_options,
controller=animat_controller,
timestep=simulation_options.timestep,
iterations=simulation_options.n_iterations,
units=simulation_options.units,
)
# Setup simulation
pylog.info('Creating simulation')
sim = SalamandraSimulation(
simulation_options=simulation_options,
animat=animat,
arena=arena,
)
return sim, animat_data
def pendulum_spring_damper(x0, time=0.0, *args):
""" Function to analyse the pendulum spring damper system"""
pendulum = args[0]
pendulum.parameters.b1 = 0.5
pendulum.parameters.b2 = 0.5
pendulum.parameters.k1 = 50.0
pendulum.parameters.k2 = 50.0
pendulum.parameters.s_theta_ref1 = np.deg2rad(-45.0)
pendulum.parameters.s_theta_ref2 = np.deg2rad(45.0)
pylog.info(
"20. Running pendulum_system for analysing role of spring and damper muscle"
)
pylog.info(pendulum.parameters.showParameters())
title = "{} Spring Damper (x0 = {})"
res = integrate(pendulum_perturbation, x0, time, args=args)
res.plot_state(title.format("State", x0))
res.plot_phase(title.format("Phase", x0))
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 """
fixed_points() # Optional
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
# Evolutions
evolution_cases(time)
x0 = [0.1, 0]
evolution_no_damping(x0, time)
evolution_perturbation(x0, time)
evolution_dry(x0, time)
# Show plots of all results
if not clargs.save_figures:
plt.show()
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 show_system_properties(self):
"""Function prints the system properties in the system.
"""
for sys in self.systems_list:
if (sys == 'pendulum'):
pylog.info(self.pendulum_sys.parameters.showParameters())
elif (sys == 'muscle'):
pylog.info(
self.muscle_sys.muscle_1.parameters.showParameters())
pylog.info(
self.muscle_sys.muscle_2.parameters.showParameters())
elif (sys == 'neural'):
pylog.info(self.neural_sys.parameters.showParameters())
def exercise2(clargs):
""" Exercise 2 """
pylog.info("Running exercise 2")
# System definition
pylog.warning("Proper matrix A must be implemented")
A = np.array([[1, 0], [0, 1]])
time_total = 10
time_step = 0.01
x0, time = [0, 1], np.arange(0, time_total, time_step)
# Normal run
pylog.warning("System integration must be implemented")
# integration(x0, time, A, "example")
# Stable point (Optional)
pylog.warning("Stable point integration must be implemented")
# Periodic
pylog.warning("Periodic system must be implemented")
# Plot
if not clargs.save_figures:
plt.show()