Élie Michel 5 years ago
parent
commit
202ffdeaa0
3 changed files with 52 additions and 23 deletions
  1. 31
    20
      control_law.py
  2. BIN
      door.blend
  3. 21
    3
      emulate.py

+ 31
- 20
control_law.py View File

@@ -1,5 +1,8 @@
1 1
 #!/usr/bin/env python
2
-import bpy
2
+try:
3
+    import bpy
4
+except ImportError:
5
+    print("Not running in Blender.")
3 6
 import numpy as np
4 7
 import subprocess
5 8
 
@@ -19,7 +22,10 @@ class Positions:
19 22
 
20 23
     def get_positions_in_image_plane(self, img):
21 24
         """Returns the position of the two markers in the image plane."""
22
-        basepath = bpy.path.abspath('//')
25
+        try:
26
+            basepath = bpy.path.abspath('//')
27
+        except NameError:
28
+            basepath = "."
23 29
         out = subprocess.check_output([basepath + "/blob",
24 30
                                        img,
25 31
                                        "%s" % self.markers_color['h']])
@@ -70,20 +76,21 @@ class Positions:
70 76
                          markers_pos[1]['x'] * markers_pos[0]['y'])**2 +
71 77
                         self.alpha_x**2 * (markers_pos[1]['y'] -
72 78
                                            markers_pos[0]['y'])**2)
73
-        ksi = np.arccosh(ksi_num / denom)
79
+        ksi = np.arccosh(abs(ksi_num / denom))
74 80
         eta_num = (markers_pos[1]['y'] * np.sqrt(markers_pos[0]['x']**2 +
75 81
                                                  self.alpha_x**2) -
76 82
                    markers_pos[0]['y'] * np.sqrt(markers_pos[1]['x']**2 +
77 83
                                                  self.alpha_x**2))
78
-        eta = np.pi / 2 - np.acos(eta_num / denom)
84
+        eta = np.pi / 2 - np.arccos(eta_num / denom)
79 85
 
80
-        beta_e = -1/2 (np.atan(markers_pos[0]['x'] / self.alpha_x) +
81
-                       np.atan(markers_pos[1]['x'] / self.alpha_x))
86
+        beta_e = -1/2 * (np.arctan(markers_pos[0]['x'] / self.alpha_x) +
87
+                         np.arctan(markers_pos[1]['x'] / self.alpha_x))
82 88
         return (ksi, eta, beta_e)
83 89
 
84 90
 
85 91
 def simple_control_law(pos_object, img):
