46 lines
951 B
Python
46 lines
951 B
Python
import plotutils
|
|
import math
|
|
|
|
|
|
#STRATEGY = "Normal"
|
|
STRATEGY = "Boundary"
|
|
#STRATEGY = "FullNeighborhood"
|
|
RANGE = [-5.0, 5.0]
|
|
INITIAL_STEP = 1.0
|
|
STEP_SIZE = 0.05
|
|
LATENCY = 0.05
|
|
NOISE_FILTER = 1
|
|
|
|
|
|
robot.enable()
|
|
move_to_laser()
|
|
|
|
|
|
robot.set_motors_enabled(True)
|
|
|
|
current_x = robot_x.getPosition()
|
|
current_z = robot_z.getPosition()
|
|
|
|
|
|
|
|
|
|
class Distance(Readable):
|
|
def read(self):
|
|
ret = ue.readable.read()
|
|
ret = 0.0 if math.isnan(ret) else ret
|
|
return ret
|
|
|
|
laser_distance=Distance()
|
|
start = time.time()
|
|
r = hsearch([robot_x, robot_z],laser_distance, [RANGE[0], RANGE[0]], [RANGE[1], RANGE[1]], [INITIAL_STEP,INITIAL_STEP], [STEP_SIZE,STEP_SIZE], NOISE_FILTER, relative = True, maximum=True, latency = LATENCY, title = "Hill Climbing XZ")
|
|
|
|
|
|
|
|
print r.print()
|
|
opt_x, opt_z= r.getOptimalPosition()
|
|
offset_x, offset_z = opt_x - current_x, opt_z - current_z
|
|
|
|
print "offset_x: ", offset_x, " offset_z: ", offset_z
|
|
|
|
|