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 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)
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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 log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(self.iteration, i,
self.position_sensors[i].getValue())
# Velocity
self.log.log_joint_velocity(self.iteration, i, motor.getVelocity())
# Command
self.log.log_joint_cmd(self.iteration, i,
motor.getTargetPosition())
# Torque
self.log.log_joint_torque(self.iteration, i,
motor.getTorqueFeedback())
# Torque feedback
self.log.log_joint_torque_feedback(self.iteration, i,
motor.getTorqueFeedback())
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
# Log data
self.log_iteration()
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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.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
)
#GPS stuff
self.waterPosx = 0
self.NetworkParameters = self.network.parameters
self.SimulationParameters = parameters
self.doTransition = False
self.keyboard = Keyboard()
self.keyboard.enable(samplingPeriod=100)
self.lastkey = 0
def log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(
self.iteration, i,
self.position_sensors[i].getValue()
)
# # Velocity
# self.log.log_joint_velocity(
# self.iteration, i,
# motor.getVelocity()
# )
# Command
self.log.log_joint_cmd(
self.iteration, i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, i,
motor.getTorqueFeedback()
)
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10+i,
self.position_sensors[10+i].getValue()
)
# # Velocity
# self.log.log_joint_velocity(
# self.iteration, i,
# motor.getVelocity()
# )
# Command
self.log.log_joint_cmd(
self.iteration, 10+i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
positions[self.N_BODY_JOINTS+i] - np.pi/2
)
key=self.keyboard.getKey()
if (key==ord('A') and key is not self.lastkey):
print('Turning left')
self.SimulationParameters.turnRate = [0.5,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('D') and key is not self.lastkey):
print('Turning right')
self.SimulationParameters.turnRate = [1,0.5]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('W') and key is not self.lastkey):
print('Going forward')
self.SimulationParameters.turnRate = [1,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.SimulationParameters.Backwards = False
self.NetworkParameters.set_phase_bias(self.SimulationParameters)
self.lastkey = key
if (key==ord('S') and key is not self.lastkey):
print('Going backward')
self.SimulationParameters.Backwards = True
self.NetworkParameters.set_phase_bias(self.SimulationParameters)
self.SimulationParameters.turnRate = [1,1]
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.lastkey = key
if (key==ord('T') and key is not self.lastkey):
if self.doTransition:
print('Disabling transition')
self.doTransition = False
else:
print('Enabling transition')
self.doTransition = True
self.lastkey = key
if self.doTransition:
gpsPos = self.gps.getValues()
if gpsPos[0] < self.waterPosx+2 and gpsPos[0] > self.waterPosx -0.5:
gain = 4/2.5*(gpsPos[0]+0.5) + 1
self.SimulationParameters.drive = gain
#print('Transitioning')
self.NetworkParameters.set_nominal_amplitudes(self.SimulationParameters)
self.NetworkParameters.set_frequencies(self.SimulationParameters)
# Log data
self.log_iteration()
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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.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 log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(self.iteration, i,
self.position_sensors[i].getValue())
# Command
self.log.log_joint_cmd(self.iteration, i,
motor.getTargetPosition())
# Torque
self.log.log_joint_torque(self.iteration, i,
motor.getTorqueFeedback())
# Torque feedback
self.log.log_joint_torque_feedback(self.iteration, i,
motor.getTorqueFeedback())
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10 + i,
self.position_sensors[10 + i].getValue())
# Command
self.log.log_joint_cmd(self.iteration, 10 + i,
motor.getTargetPosition())
# Torque
self.log.log_joint_torque(self.iteration, 10 + i,
motor.getTorqueFeedback())
# Torque feedback
self.log.log_joint_torque_feedback(self.iteration, 10 + i,
motor.getTorqueFeedback())
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
#print(self.gps.getValues()[0])
if self.gps.getValues()[0] > 0.:
self.network.parameters.drive_mlr = 3.5
freqamp = computedrive.computefreqamp(
self.network.parameters.drive_mlr, False)
self.network.parameters.freqs = freqamp[0]
self.network.parameters.nominal_amplitudes = freqamp[1]
self.network.parameters.set_frequencies(self.network.parameters)
self.network.parameters.set_nominal_amplitudes(
self.network.parameters)
#frequency = (np.ones((self.network.parameters.n_oscillators,1))*vfreq)[:,0] #2Hz
#self.network.parameters.freqs = np.ones(self.network.parameters.n_oscillators)*frequency
#nominal_amplitude = np.ones(self.network.parameters.n_body_joints*2) *amplitude_value
#self.network.parameters.nominal_amplitude = np.concatenate((nominal_amplitude, np.zeros(4)), axis=None)
#self.network.parameters.set_frequencies(self.network.parameters)
#self.network.parameters.set_nominal_amplitudes(self.network.parameters)
#elif self.gps.getValues()[0]<0.4:
#drive=1
#freqamp=computedrive.computefreqamp(drive, False)
#self.network.parameters.freqs=freqamp[0]
#self.network.parameters.nominal_amplitudes=freqamp[1]
#self.network.parameters.set_frequencies(self.network.parameters)
#self.network.parameters.set_nominal_amplitudes(self.network.parameters)
#self.network.parameters.update(self.network.parameters)
#drive=1
#amplitude_value=0.15+(0.25/4)*(drive-1)
#vfreq=1+(0.5)*(drive-1)
#frequency = (np.ones((self.network.parameters.n_oscillators,1))*vfreq)[:,0] #2Hz
#self.network.parameters.freqs = np.ones(self.network.parameters.n_oscillators)*frequency
#nominal_amplitude = np.ones(self.network.parameters.n_body_joints*2) *amplitude_value
#self.network.parameters.nominal_amplitude = np.concatenate((nominal_amplitude, np.ones(4)*self.network.parameters.amplitude_leg_nominal), axis=None)
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(positions[self.N_BODY_JOINTS + i] -
np.pi / 2)
# Log data
self.log_iteration()
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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.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=10,
n_joints=self.N_BODY_JOINTS+self.N_LEGS,
filename=logs,
timestep=1e-3*timestep,
**parameters
)
def log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(
self.iteration, i,
self.position_sensors[i].getValue()
)
# Velocity
'''self.log.log_joint_velocity(
self.iteration, i,
motor.getVelocity()
)'''
# Command
self.log.log_joint_cmd(
self.iteration, i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, i,
motor.getTorqueFeedback()
)
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10+i,
self.position_sensors[10+i].getValue()
)
# Command
self.log.log_joint_cmd(
self.iteration, 10+i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# add network outputs logging
self.log.log_network_state(
self.iteration,
self.network.state
)
self.log.log_network_output(
self.iteration,
self.network.get_motor_position_output()
)
self.log.log_gps(
self.iteration,
self.gps.getValues()
)
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
positions[self.N_BODY_JOINTS+i] - np.pi/2
)
# Log data
self.log_iteration()
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')
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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.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 get_coord(self):
return self.gps.getValues()
def log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(
self.iteration, i,
self.position_sensors[i].getValue()
)
# Command
self.log.log_joint_cmd(
self.iteration, i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, i,
motor.getTorqueFeedback()
)
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10+i,
self.position_sensors[10+i].getValue()
)
# Command
self.log.log_joint_cmd(
self.iteration, 10+i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
def step(self,parameters):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
if parameters.flag == "9g" and self.gps.getValues()[0]>0.4 and parameters.toggle=="walk":
parameters.toggle="swim"
parameters.drive=4
print("d={}".format(parameters.drive))
self.network.parameters.update(parameters)
if parameters.flag == "9g" and self.gps.getValues()[0]<0.2 and parameters.toggle=="swim":
parameters.drive=2
print("d={}".format(parameters.drive))
self.network.parameters.update(parameters)
parameters.toggle="walk"
self.network.reset_leg_phases()
if parameters.flag=="9d1" and self.iteration==2000:
print("turn on")
parameters.turning=True
self.network.parameters.update(parameters)
if parameters.flag=="9d1" and self.iteration==4000:
print("turn off")
parameters.turning=False
self.network.parameters.update(parameters)
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
if np.all(abs(self.network.parameters.nominal_amplitudes[20:24])<=0.0001):
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
round(self.position_sensors[i+10].getValue()/(2*math.pi))*2*math.pi-math.pi/2
)
else:
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
positions[self.N_BODY_JOINTS+i] - np.pi/2
)
# Log data
self.log_iteration()
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)
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
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.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 log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
# for values of x bigger than 0 we are swimming
if self.gps.getValues()[0] > 0.7:
self.network.parameters.drive = 4.5
self.network.parameters.freqs[:
20] = 0.2 * self.network.parameters.drive + 0.3
self.network.parameters.freqs[20:] = 0
self.network.parameters.nominal_amplitudes[:
20] = 0.065 * self.network.parameters.drive + 0.196
self.network.parameters.nominal_amplitudes[20:] = 0
self.network.parameters.turn = [0.1, 'Right']
else:
self.network.parameters.turn = [1, 'Right']
self.network.parameters.drive = 1.5
self.network.parameters.freqs[:
20] = 0.2 * self.network.parameters.drive + 0.3
self.network.parameters.freqs[
20:] = 0.2 * self.network.parameters.drive
self.network.parameters.nominal_amplitudes[:
20] = 0.065 * self.network.parameters.drive + 0.196
self.network.parameters.nominal_amplitudes[
20:] = 0.131 * self.network.parameters.drive + 0.131
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(self.iteration, i,
self.position_sensors[i].getValue())
# Command
self.log.log_joint_cmd(self.iteration, i,
motor.getTargetPosition())
# Torque
self.log.log_joint_torque(self.iteration, i,
motor.getTorqueFeedback())
# Torque feedback
self.log.log_joint_torque_feedback(self.iteration, i,
motor.getTorqueFeedback())
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10 + i,
self.position_sensors[10 + i].getValue())
# Command
self.log.log_joint_cmd(self.iteration, 10 + i,
motor.getTargetPosition())
# Torque
self.log.log_joint_torque(self.iteration, 10 + i,
motor.getTorqueFeedback())
# Torque feedback
self.log.log_joint_torque_feedback(self.iteration, 10 + i,
motor.getTorqueFeedback())
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(positions[self.N_BODY_JOINTS + i] -
np.pi / 2)
# Log data
self.log_iteration()
class SalamanderCMC(object):
"""Salamander robot for CMC"""
N_BODY_JOINTS = 10
N_LEGS = 4
X_HIGH_POS = 1.0
X_LOW_POS = 0.25
SWIM = True
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 log_iteration(self):
"""Log state"""
self.log.log_link_positions(self.iteration, 0, self.gps.getValues())
for i, motor in enumerate(self.motors_body):
# Position
self.log.log_joint_position(
self.iteration, i,
self.position_sensors[i].getValue()
)
# Command
self.log.log_joint_cmd(
self.iteration, i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, i,
motor.getTorqueFeedback()
)
for i, motor in enumerate(self.motors_legs):
# Position
self.log.log_joint_position(
self.iteration, 10+i,
self.position_sensors[10+i].getValue()
)
# Command
self.log.log_joint_cmd(
self.iteration, 10+i,
motor.getTargetPosition()
)
# Torque
self.log.log_joint_torque(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
# Torque feedback
self.log.log_joint_torque_feedback(
self.iteration, 10+i,
motor.getTorqueFeedback()
)
def step(self):
"""Step"""
# Increment iteration
self.iteration += 1
# Update network
self.network.step()
positions = self.network.get_motor_position_output()
# Update control
for i in range(self.N_BODY_JOINTS):
self.motors_body[i].setPosition(positions[i])
for i in range(self.N_LEGS):
self.motors_legs[i].setPosition(
positions[self.N_BODY_JOINTS+i] - np.pi/2
)
# Log data
self.log_iteration()
# Retrieve GPS to change from walking to swimming
if self.iteration == 1:
self.enable = True
pos = self.gps.getValues()
if self.network.parameters.enable_transitions:
if pos[0] > self.X_HIGH_POS:
if not self.SWIM:
self.SWIM = True
self.network.parameters.drive_left = 4.0
self.network.parameters.drive_right = 4.0
elif pos[0] < self.X_LOW_POS:
self.network.parameters.drive_left = 2.0
self.network.parameters.drive_right = 2.0
if self.SWIM:
self.network.state.phases = 1e-4 * np.random.ranf(self.network.parameters.n_oscillators)
self.SWIM = False
self.network.parameters.set_saturation_params(self.network.parameters)
self.network.parameters.saturate_params()
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')