Merge pull request #344 from compas-dev/reachability_map

Reachability Map

Author: gonzalocasas

.github/workflows/integration.yml

@@ -34,7 +34,7 @@ jobs:
           curl --output $(python -c "import os;import compas_fab.backends.vrep.remote_api as v; print(os.path.dirname(v.__file__))")/remoteApi.so https://wolke.ethz.ch/s/idbWPRKzJ8pFkAn/download/remoteApi.so
       - name: Run integration tests
         run: |
-          pytest --doctest-modules --ignore-glob="**ghpython/components/**.py"
+          pytest --doctest-modules
           pytest docs
       - name: Tear down docker containers
         run: |

CHANGELOG.rst

@@ -12,6 +12,10 @@ Unreleased
 
 **Added**
 
+* Added ``compas_fab.robots.ReachabilityMap``
+* Added ``compas_fab.robots.DeviationVectorsGenerator``
+* Added ``compas_fab.robots.OrthonormalVectorsFromAxisGenerator``
+
 **Changed**
 
 **Fixed**

conftest.py

@@ -1,3 +1,14 @@
 def pytest_configure(config):
     from twisted.internet import selectreactor
     selectreactor.install()
+
+
+def pytest_ignore_collect(path):
+    if "rhino" in str(path):
+        return True
+
+    if "blender" in str(path):
+        return True
+
+    if "ghpython" in str(path):
+        return True

docs/examples/07_reachability_map.rst

