def simulate(self):
# Create the model
self.cpg = control.CPG(self.cpg_conf)
self.pose = []
self.imu_sig = []
self.sens_sig = []
# Create the simulation handle
p = physics.Gazebo(view=True)
p.start()
# Wait for the gzserver process to be started
t_init = time.time()
while not p.is_sim_started():
time.sleep(0.001)
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Perform simulation loop
i = 0
t = 0
t_init = time.time()
prev_t = -1
while t < self.stop_time:
# Timeout if failure
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Get sim time
t = p.get_gazebo_time()
# Check that the gazebo timestep is larger than the CPG integration timestep
if t - prev_t > self.cpg.dt:
# Actuate
command = self.act(t)
p.actuate(command)
# Record simulation sensor signal
self.sens_sig.append(p.get_sensors().copy())
self.pose.append(p.get_pose().copy())
if t > 5:
self.imu_sig.append(p.get_imu().copy())
# Wait here not to overload the sensor vector
time.sleep(0.001)
prev_t = t
p.stop()
print("Simulation score = {0:.4f}".format(self.getScore()) +
" and Optim score = {0:.4f}".format(self.score))
return 0
def sim_rt(self, time_step=0.005):
# Create the simulation handle
p = physics.Gazebo("tigrillo_rt.world", view=False)
p.start()
# Wait for the gzserver process to be started
t_init = time.time()
while not p.is_sim_started():
time.sleep(0.001)
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Perform simulation loop
time_bias = self.start_time
rt_init = datetime.datetime.now()
st = 0
j = 0
t_init = time.time()
while st < (self.stop_time - self.start_time):
# Timeout if failure
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Actuate
st = p.get_gazebo_time()
rt = rt_init + datetime.timedelta(seconds=((j + 1) * time_step))
command = self.act(st + time_bias)
p.actuate(command)
# Record simulation sensor signal
s = p.get_sensors().copy()
i = p.get_imu().copy()
s["time"] += time_bias
self.sens_sim_sig.append(s)
self.imu_sim_sig.append(i)
self.mot_sim_sig.append({
"FL": command[0],
"FR": command[1],
"BL": command[2],
"BR": command[3],
"time": st + time_bias
})
# Pause
pause.until(rt)
j += 1
# Stop the simulator
p.stop()
return 0
def sim(self):
# Create the simulation handle
p = physics.Gazebo()
p.start()
# Wait for the gzserver process to be started
t_init = time.time()
while not p.is_sim_started():
time.sleep(0.001)
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Perform simulation loop
t = self.start_time
i = 0
t_prev = 0
t_init = time.time()
while t < self.stop_time:
# Timeout if failure
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Get sim time
t = p.get_gazebo_time() + self.start_time
if t == t_prev:
continue
# Actuate
command = self.act(t)
p.actuate(command)
# Record simulation sensor signal
s = p.get_sensors().copy()
i = p.get_imu().copy()
s["time"] += self.start_time
self.sens_sim_sig.append(s)
self.imu_sim_sig.append(i)
self.mot_sim_sig.append({
"FL": command[0],
"FR": command[1],
"BL": command[2],
"BR": command[3],
"time": t
})
# Wait here not to overload the sensor vector
t_prev = t
p.stop()
return 0
def sim_sync(self, time_step=0.01):
# Create the simulation handle
p = physics.Gazebo("tigrillo_slow.world", view=False)
p.start()
# Wait for the gzserver process to be started, then directly pause it
t_init = time.time()
while not p.is_sim_started():
time.sleep(0.001)
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
p.pause_gazebo()
# Perform simulation loop
time_bias = self.start_time
st = 0
j = 0
t_init = time.time()
while st < (self.stop_time - self.start_time):
# Timeout if failure
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Actuate
st = p.get_gazebo_time()
command = self.act(st + time_bias)
p.actuate(command)
# Record simulation sensor signal
s = p.get_sensors().copy()
i = p.get_imu().copy()
s["time"] += time_bias
self.sens_sim_sig.append(s)
self.imu_sim_sig.append(i)
self.mot_sim_sig.append({
"FL": command[0],
"FR": command[1],
"BL": command[2],
"BR": command[3],
"time": st + time_bias
})
# Update gazebo
p.step_gazebo(int(time_step * 1000))
j += 1
# Stop the simulator
p.stop()
return 0
def sim(self):
# Create the simulation handle
p = physics.Gazebo()
p.start()
# Wait for the gzserver process to be started
t_init = time.time()
while not p.is_sim_started():
time.sleep(0.001)
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Perform simulation loop
i = 0
t = 0
t_init = time.time()
prev_t = -1
while t < self.stop_time:
# Timeout if failure
if (time.time() - t_init) > self.sim_timeout:
p.stop()
return -1
# Get sim time
t = p.get_gazebo_time()
# Actuate
command = self.act(t)
p.actuate(command)
# Record simulation sensor signal
self.sens_sig.append(p.get_sensors().copy())
self.mot_sig.append(p.get_motors().copy())
self.imu_sig.append(p.get_imu().copy())
self.pose.append(p.get_pose().copy())
# Wait here not to overload the sensor vector
time.sleep(0.001)
prev_t = t
p.stop()
return 0
def __init__(self, win, sim_id=0, time_step=0.02):
self.physics = physics.Gazebo("tigrillo_rt.world", view=True)
self.sim_id = sim_id
self.win = win
self.time_step = time_step
ros.init_node('view', anonymous=True)
ros.on_shutdown(utils.cleanup)
# Simulation data retrieved from file
data = self.win.sel_conf[self.sim_id]
data_init = self.win.sel_conf[0]
self.model(data, data_init)
self.file = "/home/gabs48/.gazebo/models/tigrillo/model.sdf"
self.stop_time = data_init["stop_time"]
self.sim_timeout = data_init["sim_timeout"]
self.score = data["score"]
def __init__(self, win, sim_id=0, time_step=0.02):
self.physics = physics.Gazebo("tigrillo_rt.world", view=True)
self.sim_id = sim_id
self.win = win
self.time_step = time_step
ros.init_node('view', anonymous=True)
ros.on_shutdown(utils.cleanup)
# Simulation data retrieved from file
data = self.win.sel_conf[sim_id]
data_init = self.win.sel_conf[0]
self.f_fl = interp1d(data_init["t_act"],
data_init["fl_act"],
assume_sorted=False,
fill_value="extrapolate")
self.f_fr = interp1d(data_init["t_act"],
data_init["fr_act"],
assume_sorted=False,
fill_value="extrapolate")
self.f_bl = interp1d(data_init["t_act"],
data_init["bl_act"],
assume_sorted=False,
fill_value="extrapolate")
self.f_br = interp1d(data_init["t_act"],
data_init["br_act"],
assume_sorted=False,
fill_value="extrapolate")
self.params_unormed = utils.unorm(data["params"],
data_init["params_min"],
data_init["params_max"])
self.config = data_init["config"]
self.file = "/home/gabs48/.gazebo/models/tigrillo/model.sdf" #data_init["model_file"]
self.time_bias = data_init["start_time"]
self.sim_time = data_init["stop_time"] - data_init["start_time"]