86
-    """Compute the control law (7) from the image img. Returns the value of
92
+    """
93
+    Compute the control law (7) in cartesian coordinates obtained from the image img. Returns the value of
87 94
     omega."""
88 95
     doors_in_cam_ref = pos_object.get_cartesian_positions_in_camera_frame(img)
89 96
     return (- (doors_in_cam_ref[0]['x'] * doors_in_cam_ref[1]['z']**2 +
@@ -96,16 +103,20 @@ def simple_control_law(pos_object, img):
96 103
              (doors_in_cam_ref[1]['z']**2 * doors_in_cam_ref[0]['z'] *
97 104
               (doors_in_cam_ref[1]['z'] + doors_in_cam_ref[0]['z']))))
98 105
 
99
-def advanced_control_law(pos_object, img):
100
-    """Compute the control law (7) from the image img. Returns the value of
101
-    omega."""
102
-    doors_in_cam_ref = pos_object.get_cartesian_positions_in_camera_frame(img)
103
-    return (- (doors_in_cam_ref[0]['x'] * doors_in_cam_ref[1]['z']**2 +
104
-               doors_in_cam_ref[1]['x'] * doors_in_cam_ref[0]['z']**2)
105
-            /
106
-            ((doors_in_cam_ref[1]['x'] *
107
-              doors_in_cam_ref[0]['x'] *
108
-              doors_in_cam_ref[1]['z']**2) +
109
-             (doors_in_cam_ref[1]['x']**2 * doors_in_cam_ref[0]['z']**2) +
110
-             (doors_in_cam_ref[1]['z']**2 * doors_in_cam_ref[0]['z'] *
111
-              (doors_in_cam_ref[1]['z'] + doors_in_cam_ref[0]['z']))))
106
+def elliptic_control_law(pos_object, img, K, l, w_bar, a):
107
+    """
108
+    Compute the control law (17) using elliptic coordinates obtained from the image img. Returns the value of
109
+    (v, omega) used in cartesian coordinates so that effective motion is still done in cartesian coordinates.
110
+
111
+    K is the K constant in (17)
112
+    l is the lambda constant in (17)
113
+    w_bar is the \bar{w} constant in (17)
114
+    a is the half-width of the door
115
+    """
116
+    ksi, eta, beta_e = pos_object.get_elliptic_coordinates(img)
117
+    omega_0 = -K * beta_e - w_bar * l * eta * np.sin(beta_e) / beta_e
118
+
119
+    v = w_bar * a * np.sqrt(1 + np.tan(eta)**2 * np.tanh(ksi)**2) * np.cos(eta) * np.cosh(ksi)
120
+    omega = -omega_0 + 2*v*(np.cos(beta_e) / np.cosh(ksi) * np.sin(eta) - np.sin(beta_e) / np.cos(eta) * np.sinh(ksi)) / (a * (np.cos(2*eta) + np.cosh(2*ksi)) * np.sqrt(1 + np.tan(eta)**2 * np.tanh(ksi)**2))
121
+
122
+    return (v, omega)

BIN
door.blend View File


+ 21
- 3
emulate.py View File

@@ -18,6 +18,11 @@ i = 1  # Step number
18 18
 alpha_x = 100
19 19
 alpha_y = -100
20 20
 
21
+## Constants for the elliptic control law
22
+K = 1.5  # TODO
23
+l = 1.0  # TODO
24
+w_bar = 0.1  # TODO
25
+
21 26
 
22 27
 def compute_delta_position(v, omega, old_theta):
23 28
     dt = 1 / fps
@@ -38,6 +43,18 @@ def do_step_simple_control_law(render_path, pos_object, old_theta):
38 43
     return delta_pos
39 44
 
40 45
 
46
+def do_step_elliptic_control_law(render_path, pos_object, old_theta):
47
+    """Do a step using the elliptic control law."""
48
+    v_local = v
49
+    a = (bpy.data.objects["Marker_R"].location.y - bpy.data.objects["Marker_L"].location.y) / 2
50
+    try:
51
+        v_local, omega = control_law.elliptic_control_law(pos_object, render_path, K, l, w_bar, a)
52
+    except AssertionError:
53
+        v_local, omega = v, 0
54
+    print("Control law: ", (v_local, omega, old_theta))
55
+    delta_pos = compute_delta_position(v_local, omega, old_theta)
56
+    return delta_pos
57
+
41 58
 def init_pos_object():
42 59
     """Init the position object, to fetch positions from image, using settings
43 60
     extracted from the Blender scene."""
@@ -56,7 +73,7 @@ def init_pos_object():
56 73
     return control_law.Positions(h, alpha_x, alpha_y, markers_color)
57 74
 
58 75
 
59
-def emulate_simple_control_law(nb_steps=float("inf")):
76
+def emulate_control_law(callback=do_step_simple_control_law, nb_steps=float("inf")):
60 77
     """
61 78
     Run the simulation of the simple control law.
62 79
 
@@ -83,7 +100,7 @@ def emulate_simple_control_law(nb_steps=float("inf")):
83 100
 
84 101
         # Run steering algorithm
85 102
         # tx, ty is the resulting translation and r the rotational shift
86
-        tx, ty, r = do_step_simple_control_law(scene.render.filepath, pos_object, theta)
103
+        tx, ty, r = callback(scene.render.filepath, pos_object, theta)
87 104
 
88 105
         print("Step %s: (tx, ty, r) = (%s, %s, %s)" % (i, tx, ty, r))
89 106
 
@@ -115,5 +132,6 @@ if __name__ == "__main__":
115 132
     
116 133
     print("======================================")
117 134
     print("Found existing steps, starting from", i)
118
-    emulate_simple_control_law(1)
135
+    emulate_control_law(callback=do_step_simple_control_law, nb_steps=1)  # Simple control law (7)
136
+    # emulate_control_law(callback=do_step_elliptic_control_law, nb_steps=1)  # Elliptic control law (17)
119 137
     print("\n\n")