@@ -0,0 +1,11 @@
+.. _examples_kinematics:
+
+*******************************************************************************
+Planning: Reachability Map
+*******************************************************************************
+
+.. toctree::
+    :maxdepth: 2
+    :glob:
+
+    07_reachability_map/*

docs/examples/07_reachability_map/01_reachability_map.rst

@@ -0,0 +1,173 @@
+*******************************************************************************
+Introduction to the Reachability Map
+*******************************************************************************
+
+For workspace planning or robot path planning, it is useful to calculate and
+visualize the space of the robot's reachability, depending on its attached tool,
+obstacles in the environment and its own kinematic and geometric constraints.
+
+The ``ReachabilityMap`` is a collection of all valid IK solutions, i.e. poses that the end-effector can reach, at specified locations. The map is built by discretizing the robot's environment, defining frames to be checked, and calculating the IK solutions for each frame.
+
+The creation of this map depends on the availability of an analytical inverse
+kinematic solver for the used robot. Please checkout the kinematic backend for
+available robots.
+
+Links
+=====
+* `Realuex (ROS's reachability map) <http://wiki.ros.org/reuleaux>`_
+
+
+Example 01: reachability map 1D
+===============================
+
+Let's consider the following (abstract) example:
+
+We have a UR5 and want the TCP of the robot to always orient itself towards a
+defined position in front of the robot. Therefore, we define a (half-)sphere
+with a certain radius and we evaluate points on this sphere. At each point, we then
+create a plane whose normal vector points towards the center of this sphere. From these planes
+we create frames for the robot's TCP. The function is written as a generator
+because the ``ReachabilityMap`` takes a ``Frame`` generator as input.
+
+.. figure:: files/00_robot_halfsphere.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+
+.. code-block:: python
+
+    import math
+
+    from compas.geometry import Frame
+    from compas.geometry import Plane
+    from compas.geometry import Point
+    from compas.geometry import Sphere
+
+    # 1. Define frames on a sphere
+    sphere = Sphere((0.4, 0, 0), 0.15)
+
+    def points_on_sphere_generator(sphere):
+        for theta_deg in range(0, 360, 20):
+            for phi_deg in range(0, 90, 10):  # only half-sphere
+                theta = math.radians(theta_deg)
+                phi = math.radians(phi_deg)
+                x = sphere.point.x + sphere.radius * math.cos(theta) * math.sin(phi)
+                y = sphere.point.y + sphere.radius * math.sin(theta) * math.sin(phi)
+                z = sphere.point.z + sphere.radius * math.cos(phi)
+                point = Point(x, y, z)
+                axis = sphere.point - point
+                plane = Plane((x, y, z), axis)
+                f = Frame.from_plane(plane)
+                # for the old UR5 model from ROS Kinetic is zaxis the xaxis
+                yield [Frame(f.point, f.zaxis, f.yaxis)]
+
+
+Then we create a ``PyBulletClient`` (for collision checking), load the UR5 robot,
+set the analytical IK solver and define options for the IK solver.
+For simplicity, we do not add any tool or obstacles in the environment here, but in a
+real robot cell, this will usually be the case.
+
+
+.. code-block:: python
+
+    # 2. Set up robot cell
+    from compas_fab.backends import AnalyticalInverseKinematics
+    from compas_fab.backends import PyBulletClient
+
+    with PyBulletClient(connection_type='direct') as client:
+        # load robot and define settings
+        robot = client.load_ur5(load_geometry=True)
+        ik = AnalyticalInverseKinematics(client)
+        client.inverse_kinematics = ik.inverse_kinematics
+        options = {"solver": "ur5", "check_collision": True, "keep_order": True}
+
+Now we create a ``ReachabilityMap``. We calculate it passing the ``Frame``
+generator, the robot and the IK options. After calculation, we save the map as
+json for later visualization in Rhino/GH.
+
+>>> # 3. Create reachability map 1D
+>>> map = ReachabilityMap()                                                 # doctest: +SKIP
+>>> map.calculate(points_on_sphere_generator(sphere), robot, options)       # doctest: +SKIP
+>>> map.to_json(os.path.join(DATA, "reachability", "map1D.json"))           # doctest: +SKIP
+
+
+`Link to full script <files/01_example_1D.py>`_
+
+
+Visualization
+=============
+
+We can source the reachability map from the json file and use the ``Artist`` to
+visualize the saved frames by using the artist's function ``draw_frames``.
+
+>>> map = ReachabilityMap.from_json(filepath)  # doctest: +SKIP
+>>> artist = Artist(map)                       # doctest: +SKIP
+>>> frames = artist.draw_frames()              # doctest: +SKIP
+
+
+.. figure:: files/00_robot_halfsphere.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+
+By default, the ``artist.draw()`` method returns points and colors for a point cloud,
+where the points are the positions of the frames and the colors are calculated
+from the score at the respective frame. The ``ReachabilityMap.score`` is
+the number of valid IK solutions for a frame. The default colormap is 'viridis'.
+
+In the example below, the highest score is 4 (yellow) and the lowest score is 2 (violet).
+
+.. figure:: files/01_robot_map1D.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+
+If you want to visualize the frames at a specific IK index (= number between 0-7), use the method
+``artist.draw_frames(ik_index=ik_index)``. If you compare the figure below
+with the figure of ``draw_frames``, you will see that a certain portion is not
+reachable at the selected IK index.
+
+.. figure:: files/02_robot_frames_at_ik.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+
+Projects where the reachability map was applied
+===============================================
+
+------------------------------------------------------------------------------------------
+`Adaptive Detailing <https://gramaziokohler.arch.ethz.ch/web/forschung/e/0/0/0/361.html>`_
+------------------------------------------------------------------------------------------
+
+In this project, connections between structural elements are 3D printed in place,
+directly on top of parts, i.e. collision objects. A ``ReachabilityMap`` was created
+to capture the space where connections can be placed and ultimately find connecting
+geometries that the robot can print in between these objects. Printing process
+constraints can be included in the reachability map by choosing a meaningful
+``max_alpha`` in the ``DeviationVectorsGenerator``.
+
+.. figure:: files/adaptive_detailing.png
+    :figclass: figure
+    :class: figure-img img-fluid
+
+--------------------------------------------------------------------------------------------------------
+`Robotic 360° Light Painting Workshop <https://gramaziokohler.arch.ethz.ch/web/lehre/e/0/0/0/439.html>`_
+--------------------------------------------------------------------------------------------------------
+
+This project served as inspiration for the presented examples 01-03. The robot TCP
+had to be oriented towards the 360° camera. The light paths were mapped on a hemisphere
+to maintain equal distance to the camera and little distortion of the designed paths.
+The reachability map was used to determine the best position and radius for the
+sphere with the UR5e robot model, the light tool, and the camera and tripods as
+collision objects.
+
+.. |logo1| image:: files/workshop_sjsu_1.png
+   :scale: 45%
+   :align: middle
+.. |logo2| image:: files/workshop_sjsu_2.jpg
+   :scale: 45%
+   :align: middle
+
++---------+---------+
+| |logo1| | |logo2| |
++---------+---------+

docs/examples/07_reachability_map/02_vector_generators.rst

@@ -0,0 +1,97 @@
+*******************************************************************************
+Vector Generators
+*******************************************************************************
+
+For using the ``ReachabilityMap`` we need a ``Frame`` generator function. In a
+simple case, this can be only a list of frames. However, sometimes it makes
+sense to generate multiple frames at a certain location that deviate a bit in
+orientation. We provide 2 helper vector generators with very verbose names:
+
+* ``OrthonormalVectorsFromAxisGenerator``
+* ``DeviationVectorsGenerator``
+
+Generate orthonormal vectors from an axis
+=========================================
+
+The ``OrthonormalVectorsFromAxisGenerator`` generates vectors that are orthonormal a given axis.
+
+In the example below, the given axis is visualized in red, and the generated vectors in black.
+
+.. figure:: files/03_ortho_vectors.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+>>> import math
+>>> from compas.geometry import Vector
+>>> from compas_fab.robots import OrthonormalVectorsFromAxisGenerator
+>>> zaxis = Vector(0, 0, 1)
+>>> max_alpha = 60
+>>> for xaxis in OrthonormalVectorsFromAxisGenerator(zaxis, math.radians(max_alpha)):
+...     print(xaxis)
+Vector(0.000, -1.000, 0.000)
+Vector(0.866, -0.500, 0.000)
+Vector(0.866, 0.500, 0.000)
+Vector(0.000, 1.000, 0.000)
+Vector(-0.866, 0.500, 0.000)
+Vector(-0.866, -0.500, 0.000)
+
+
+Generate vectors that deviate
+=============================
+
+The ``DeviationVectorsGenerator`` generates equally distributed vectors that deviate from the passed one by a maximal angle of max_alpha.
+
+In the example below, the given axis is visualized in red, and the generated vectors in black.
+
+.. figure:: files/04_devi_vectors.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+>>> import math
+>>> from compas_fab.robots import DeviationVectorsGenerator
+>>> zaxis = Vector(0, 0, 1)
+>>> max_alpha = 40
+>>> step = 1
+>>> for axis in DeviationVectorsGenerator(zaxis, math.radians(max_alpha), step):
+...     print(axis)
+Vector(0.000, 0.000, 1.000)
+Vector(-0.643, 0.000, 0.766)
+Vector(-0.321, -0.557, 0.766)
+Vector(0.321, -0.557, 0.766)
+Vector(0.643, -0.000, 0.766)
+Vector(0.321, 0.557, 0.766)
+Vector(-0.321, 0.557, 0.766)
+
+
+Or another example with using ``max_angle = 60`` and ``step = 2``.
+
+.. figure:: files/05_devi_vectors_2.jpg
+    :figclass: figure
+    :class: figure-img img-fluid
+
+
+Generate frames
+===============
+
+Now having these two vector generators, we can combine them for generating frames.
+
+.. code-block:: python
+
+    from compas.geometry import Frame
+    from compas.geometry import Point
+    from compas.geometry import Vector
+    from compas_fab.robots import DeviationVectorsGenerator
+    from compas_fab.robots import OrthonormalVectorsFromAxisGenerator
+
+    def frame_generator():
+        for i in range(6):
+            pt = Point(0, 0, 0) + Vector(0, i, 0)
+            zaxis = Vector(0, 0, 1)
+            for axis in DeviationVectorsGenerator(zaxis, math.radians(40), 1):
+                for xaxis in OrthonormalVectorsFromAxisGenerator(axis, math.radians(60)):
+                    yaxis = axis.cross(xaxis)
+                    yield Frame(pt, xaxis, yaxis)
+
+.. figure:: files/06_devi_frames.jpg
+    :figclass: figure
+    :class: figure-img img-fluid