def set_cpg(self, gait, f, phase_offset):
cpg_gait = gait
cpg_gait.f = f
cpg_gait.phase_offset = phase_offset
cpg_gait.coupling_phase_biases = \
cpg_gait.generate_coupling(cpg_gait.phase_groupings)
self.cpg = CpgController(cpg_gait)
for _ in range(1000):
self.cpg.update(plot=False)
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):
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 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()
class DistanceSimulation(Simulation):
def __init__(self, scene=DEFAULT_SCENE, walker=DEFAULT_WALKER):
super(DistanceSimulation, self).__init__(scene, walker)
self.cpg = None
def set_cpg(self, gait, f, phase_offset):
cpg_gait = gait
cpg_gait.f = f
cpg_gait.phase_offset = phase_offset
cpg_gait.coupling_phase_biases = \
cpg_gait.generate_coupling(cpg_gait.phase_groupings)
self.cpg = CpgController(cpg_gait)
for _ in range(1000):
self.cpg.update(plot=False)
def walk(self, steps):
assert self.cpg is not None, "Can't walk without a cpg loaded!"
for _ in range(steps):
output = self.cpg.output()
for i in range(6):
self.walker.legs[i].extendZ(output[i][0])
self.walker.legs[i].extendX(output[i][1])
self.wait(1)
self.cpg.update()
def get_obj_f(self, max_steps, gait=DualTripod):
self.start()
self.load_scene()
self.load_walker()
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
return objective
@staticmethod
def calc_dist(start, end):
dist = start[0] - end[0]
return dist + (.1 * np.abs(end[1] - start[1]))
def set_cpg(self, gait, f=None, phase_offset=None):
self.cpg = CpgController(gait)
for _ in range(1000):
self.cpg.update(plot=False)
class JointObj3(DistanceSimulation):
def set_cpg(self, gait, f=None, phase_offset=None):
self.cpg = CpgController(gait)
for _ in range(1000):
self.cpg.update(plot=False)
def scale_walker(self, x):
self.walker.scale_vleg(leg_mapping["front_l"], x[0])
self.walker.scale_vleg(leg_mapping["front_r"], x[1])
self.walker.scale_vleg(leg_mapping["mid_l"], x[2])
self.walker.scale_vleg(leg_mapping["mid_r"], x[3])
self.walker.scale_vleg(leg_mapping["rear_l"], x[4])
self.walker.scale_vleg(leg_mapping["rear_r"], x[5])
def get_obj_f(self, max_steps, gait=None):
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
return objective