Using the geometry of confocal curves for passing through a door, implementation.

control_law.py 5.4KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122
  1. #!/usr/bin/env python
  2. try:
  3. import bpy
  4. except ImportError:
  5. print("Not running in Blender.")
  6. import numpy as np
  7. import subprocess
  8. class Positions:
  9. def __init__(self, h, alpha_x, alpha_y, markers_color):
  10. """Same notations as in the paper. Markers_color is an OpenCV HSV dict
  11. representing the color of the markers.
  12. - h is the height of the markers relative to the camera, in meters
  13. - alpha are the coefficients for the proportionnality between coordinates in
  14. image plane and camera frame, according to pinhole model
  15. - markers_color is an HSV dictionnary describing door markers color"""
  16. self.h = h
  17. self.alpha_x = alpha_x
  18. self.alpha_y = alpha_y
  19. self.markers_color = markers_color
  20. raise IndexError
  21. def get_positions_in_image_plane(self, img):
  22. """Returns the position of the two markers in the image plane."""
  23. try:
  24. basepath = bpy.path.abspath('//')
  25. except NameError:
  26. basepath = "."
  27. out = subprocess.check_output([basepath + "/blob",
  28. img,
  29. "%s" % self.markers_color['h']])
  30. print([basepath + "/blob",
  31. img,
  32. "%s" % self.markers_color['h'],
  33. "%s" % self.markers_color['s'],
  34. "%s" % self.markers_color['v']])
  35. out = out.decode('utf-8').split("\n")
  36. assert(int(out[0].split(" ")[0]) > 1)
  37. marker1 = {
  38. 'x': int(out[1].replace('X: ', '').strip()),
  39. 'y': int(out[2].replace('Y: ', '').strip())
  40. }
  41. marker2 = {
  42. 'x': int(out[3].replace('X: ', '').strip()),
  43. 'y': int(out[4].replace('Y: ', '').strip())
  44. }
  45. if marker1['x'] > marker2['x']:
  46. marker1, marker2 = marker2, marker1
  47. print("Blobs found: ", marker1, marker2)
  48. return (marker1, marker2)
  49. def get_cartesian_positions_in_camera_frame(self, img):
  50. """Returns the position of the two markers in the camera frame."""
  51. markers_pos = self.get_positions_in_image_plane(img)
  52. tmp = self.alpha_y * self.h / markers_pos[0]['y']
  53. marker1 = {
  54. 'x': markers_pos[0]['x'] * tmp / self.alpha_x,
  55. 'z': tmp,
  56. }
  57. tmp = self.alpha_y * self.h / markers_pos[1]['y']
  58. marker2 = {
  59. 'x': markers_pos[1]['x'] * tmp / self.alpha_x,
  60. 'z': tmp,
  61. }
  62. print("Positions of the blobs IRL: ", marker1, marker2)
  63. return (marker1, marker2)
  64. def get_elliptic_coordinates(self, img):
  65. """Returns the elliptic coordinates using Remark 2."""
  66. markers_pos = self.get_positions_in_image_plane(img)
  67. ksi_num = (markers_pos[1]['y'] * np.sqrt(markers_pos[0]['x']**2 +
  68. self.alpha_x**2) +
  69. markers_pos[0]['y'] * np.sqrt(markers_pos[1]['x']**2 +
  70. self.alpha_x**2))
  71. denom = np.sqrt((markers_pos[0]['x'] * markers_pos[1]['y'] -
  72. markers_pos[1]['x'] * markers_pos[0]['y'])**2 +
  73. self.alpha_x**2 * (markers_pos[1]['y'] -
  74. markers_pos[0]['y'])**2)
  75. ksi = np.arccosh(abs(ksi_num / denom))
  76. eta_num = (markers_pos[1]['y'] * np.sqrt(markers_pos[0]['x']**2 +
  77. self.alpha_x**2) -
  78. markers_pos[0]['y'] * np.sqrt(markers_pos[1]['x']**2 +
  79. self.alpha_x**2))
  80. eta = np.pi / 2 - np.arccos(eta_num / denom)
  81. beta_e = -1/2 * (np.arctan(markers_pos[0]['x'] / self.alpha_x) +
  82. np.arctan(markers_pos[1]['x'] / self.alpha_x))
  83. return (ksi, eta, beta_e)
  84. def simple_control_law(pos_object, img):
  85. """
  86. Compute the control law (7) in cartesian coordinates obtained from the image img. Returns the value of
  87. omega."""
  88. doors_in_cam_ref = pos_object.get_cartesian_positions_in_camera_frame(img)
  89. return (- (doors_in_cam_ref[0]['x'] * doors_in_cam_ref[1]['z']**2 +
  90. doors_in_cam_ref[1]['x'] * doors_in_cam_ref[0]['z']**2)
  91. /
  92. ((doors_in_cam_ref[1]['x'] *
  93. doors_in_cam_ref[0]['x'] *
  94. doors_in_cam_ref[1]['z']**2) +
  95. (doors_in_cam_ref[1]['x']**2 * doors_in_cam_ref[0]['z']**2) +
  96. (doors_in_cam_ref[1]['z']**2 * doors_in_cam_ref[0]['z'] *
  97. (doors_in_cam_ref[1]['z'] + doors_in_cam_ref[0]['z']))))
  98. def elliptic_control_law(pos_object, img, K, l, w_bar, a):
  99. """
  100. Compute the control law (17) using elliptic coordinates obtained from the image img. Returns the value of
  101. (v, omega) used in cartesian coordinates so that effective motion is still done in cartesian coordinates.
  102. K is the K constant in (17)
  103. l is the lambda constant in (17)
  104. w_bar is the \bar{w} constant in (17)
  105. a is the half-width of the door
  106. """
  107. ksi, eta, beta_e = pos_object.get_elliptic_coordinates(img)
  108. omega_0 = -K * beta_e - w_bar * l * eta * np.sin(beta_e) / beta_e
  109. v = w_bar * a * np.sqrt(1 + np.tan(eta)**2 * np.tanh(ksi)**2) * np.cos(eta) * np.cosh(ksi)
  110. 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))
  111. return (v, omega)