script/calibration/HillClimbingXZ.py

@@ -0,0 +1,45 @@
+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
+
+