def init_vrep():
vrep.simxFinish(-1)
client_id = vrep.simxStart('127.0.0.1',19997,True,True,5000,5)
assert client_id != -1, 'Failed to connect to remote API server'
vrep.simxSynchronous(client_id, 1)
ENV_VAR['client_id'] = client_id
print('Initialized simulation environment')
def start(self):
vrep.simxFinish(-1)
self.client_id = vrep.simxStart('127.0.0.1', 19997, True, True, 5000,
5)
if self.client_id == 1:
raise ConnectionError
vrep.simxSynchronous(self.client_id, 1)
print('Initialized simulation environment with client id {}'.format(
self.client_id))
def incline_obj_f(x):
print('\nParameters: ' + str(x))
try:
CLIENTID = ENV_VAR['client_id']
load_scene('scenes/inclines/' + str(int(x[3])) + 'deg.ttt')
walker = ENV_VAR['walker']
vrep.simxSynchronous(CLIENTID, 1)
vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking)
errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
assert errorCode in VALID_ERROR_CODES, 'Error getting object position'
gait = DualTripod
gait.f = x[0]
gait.phase_offset = x[1]
gait.coupling_phase_biases = gait.coupling_phase_biases = \
gait.generate_coupling(gait.phase_groupings)
cpg = CpgController(gait)
for _ in range(1000):
cpg.update(plot=False)
print('Running trial...')
for _ in range(100):
output = cpg.output()
for i in range(6):
walker.legs[i].extendZ(output[i][0] * x[2])
walker.legs[i].extendX(output[i][1] * x[2])
vrep.simxSynchronousTrigger(CLIENTID)
cpg.update()
errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking)
vrep.simxGetPingTime(CLIENTID)
vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking)
# to be maximized
print('Objective: ' + str(start[0] - end[0]))
return np.array([start[0] - end[0]])
except Exception:
print('Encountered an exception, disconnecting from remote API server')
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot)
vrep.simxGetPingTime(CLIENTID)
vrep.simxFinish(CLIENTID)
traceback.print_exc()
exit()
def objective(x):
x = np.asarray(x)[0]
print('\nParameters: ' + str(x))
try:
# Clean up VREP
self.run()
# Get starting position
start = self.get_pos(self.walker.base_handle)
# Assign parameters
self.walker.set_left_bias(x[2])
self.walker.set_right_bias(x[3])
self.set_cpg(gait, x[0], x[1])
for _ in range(1000):
self.cpg.update(plot=False)
# Run the simulation
print('Running trial...')
self.walk(max_steps)
# Calculate how far the robot walked
end = self.get_pos(self.walker.base_handle)
dist = self.calc_dist(start, end)
print('Distance traveled: ' + str(dist))
# Clean up VREP
self.stop()
return np.array([dist])
except (ConnectionError, SceneLoadingError, WalkerLoadingError,
MotorLoadingError, HandleLoadingError) as e:
print("Encountered an exception: {} "
"disconnecting from remote API server".format(e))
vrep.simxFinish(self.client_id)
traceback.print_exc()
self.stop()
raise e
def objective(x):
try:
walker.reset()
vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking)
errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
assert errorCode in VALID_ERROR_CODES, 'Walker not found'
x = np.asarray(x)[0]
print('\nParameters: ' + str(x))
cpg_gait = gait
if parameter_mode is 'normal':
cpg_gait.f = x[0]
cpg_gait.phase_offset = x[1]
walker.set_left_bias(x[2])
walker.set_right_bias(x[3])
elif parameter_mode is 'contextual':
cpg_gait.f = x[0]
cpg_gait.phase_offset = x[1]
walker.set_left_bias(x[2])
walker.set_right_bias(x[2])
elif parameter_mode is 'discovery':
cpg_gait.f = x[0]
cpg_gait.phase_offset = x[1]
cpg_gait.phase_groupings = {
0: x[2],
1: x[3],
2: x[4],
3: x[5],
4: x[6],
5: x[7]
}
else:
raise Exception('Invalid parameter mode')
cpg_gait.coupling_phase_biases = \
cpg_gait.generate_coupling(cpg_gait.phase_groupings)
cpg = CpgController(cpg_gait)
for _ in range(1000):
cpg.update(plot=False)
print('Running trial...')
for _ in range(steps):
output = cpg.output()
if objective_mode is not 'normal':
walker.update_energy()
for i in range(6):
walker.legs[i].extendZ(output[i][0])
walker.legs[i].extendX(output[i][1])
wait(1)
cpg.update()
errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
if objective_mode is not 'single':
total_energy = walker.calculate_energy() / 5
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking)
vrep.simxGetPingTime(CLIENTID)
vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking)
if objective_mode is 'target':
target = x[-2], x[-1]
return np.array([distance(end, target)])
else:
distance = start[0] - end[0]
if penalize_offset: distance += (.1 * np.abs(end[1] - start[1]))
if objective_mode is 'single':
print('Distance traveled: ' + str(distance))
return np.array([distance])
else:
print('Distance traveled: ' + str(distance))
print('Total power: ' + str(total_energy) + 'W')
return np.array([distance, total_energy])
except Exception as e:
print('Encountered an exception, disconnecting from remote API server')
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot)
vrep.simxGetPingTime(CLIENTID)
vrep.simxFinish(CLIENTID)
traceback.print_exc()
exit()
def exit_vrep():
vrep.simxFinish(ENV_VAR['client_id'])
print('Exited simulation environment')
def turning_obj_f(x):
print('\nParameters: ' + str(x))
try:
CLIENTID = ENV_VAR['client_id']
walker = ENV_VAR['walker']
errorCode,baseCollectionHandle=\
vrep.simxGetCollectionHandle(CLIENTID, 'Base', \
vrep.simx_opmode_blocking)
vrep.simxSynchronous(CLIENTID, 1)
vrep.simxStartSimulation(CLIENTID, vrep.simx_opmode_blocking)
walker.reset()
errorCode, start = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
assert errorCode in VALID_ERROR_CODES, 'Error getting object position'
q1 = vrep.simxGetObjectGroupData(CLIENTID, baseCollectionHandle, \
7, vrep.simx_opmode_blocking)
o1 = quatToDirection(q1[3]) + np.pi
x[2] = 15 + (3 * x[2])
x[3] = x[3] * 2 * np.pi / 10
gait = DualTripod
gait.f = x[2]
gait.phase_offset = x[3]
gait.coupling_phase_biases=gait.generate_coupling(gait.phase_groupings)
walker.set_left_bias(x[0] / 10)
walker.set_right_bias(x[1] / 10)
cpg = CpgController(gait)
for _ in range(1000):
cpg.update(plot=False)
print('Running trial...')
for _ in range(100):
output = cpg.output()
walker.update_energy()
for i in range(6):
walker.legs[i].extendZ(output[i][0])
walker.legs[i].extendX(output[i][1])
vrep.simxSynchronousTrigger(CLIENTID)
cpg.update()
total_energy = walker.calculate_energy() / 5
errorCode, end = vrep.simxGetObjectPosition(CLIENTID, \
walker.base_handle, -1, vrep.simx_opmode_blocking)
q2 = vrep.simxGetObjectGroupData(CLIENTID, baseCollectionHandle, \
7, vrep.simx_opmode_blocking)
o2 = quatToDirection(q2[3]) + np.pi
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_blocking)
vrep.simxGetPingTime(CLIENTID)
vrep.simxClearIntegerSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearStringSignal(CLIENTID, '', vrep.simx_opmode_blocking)
vrep.simxClearFloatSignal(CLIENTID, '', vrep.simx_opmode_blocking)
displacement = np.array(end) - np.array(start)
orientation = o2 - o1
info = np.array([total_energy, displacement[0], displacement[1], \
orientation])
objective = distance(displacement, (x[4], x[5]))
print('Orientation change: ' + str(orientation))
print('Total power: ' + str(total_energy) + 'W')
print('Target offset: ' + str(objective))
return np.array([objective]), info
except Exception:
print('Encountered an exception, disconnecting from remote API server')
vrep.simxStopSimulation(CLIENTID, vrep.simx_opmode_oneshot)
vrep.simxGetPingTime(CLIENTID)
vrep.simxFinish(CLIENTID)
traceback.print_exc()
exit()
def objective(x, cache_walker=True):
"""
x[0] = cpg frequency [1, 45]
x[1] = cpg phase offset [-np.pi, np.pi]
x[2] = left bias of walker [0, 1]
x[3] = right bias of walker [0, 1]
x[4] = front pair leg scaling [.8, 1.2]
x[5] = middle pair leg scaling [.8, 1.2]
x[6] = rear pair leg scaling [.8, 1.2]
:return:
"""
self.start()
x = np.asarray(x)[0]
print('\nParameters: ' + str(x))
try:
# Clean up VREP
if cache_walker and \
self.last_walker_params is not None and \
np.array_equal(x[4:], self.last_walker_params):
self.run()
self.load_prev_walker()
else:
self.exit()
self.start()
self.load_scene()
self.load_walker()
self.run()
# Assign parameters
self.scale_walker(x[4:])
# Cache walker
self.walker_filename = "logs/models/{}.ttm" \
.format(time.strftime("%Y.%m.%d-%H.%M.%S"))
self.last_walker_params = x[4:]
self.save_walker(self.walker_filename)
# Set the other parameters
self.set_cpg(gait, x[0], x[1])
self.walker.set_left_bias(x[2])
self.walker.set_right_bias(x[3])
# Get starting position
start = self.get_pos(self.walker.base_handle)
# Run the simulation
print('Running trial...')
self.walk(max_steps)
# Calculate how far the robot walked
end = self.get_pos(self.walker.base_handle)
dist = self.calc_dist(start, end)
print("Distance traveled: {}".format(dist))
# Clean up VREP
self.remove_walker()
self.close_scene()
self.stop()
self.exit()
return np.array([dist])
except SceneLoadingError as e:
print(str(e))
self.stop()
self.close_scene()
except (ConnectionError, WalkerLoadingError, MotorLoadingError,
HandleLoadingError) as e:
print("Encountered an exception: {} "
"disconnecting from remote API server".format(e))
vrep.simxFinish(self.client_id)
traceback.print_exc()
self.stop()
self.close_scene()
raise e
def exit(self):
print("Exiting VREP")
vrep.simxFinish(self.client_id)
def objective(x, cache_walker=True):
"""
8 sw, 6 hw
x[0] = f # Frequency [1, 45]
x[1] = phase_offset # phase difference btwn leg and foot. [-pi, pi]
x[2] = R_l # Sequence of leg extension length. [0, .04]
x[3] = R_f # Sequence of foot extension lengths. [0, .04]
x[4] = X_l # Sequence of leg offset values. [0, .04]
x[5] = X_f # Sequence of foot offset values. [0, .04]
x[6] = left bias of walker [.5, 1]
x[7] = right bias of walker [.5, 1]
x[8] = front left leg scaling [.8, 1.2]
x[9] = front right leg scaling [.8, 1.2]
x[11] = middle left leg scaling [.8, 1.2]
x[12] = middle right leg scaling [.8, 1.2]
x[13] = rear left pair leg scaling [.8, 1.2]
x[14] = rear right pair leg scaling [.8, 1.2]
walker params = x[8:]
:return:
"""
if not is_1D(x):
x = np.asarray(x)[0]
else:
x = np.asarray(x)
self.start()
gait_params = x[:6]
left_bias = x[6]
right_bias = x[7]
walker_params = x[8:]
print("walker params", walker_params)
print('\nParameters: ' + str(x))
try:
# Clean up VREP
if cache_walker and \
self.last_walker_params is not None and \
np.array_equal(walker_params, self.last_walker_params):
self.run()
self.load_prev_walker()
else:
self.exit()
self.start()
self.load_scene()
self.load_walker()
self.run()
# Assign parameters
self.scale_walker(walker_params)
# Cache walker
self.walker_filename = "logs/models/{}.ttm" \
.format(time.strftime("%Y.%m.%d-%H.%M.%S"))
self.last_walker_params = walker_params
self.save_walker(self.walker_filename)
# Set the other parameters
self.set_cpg(build_gait(gait_params))
self.walker.set_left_bias(left_bias)
self.walker.set_right_bias(right_bias)
# Get starting position
start = self.get_pos(self.walker.base_handle)
# Run the simulation
print('Running trial...')
self.walk(max_steps)
# Calculate how far the robot walked
end = self.get_pos(self.walker.base_handle)
dist = self.calc_dist(start, end)
print("Distance traveled: {}".format(dist))
# Clean up VREP
self.remove_walker()
self.close_scene()
self.stop()
self.exit()
return np.array([dist])
except SceneLoadingError as e:
print(str(e))
self.stop()
self.close_scene()
except (ConnectionError, WalkerLoadingError, MotorLoadingError,
HandleLoadingError) as e:
print("Encountered an exception: {} "
"disconnecting from remote API server".format(e))
vrep.simxFinish(self.client_id)
traceback.print_exc()
self.stop()
self.close_scene()
raise e