def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameter_set = SimulationParameters(
simulation_duration=100,
drive_mlr=2.5,
phase_lag=2 * np.pi / 10,
)
network = SalamanderNetwork(timestep, parameter_set)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
# Logs
phases_log = np.zeros([n_iterations, len(network.state.phases)])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([n_iterations, len(network.state.amplitudes)])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([n_iterations, len(network.parameters.freqs)])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros(
[n_iterations, len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
drive_mlr=2
# amplitude_gradient=None,
# phase_lag=None
))
network.step()
phases_log[i + 1, :] = network.state.phases
amplitudes_log[i + 1, :] = network.state.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
freqs_log[i + 1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
# Implement plots of network results
pylog.warning("Implement plots")
#plot_results.main(plot=True)
#plt.plot(phases_log)
plt.close('all')
plt.figure('a')
plt.plot(outputs_log)
plt.figure('b')
plt.plot(amplitudes_log)
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
drive=drive,
amplitude_gradient=None,
phase_lag=None,
turn=1.0,
use_drive_saturation=1,
shes_got_legs=1,
cR_body=[0.05, 0.16], #cR1, cR0
cR_limb=[0.131, 0.131], #cR1, cR0
amplitudes_rate=40,
)
network = SalamanderNetwork(timestep, parameters)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
# Logs
phases_log = np.zeros([n_iterations, len(network.state.phases)])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([n_iterations, len(network.state.amplitudes)])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([n_iterations, len(network.parameters.freqs)])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros(
[n_iterations, len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step()
phases_log[i + 1, :] = network.state.phases
amplitudes_log[i + 1, :] = network.state.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
freqs_log[i + 1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
# Implement plots of network results
pylog.warning("Implement plots")
plot_results.main()
def __init__(self, robot, n_iterations, **kwargs):
super(SalamanderCMC, self).__init__()
self.robot = robot
timestep = int(robot.getBasicTimeStep())
freqs = kwargs.pop("freqs", None)
amplitudes = kwargs.pop("amplitudes", None)
phase_lag = kwargs.pop("phase_lag", None)
turn = kwargs.pop("turn", None)
self.network = SalamanderNetwork(1e-3 * timestep, freqs, amplitudes,
phase_lag, turn)
# Position sensors
self.position_sensors = [
self.robot.getPositionSensor('position_sensor_{}'.format(i + 1))
for i in range(self.N_BODY_JOINTS)
]
for sensor in self.position_sensors:
sensor.enable(timestep)
# GPS
self.gps = robot.getGPS("fgirdle_gps")
self.gps.enable(timestep)
# Get motors
self.motors_body = [
self.robot.getMotor("motor_{}".format(i + 1))
for i in range(self.N_BODY_JOINTS)
]
self.motors_legs = [
self.robot.getMotor("motor_leg_{}".format(i + 1))
for i in range(self.N_LEGS)
]
# Set motors
for motor in self.motors_body:
motor.setPosition(0)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
for motor in self.motors_legs:
motor.setPosition(-np.pi / 2)
# Iteration counter
self.iteration = 0
# Logging
self.log = ExperimentLogger(n_iterations,
n_links=1,
n_joints=self.N_BODY_JOINTS,
filename=kwargs.pop(
"logs", "logs/log.npz"),
timestep=1e-3 * timestep,
freqs=freqs,
amplitude=amplitudes,
phase_lag=phase_lag,
turn=turn)
def __init__(self, robot, n_iterations, parameters, logs="logs/log.npz"):
super(SalamanderCMC, self).__init__()
self.robot = robot
timestep = int(robot.getBasicTimeStep())
self.network = SalamanderNetwork(1e-3*timestep, parameters)
# Position sensors
self.position_sensors = [
self.robot.getPositionSensor('position_sensor_{}'.format(i+1))
for i in range(self.N_BODY_JOINTS)
] + [
self.robot.getPositionSensor('position_sensor_leg_{}'.format(i+1))
for i in range(self.N_LEGS)
]
for sensor in self.position_sensors:
sensor.enable(timestep)
# GPS
self.enable = False
self.gps = robot.getGPS("fgirdle_gps")
self.gps.enable(timestep)
# Get motors
self.motors_body = [
self.robot.getMotor("motor_{}".format(i+1))
for i in range(self.N_BODY_JOINTS)
]
self.motors_legs = [
self.robot.getMotor("motor_leg_{}".format(i+1))
for i in range(self.N_LEGS)
]
# Set motors
for motor in self.motors_body:
motor.setPosition(0)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
for motor in self.motors_legs:
motor.setPosition(-np.pi/2)
motor.enableForceFeedback(timestep)
motor.enableTorqueFeedback(timestep)
# Iteration counter
self.iteration = 0
# Logging
self.log = ExperimentLogger(
n_iterations,
n_links=1,
n_joints=self.N_BODY_JOINTS+self.N_LEGS,
filename=logs,
timestep=1e-3*timestep,
**parameters
)
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters()
network = SalamanderNetwork(timestep, parameters)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
# Logs
phases_log = np.zeros([n_iterations, len(network.state.phases)])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([n_iterations, len(network.state.amplitudes)])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([n_iterations, len(network.parameters.freqs)])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros(
[n_iterations, len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step()
phases_log[i + 1, :] = network.state.phases
amplitudes_log[i + 1, :] = network.state.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
freqs_log[i + 1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
# Implement plots of network results
#pylog.warning("Implement plots")
# body joint angles
#plot_positions(times, outputs_log[:,:10])
# limb joint angles
plot_positions(times, outputs_log[:, 10:])
def main(plot=True):
"""Main - Run network without Webots and plot results"""
# Simulation setup
timestep = 1e-3
times = np.arange(0, 2, timestep)
freqs = np.ones(20) # 20 amplitudes to be specified
amplitudes = [1, 1] #np.ones(20) #[1, 1] # 20 amplitudes to be specified
phase_lag = 2 * np.pi / 10 # Single scalar
turn = 0 # Will be used to modify set_parameters from AmplitudeEquation in network.py
network = SalamanderNetwork(timestep, freqs, amplitudes, phase_lag, turn)
# Logs
phases_log = np.zeros([len(times), len(network.phase_equation.phases)])
phases_log[0, :] = network.phase_equation.phases
amplitudes_log = np.zeros(
[len(times), len(network.amplitude_equation.amplitudes)])
amplitudes_log[0, :] = network.amplitude_equation.amplitudes
outputs_log = np.zeros(
[len(times), len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Simulation
tic = time.time()
for i, _ in enumerate(times[1:]):
network.step()
phases_log[i + 1, :] = network.phase_equation.phases
amplitudes_log[i + 1, :] = network.amplitude_equation.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
toc = time.time()
# Simulation information
print("Time to run simulation for {} steps: {} [s]".format(
len(times), toc - tic))
plotLogData(times, phases_log, what='phase', figure_name='phases_log')
plotLogData(times,
amplitudes_log,
what='amplitude',
figure_name='amplitudes_log')
plotLogData(times, outputs_log, what='output', figure_name='outputs_log')
# Show plots
if plot:
plt.show()
else:
save_figures()
def main(plot=True):
"""Main - Run network without Webots and plot results"""
# Simulation setup
timestep = 1e-3
times = np.arange(0, 2, timestep)
freqs = 1
amplitudes = None
phase_lag = None
turn = None
amplitudes = [1, 1]
phase_lag = 2 * np.pi / (10 - 1)
turn = 0
network = SalamanderNetwork(timestep, freqs, amplitudes, phase_lag, turn)
# Logs
phases_log = np.zeros([len(times), len(network.phase_equation.phases)])
phases_log[0, :] = network.phase_equation.phases
amplitudes_log = np.zeros(
[len(times), len(network.amplitude_equation.amplitudes)])
amplitudes_log[0, :] = network.amplitude_equation.amplitudes
outputs_log = np.zeros(
[len(times), len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Simulation
tic = time.time()
for i, _ in enumerate(times[1:]):
network.step()
phases_log[i + 1, :] = network.phase_equation.phases
amplitudes_log[i + 1, :] = network.amplitude_equation.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
toc = time.time()
# Simulation information
print("Time to run simulation for {} steps: {} [s]".format(
len(times), toc - tic))
# Show plots
if plot:
plt.show()
else:
save_figures()
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
#drive=drive,
amplitude_gradient=None,
phase_lag=None,
turn=None,
)
network = SalamanderNetwork(timestep, parameters)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
cw = np.zeros([24, 24])
for i in range(network.parameters.n_body_joints * 2):
for j in range(network.parameters.n_body_joints * 2):
if j == i + 1 or j == i - 1 or j == i + 10 or j == i - 10:
cw[i][j] = 10
cw[20][1:10] = 1
cw[21][7:10] = 1
cw[22][11:20] = 1
cw[23][17:20] = 1
for i in range(20, 24):
for j in range(20, 24):
if j == i + 1 or j == i - 1 or j == i + 2 or j == i - 2:
cw[i][j] = 10
network.parameters.freqs = 3 * np.ones(24)
network.parameters.coupling_weights = cw
network.parameters.nominal_amplitudes = 0.2 * np.ones(24)
#network.parameters.nominal_amplitudes[0] = 1
fb = np.zeros([24, 24])
for i in range(network.parameters.n_body_joints * 2):
for j in range(network.parameters.n_body_joints * 2):
if j == i + 1:
fb[i][j] = -2 * np.pi / 8
elif j == i - 1:
fb[i][j] = 2 * np.pi / 8
elif j == i + 10 or j == i - 10:
fb[i][j] = np.pi
network.parameters.phase_bias = fb
# Logs
phases_log = np.zeros([n_iterations, len(network.state.phases)])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([n_iterations, len(network.state.amplitudes)])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([n_iterations, len(network.parameters.freqs)])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros(
[n_iterations, len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step()
phases_log[i + 1, :] = network.state.phases
amplitudes_log[i + 1, :] = network.state.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
freqs_log[i + 1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
plt.figure('phases')
plt.plot(phases_log)
plt.figure('amplitude')
plt.plot(amplitudes_log)
plt.figure('frequency')
plt.plot(freqs_log)
plt.figure('out')
plt.plot(outputs_log)
def run_network(duration, update=False, drive=1., gait="Walking"):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
drive=drive,
amplitude_gradient=None,
phase_lag=None,
turn=None,
)
network = SalamanderNetwork(timestep, parameters)
# Logs
phases_log = np.zeros([
n_iterations,
len(network.state.phases)
])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([
n_iterations,
len(network.state.amplitudes)
])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([
n_iterations,
len(network.parameters.freqs)
])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros([
n_iterations,
len(network.get_motor_position_output())
])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
)
)
network.step()
phases_log[i+1, :] = network.state.phases
amplitudes_log[i+1, :] = network.state.amplitudes
outputs_log[i+1, :] = network.get_motor_position_output()
freqs_log[i+1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations,
toc - tic
))
# Result plots
plt_res.plot_body_joints(times, outputs_log, f'exercise_9a_{gait}_body_joints', gait=gait)
plt_res.plot_leg_joints(times, outputs_log, f'exercise_9a_{gait}_leg_joints')
def run_network(duration, update=False, drive=4):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
drive=drive,
amplitude_gradient=None,
turn=None,
)
network = SalamanderNetwork(timestep, parameters)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
# Logs
phases_log = np.zeros([
n_iterations,
len(network.state.phases)
])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([
n_iterations,
len(network.state.amplitudes)
])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([
n_iterations,
len(network.parameters.freqs)
])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros([
n_iterations,
len(network.get_motor_position_output())
])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
)
)
network.step()
phases_log[i+1, :] = network.state.phases
amplitudes_log[i+1, :] = network.state.amplitudes
outputs_log[i+1, :] = network.get_motor_position_output()
freqs_log[i+1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations,
toc - tic
))
# Implement plots of network results
print(np.shape(outputs_log),n_iterations)
plot_positions(times,outputs_log[:,0:10],["q1","q2","q3","q4","q5","q6","q7","q8","q9","q10"],drive)
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
drive=2.5,
amplitude=[0.1, 0.1], # head, tail
phase_lag=2 * np.pi / 8,
turn=None,
couplingBody=10,
couplingLeg=30,
rate=20,
limb_spine_phase_lag=1)
network = SalamanderNetwork(timestep, parameters)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
# Logs
phases_log = np.zeros([n_iterations, len(network.state.phases)])
phases_log[0, :] = network.state.phases
amplitudes_log = np.zeros([n_iterations, len(network.state.amplitudes)])
amplitudes_log[0, :] = network.state.amplitudes
freqs_log = np.zeros([n_iterations, len(network.parameters.freqs)])
freqs_log[0, :] = network.parameters.freqs
outputs_log = np.zeros(
[n_iterations, len(network.get_motor_position_output())])
outputs_log[0, :] = network.get_motor_position_output()
# Run network ODE and log data
tic = time.time()
for i, _ in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step()
phases_log[i + 1, :] = network.state.phases
amplitudes_log[i + 1, :] = network.state.amplitudes
outputs_log[i + 1, :] = network.get_motor_position_output()
freqs_log[i + 1, :] = network.parameters.freqs
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
# Implement plots of network results
#pylog.warning("Implement plots")
plotLogData(times, phases_log, what='phase', figure_name='phase log')
plotLogData(times,
amplitudes_log,
what='amplitude',
figure_name='amplitude log')
plotLogData(times,
outputs_log[:, 0:14],
what='output',
figure_name='output log')