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

emulate.py 4.4KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137
  1. #!/usr/bin/env python
  2. import bpy
  3. import control_law
  4. import math
  5. import os
  6. """
  7. Script to be run from inside Blender Python scripting window.
  8. """
  9. fps = 5 # Images per second
  10. v = 1 # Forward speed, in meters per second
  11. i = 1 # Step number
  12. # alpha are the coefficients for the proportionnality between coordinates in
  13. # image plane and camera frame, according to pinhole model
  14. alpha_x = 100
  15. alpha_y = -100
  16. ## Constants for the elliptic control law
  17. K = 1.5 # TODO
  18. l = 1.0 # TODO
  19. w_bar = 0.1 # TODO
  20. def compute_delta_position(v, omega, old_theta):
  21. dt = 1 / fps
  22. y = v * math.cos(old_theta) * dt
  23. x = -v * math.sin(old_theta) * dt
  24. theta = omega * dt
  25. return (x, y, theta)
  26. def do_step_simple_control_law(render_path, pos_object, old_theta):
  27. """Do a step using the simple control law."""
  28. try:
  29. omega = control_law.simple_control_law(pos_object, render_path)
  30. except AssertionError:
  31. omega = 0
  32. print("Control law: ", (v, omega, old_theta))
  33. delta_pos = compute_delta_position(v, omega, old_theta)
  34. return delta_pos
  35. def do_step_elliptic_control_law(render_path, pos_object, old_theta):
  36. """Do a step using the elliptic control law."""
  37. v_local = v
  38. a = (bpy.data.objects["Marker_R"].location.y - bpy.data.objects["Marker_L"].location.y) / 2
  39. try:
  40. v_local, omega = control_law.elliptic_control_law(pos_object, render_path, K, l, w_bar, a)
  41. except AssertionError:
  42. v_local, omega = v, 0
  43. print("Control law: ", (v_local, omega, old_theta))
  44. delta_pos = compute_delta_position(v_local, omega, old_theta)
  45. return delta_pos
  46. def init_pos_object():
  47. """Init the position object, to fetch positions from image, using settings
  48. extracted from the Blender scene."""
  49. # Get right door marker color
  50. marker_r = bpy.data.objects["Marker_R"]
  51. marker_materials = marker_r.data.materials
  52. raw_markers_color = marker_materials[marker_materials.keys()[0]].diffuse_color
  53. markers_color = {}
  54. markers_color['h'] = raw_markers_color.h * 360
  55. markers_color['s'] = raw_markers_color.s * 100
  56. markers_color['v'] = raw_markers_color.v * 100
  57. # h is the height of the markers relative to the camera, in meters
  58. h = (marker_r.location.z - bpy.data.scenes["Scene"].camera.location.z)
  59. return control_law.Positions(h, alpha_x, alpha_y, markers_color)
  60. def emulate_control_law(callback=do_step_simple_control_law, nb_steps=float("inf")):
  61. """
  62. Run the simulation of the simple control law.
  63. Note: (X, Z) in the paper are (X, Y) in Blender.
  64. """
  65. global i
  66. pos_object = init_pos_object()
  67. scene = bpy.data.scenes["Scene"]
  68. basepath = bpy.path.abspath('//')
  69. render_path = basepath + "out/tmp%s.png"
  70. # Set render settings
  71. scene.render.resolution_x = 640
  72. scene.render.resolution_y = 480
  73. scene.render.filepath = render_path % i
  74. scene.camera.rotation_mode = 'XYZ'
  75. for j in range(nb_steps):
  76. theta = scene.camera.rotation_euler[2]
  77. # Do fast opengl rendering
  78. bpy.ops.render.opengl(write_still=True, view_context=False)
  79. # Run steering algorithm
  80. # tx, ty is the resulting translation and r the rotational shift
  81. tx, ty, r = callback(scene.render.filepath, pos_object, theta)
  82. print("Step %s: (tx, ty, r) = (%s, %s, %s)" % (i, tx, ty, r))
  83. # Update camera position
  84. scene.camera.location.x += tx
  85. scene.camera.location.y += ty
  86. scene.camera.rotation_euler[2] += r
  87. i += 1
  88. if __name__ == "__main__":
  89. # Create out dir if it does not exist. It is usedd to store renders.
  90. if not os.path.isdir(bpy.path.abspath("//") + "out"):
  91. os.mkdir(bpy.path.abspath("//") + "out")
  92. # Get list of existing frames by looking for tmpXX.png files in out dir.
  93. try:
  94. out_list = [int(i.replace(".png", "").replace("tmp", "")) for i in os.listdir(bpy.path.abspath("//") + "out") if i.startswith("tmp")]
  95. out_list.sort()
  96. except ValueError:
  97. out_list = []
  98. # Get next frame intex
  99. if len(out_list) > 0:
  100. i = out_list[-1] + 1
  101. else:
  102. i = 1
  103. print("======================================")
  104. print("Found existing steps, starting from", i)
  105. emulate_control_law(callback=do_step_simple_control_law, nb_steps=1) # Simple control law (7)
  106. # emulate_control_law(callback=do_step_elliptic_control_law, nb_steps=1) # Elliptic control law (17)
  107. print("\n\n")