def run_network(duration, update=False, drive=0):
"""Run network without Pybullet and plot results
Parameters
----------
duration: <float>
Duration in [s] for which the network should be run
update: <bool>
description
drive: <float/array>
Central drive to the oscillators
"""
# Simulation setup
timestep = 1e-2
times = np.arange(0, duration, timestep)
n_iterations = len(times)
sim_parameters = SimulationParameters(
drive=drive,
amplitude_gradient=None,
phase_lag=None,
turn=None,
)
network = SalamandraNetwork(sim_parameters, n_iterations)
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(iteration=0))])
phases_log[0, :] = network.state.phases(iteration=0)
amplitudes_log = np.zeros([ n_iterations,len(network.state.amplitudes(iteration=0))])
amplitudes_log[0, :] = network.state.amplitudes(iteration=0)
freqs_log = np.zeros([n_iterations,len(network.robot_parameters.freqs) ])
freqs_log[0, :] = network.robot_parameters.freqs
outputs_log = np.zeros([n_iterations,len(network.get_motor_position_output(iteration=0))])
outputs_log[0, :] = network.get_motor_position_output(iteration=0)
drive_log = np.zeros([n_iterations,1])
# Run network ODE and log data
tic = time.time()
for i, time0 in enumerate(times[1:]):
if update:
drive = drive+timestep*0.2
network.robot_parameters.update(
SimulationParameters(
drive = drive ,
)
)
network.step(i, time0, timestep)
drive_log[i+1] = drive
phases_log[i+1, :] = network.state.phases(iteration=i+1)
amplitudes_log[i+1, :] = network.state.amplitudes(iteration=i+1)
outputs_log[i+1, :] = network.get_motor_position_output(iteration=i+1)
freqs_log[i+1, :] = network.robot_parameters.freqs
# # Alternative option
# phases_log[:, :] = network.state.phases()
# amplitudes_log[:, :] = network.state.amplitudes()
# outputs_log[:, :] = network.get_motor_position_output()
toc = time.time()
weights = network.robot_parameters.coupling_weights
bias = network.robot_parameters.phase_bias
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations,
toc - tic
))
# Implement plots of network results
plot_joint_angles(times,outputs_log, osc_right, osc_left, osc_legs)
plt.figure()
plt.plot(drive_log,freqs_log)
plt.legend(('Body','Limb'))
plt.xlabel('Drive')
plt.ylabel('Frequency [Hz]')
plt.grid(True)
plt.figure()
plt.plot(drive_log,phases_log)
#plt.legend(('Body','Limb'))
plt.xlabel('Drive')
plt.ylabel('Phases')
plt.grid(True)
plt.figure()
plt.plot(drive_log,amplitudes_log)
plt.legend(('Body','Limb'))
plt.xlabel('Drive')
plt.ylabel('Nominal Amplitude')
plt.grid(True)
plt.figure()
plt.plot(times,drive_log)
plt.xlabel('Time(s)')
plt.ylabel('Drive')
plt.grid(True)
def simulation(sim_parameters, arena='water', **kwargs):
"""Main"""
# Simulation options
# pylog.info('Creating simulation')
n_iterations = int(sim_parameters.duration / sim_parameters.timestep)
simulation_options = SimulationOptions.with_clargs(
timestep=sim_parameters.timestep,
n_iterations=n_iterations,
**kwargs,
)
# Arena
if arena == 'water':
water_surface = 0
arena = SimulationModels([
DescriptionFormatModel(path='arena_water.sdf',
spawn_options={
'posObj': [0, 0, water_surface],
'ornObj': [0, 0, 0, 1],
}),
GroundModel(position=[0, 0, -1]),
])
elif arena == 'amphibious':
water_surface = -0.1
arena = SimulationModels([
DescriptionFormatModel(path='arena_amphibious.sdf',
visual_options={
'path': 'BIOROB2_blue.png',
'rgbaColor': [1, 1, 1, 1],
'specularColor': [1, 1, 1],
}),
DescriptionFormatModel(path='arena_water.sdf',
spawn_options={
'posObj': [0, 0, water_surface],
'ornObj': [0, 0, 0, 1],
}),
])
else:
water_surface = -np.inf
arena = SimulationModels([GroundModel(position=[0, 0, 0])])
# Robot
network = SalamandraNetwork(sim_parameters=sim_parameters,
n_iterations=n_iterations)
sim, data = simulation_setup(
animat_sdf='salamandra_robotica.sdf',
arena=arena,
animat_options=SalamandraOptions.from_options(
dict(
water_surface=water_surface,
spawn_position=sim_parameters.spawn_position,
spawn_orientation=sim_parameters.spawn_orientation,
)),
simulation_options=simulation_options,
network=network,
)
# Run simulation
pylog.info('Running simulation')
# sim.run(show_progress=show_progress)
# contacts = data.sensors.contacts
# gps = data.sensors.gps
for iteration in sim.iterator(show_progress=False):
# print("iteration %d" %(iteration))
# pylog.info(np.asarray(
# contacts.reaction(
# iteration=iteration, # Current iteration
# sensor_i=0, # 0...3, one for each leg
# )
# ))
# network.robot_parameters.update_iteration(iteration)
# Position of head: gps.urdf_position(iteration=iteration, link_i=0)
# You can make changes to sim_parameters and then update with:
sim_parameters.current_iter = iteration
network.robot_parameters.update(sim_parameters)
assert iteration >= 0
# Terminate simulation
pylog.info('Terminating simulation')
sim.end()
return sim, data
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results
Parameters
----------
duration: <float>
Duration in [s] for which the network should be run
update: <bool>
description
drive: <float/array>
Central drive to the oscillators
"""
# 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 = SalamandraNetwork(timestep, parameters, n_iterations)
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(iteration=0))])
phases_log[0, :] = network.state.phases(iteration=0)
amplitudes_log = np.zeros(
[n_iterations,
len(network.state.amplitudes(iteration=0))])
amplitudes_log[0, :] = network.state.amplitudes(iteration=0)
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(iteration=0))])
outputs_log[0, :] = network.get_motor_position_output(iteration=0)
# Run network ODE and log data
tic = time.time()
for i, time0 in enumerate(times[1:]):
if update:
network.parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step(i, time0, timestep)
phases_log[i + 1, :] = network.state.phases(iteration=i + 1)
amplitudes_log[i + 1, :] = network.state.amplitudes(iteration=i + 1)
outputs_log[i + 1, :] = network.get_motor_position_output(iteration=i +
1)
freqs_log[i + 1, :] = network.parameters.freqs
# # Alternative option
# phases_log[:, :] = network.state.phases()
# amplitudes_log[:, :] = network.state.amplitudes()
# outputs_log[:, :] = network.get_motor_position_output()
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")
def run_network(duration, update=False, drive=0):
"""Run network without Webots and plot results
Parameters
----------
duration: <float>
Duration in [s] for which the network should be run
update: <bool>
description
drive: <float/array>
Central drive to the oscillators
"""
# Simulation setup
timestep = 5e-3
times = np.arange(0, duration, timestep)
n_iterations = len(times)
parameters = SimulationParameters(
drive=0,
amplitude_gradient=None,
phase_lag=None,
turn=None,
)
network = SalamandraNetwork(timestep, parameters, n_iterations)
osc_left = np.arange(10)
osc_right = np.arange(10, 20)
osc_legs = np.arange(20, 24)
drive_vec = np.linspace(0, 7, len(times))
# Logs
# global phases_log, amplitudes_log
phases_log = np.zeros(
[n_iterations, len(network.state.phases(iteration=0))])
phases_log[0, :] = network.state.phases(iteration=0)
amplitudes_log = np.zeros(
[n_iterations,
len(network.state.amplitudes(iteration=0))])
amplitudes_log[0, :] = network.state.amplitudes(iteration=0)
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(iteration=0))])
outputs_log[0, :] = network.get_motor_position_output(iteration=0)
# Run network ODE and log data
tic = time.time()
for i, time0 in enumerate(times[1:]):
drive = drive_vec[i]
if update:
network.parameters.update(
SimulationParameters(
drive=drive,
amplitude_gradient=parameters.amplitude_gradient,
phase_lag=parameters.phase_lag))
network.step(i, time0, timestep)
phases_log[i + 1, :] = network.state.phases(iteration=i + 1)
amplitudes_log[i + 1, :] = network.state.amplitudes(iteration=i + 1)
outputs_log[i + 1, :] = network.get_motor_position_output(iteration=i +
1)
freqs_log[i + 1, :] = network.parameters.freqs
# # Alternative option
# phases_log[:, :] = network.state.phases()
# amplitudes_log[:, :] = network.state.amplitudes()
# outputs_log[:, :] = network.get_motor_position_output()
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
# Implement plots of network results
plt.close('all')
plt.figure()
plt.plot(times, phases_log)
plt.grid()
plt.figure()
plt.plot(times, amplitudes_log)
plt.grid()
plt.figure()
for i in range(np.shape(outputs_log)[1]):
plt.plot(times, outputs_log[:, i] - i, label='Joint %s' % i)
plt.legend()
plt.grid()
def run_network(duration, update=False, drive=0):
"""Run network without Pybullet and plot results
Parameters
----------
duration: <float>
Duration in [s] for which the network should be run
update: <bool>
description
drive: <float/array>
Central drive to the oscillators
"""
# Simulation setup
timestep = 1e-2
times = np.arange(0, duration, timestep)
n_iterations = len(times)
sim_parameters = SimulationParameters(
drive=drive,
amplitude_gradient=None,
phase_lag=None,
turn=None,
)
network = SalamandraNetwork(sim_parameters, n_iterations)
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(iteration=0))])
phases_log[0, :] = network.state.phases(iteration=0)
amplitudes_log = np.zeros(
[n_iterations,
len(network.state.amplitudes(iteration=0))])
amplitudes_log[0, :] = network.state.amplitudes(iteration=0)
freqs_log = np.zeros([n_iterations, len(network.robot_parameters.freqs)])
freqs_log[0, :] = network.robot_parameters.freqs
outputs_log = np.zeros(
[n_iterations,
len(network.get_motor_position_output(iteration=0))])
outputs_log[0, :] = network.get_motor_position_output(iteration=0)
# Run network ODE and log data
tic = time.time()
for i, time0 in enumerate(times[1:]):
if update:
network.robot_parameters.update(
SimulationParameters(
# amplitude_gradient=None,
# phase_lag=None
))
network.step(i, time0, timestep)
phases_log[i + 1, :] = network.state.phases(iteration=i + 1)
amplitudes_log[i + 1, :] = network.state.amplitudes(iteration=i + 1)
outputs_log[i + 1, :] = network.get_motor_position_output(iteration=i +
1)
freqs_log[i + 1, :] = network.robot_parameters.freqs
# # Alternative option
# phases_log[:, :] = network.state.phases()
# amplitudes_log[:, :] = network.state.amplitudes()
# outputs_log[:, :] = network.get_motor_position_output()
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 of 8a")
# **********************************************************
# **********************************************************
# **********************************************************
# **********************************************************
# **********************************************************
# **********************************************************
### 8a
plt.figure("Body Joints in Walking Pattern")
body_labels = ['body joint ' + str(i + 1) for i in range(10)]
plot_results.plot_positions(times,
outputs_log[:, :10],
labels=body_labels,
ylabel=' ')
plt.figure("Limb Joints in Walking Pattern")
limb_labels = ['limb joint ' + str(i + 1) for i in range(4)]
plot_results.plot_positions(times,
outputs_log[:, 10:],
labels=limb_labels,
ylabel=' ')
plt.figure("phase of joints")
plt.plot(times, phases_log[:, 0], label='body joints')
plt.plot(times, phases_log[:, -1], label='limb joints')
plt.grid(True)
plt.xlabel("time [s]")
plt.ylabel('phase')
plt.legend()
plt.figure("amplitude of joints")
plt.plot(times, amplitudes_log[:, 0], label='body joints')
plt.plot(times, amplitudes_log[:, -1], label='limb joints')
plt.grid(True)
plt.xlabel("time [s]")
plt.ylabel('amplitude')
plt.legend()
plt.figure("frequency of joints")
plt.plot(times, freqs_log[:, 0], label='body joints')
plt.plot(times, freqs_log[:, -1], label='limb joints')
plt.grid(True)
plt.xlabel("time [s]")
plt.ylabel('frequency [Hz]')
plt.legend()
def run_network(duration, update=True, drive=0):
#
"""Run network without Pybullet and plot results
Parameters
----------
duration: <float>
Duration in [s] for which the network should be run
update: <bool>
description
drive: <float/array>
Central drive to the oscillators
"""
# Simulation setup
# timestep = 1e-2
timestep = 1e-1
times = np.arange(0, duration, timestep)
n_iterations = len(times)
sim_parameters = SimulationParameters(
drive_mlr=drive,
amplitude_gradient=None,
phase_lag=None,
turn=None,
# =============================================================================
# drive_mlr=2, # drive so the robot can walk
# amplitude_limb = 5,
# phase_lag= np.pi/2,
# phase_limb_body = np.pi/2,
# exercise_8f = True
# =============================================================================
# ...
)
network = SalamandraNetwork(sim_parameters, n_iterations)
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(iteration=0))])
phases_log[0, :] = network.state.phases(iteration=0)
amplitudes_log = np.zeros(
[n_iterations,
len(network.state.amplitudes(iteration=0))])
amplitudes_log[0, :] = network.state.amplitudes(iteration=0)
freqs_log = np.zeros([n_iterations, len(network.robot_parameters.freqs)])
freqs_log[0, :] = network.robot_parameters.freqs
freqs_calculated = np.zeros(
[n_iterations, len(network.robot_parameters.freqs)])
freqs_calculated[0, :] = network.robot_parameters.freqs
outputs_log = np.zeros(
[n_iterations,
len(network.get_motor_position_output(iteration=0))])
outputs_log[0, :] = network.get_motor_position_output(iteration=0)
drive = np.linspace(0.5, 5.5, len(times))
# Run network ODE and log data
tic = time.time()
for i, time0 in enumerate(tqdm(times[1:])):
# for i, time0 in enumerate(times[1:]):
if update:
network.robot_parameters.update(
SimulationParameters(
drive_mlr=drive[i + 1],
# amplitude_gradient=None,
# phase_lag = 1.5
), )
network.step(i, time0, timestep)
phases_log[i + 1, :] = network.state.phases(iteration=i + 1)
amplitudes_log[i + 1, :] = network.state.amplitudes(iteration=i + 1)
# outputs_log[i+1, :] = network.get_motor_position_output(iteration=i+1)
""" CHANGE THE FUNCTION FOR OUTPUTS"""
outputs_log[i + 1, :] = network.get_outputs(iteration=i + 1)
freqs_log[i + 1, :] = network.robot_parameters.freqs
# freqs_calculated =
# # Alternative option
# phases_log[:, :] = network.state.phases()
# amplitudes_log[:, :] = network.state.amplitudes()
# outputs_log[:, :] = network.get_motor_position_output()
toc = time.time()
# Network performance
pylog.info("Time to run simulation for {} steps: {} [s]".format(
n_iterations, toc - tic))
return times, drive, freqs_log, amplitudes_log, outputs_log, phases_log
def simulation(sim_parameters, arena='water', **kwargs):
"""Main"""
# Simulation options
pylog.info('Creating simulation')
n_iterations = int(sim_parameters.duration / sim_parameters.timestep)
simulation_options = SimulationOptions.with_clargs(
timestep=sim_parameters.timestep,
n_iterations=n_iterations,
**kwargs,
)
arenaName = arena
# Arena
if arena == 'water':
water_surface = 0
arena = SimulationModels([
DescriptionFormatModel(path='arena_water.sdf',
spawn_options={
'posObj': [0, 0, water_surface],
'ornObj': [0, 0, 0, 1],
}),
GroundModel(position=[0, 0, -1]),
])
elif arena == 'amphibious':
water_surface = -0.1
arena = SimulationModels([
DescriptionFormatModel(path='arena_amphibious.sdf',
visual_options={
'path': 'BIOROB2_blue.png',
'rgbaColor': [1, 1, 1, 1],
'specularColor': [1, 1, 1],
}),
DescriptionFormatModel(path='arena_water.sdf',
spawn_options={
'posObj': [0, 0, water_surface],
'ornObj': [0, 0, 0, 1],
}),
])
else:
water_surface = -np.inf
arena = SimulationModels([GroundModel(position=[0, 0, 0])])
# Robot
network = SalamandraNetwork(sim_parameters=sim_parameters,
n_iterations=n_iterations)
sim, data = simulation_setup(
animat_sdf='salamandra_robotica.sdf',
arena=arena,
animat_options=SalamandraOptions.from_options(
dict(
water_surface=water_surface,
spawn_position=sim_parameters.spawn_position,
spawn_orientation=sim_parameters.spawn_orientation,
)),
simulation_options=simulation_options,
network=network,
)
# Run simulation
pylog.info('Running simulation')
# sim.run(show_progress=show_progress)
# contacts = data.sensors.contacts
gps = data.sensors.gps
for iteration in sim.iterator(show_progress=True):
pos = gps.urdf_position(iteration=iteration, link_i=0)
#print(pos[0], sim_parameters.drive)
if arenaName == 'amphibious':
print(pos[0])
if pos[0] < -2.3:
sim_parameters.drive = 4
elif pos[
0] > -1.4 and sim_parameters.drive > sim_parameters.dhigh_L:
sim_parameters.drive = 1.5
if pos[0] < -3.5:
sim_parameters.turn = [0.5, 'Right']
elif pos[0] > -3.5:
sim_parameters.turn = [1, 'Right']
network.robot_parameters.update(sim_parameters)
assert iteration >= 0
# Terminate simulation
pylog.info('Terminating simulation')
sim.end()
return sim, data