❗Add more generalized algorithm to compute ground contacts + chores (#78)

* Add a new the more general algorithm to computer orientation properties
  - Incorporate both ground contact solver algorithms in updating hexapod pose depending on situation
  - Use the more general algorithm for the kinematics page
  - Incorporate new algorithm in test

* Fix ground contact solver module
  - Move ground contact solver to own module
  - Replace check stability with function that accounts for all cases
  - Simplify and refactor ground contact solver, remove duplicate operations

* Do pylint chores
  - Remove unused ignore declarations on pylint file
  - Dont ignore tests directory since it's not pylint isn't ignoring directories anyway


* Update README

Author: mithi

.pylintrc

@@ -7,11 +7,11 @@ extension-pkg-whitelist=
 
 # Add files or directories to the blacklist. They should be base names, not
 # paths.
-#ignore=CVS, ik_solver.py, page_kinematics.py
+#ignore=
 
 # Add files or directories matching the regex patterns to the blacklist. The
 # regex matches against base names, not paths.
-#ignore-patterns=case*.py
+#ignore-patterns=
 
 # Python code to execute, usually for sys.path manipulation such as
 # pygtk.require().

.travis.yml

@@ -8,4 +8,4 @@ script:
   - python -m pytest
   - python -m py_compile ./*/*.py ./*.py ./*/*/*.py
   - flake8 ./*/*.py ./*.py ./*/*/*.py
-  - pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py,tests --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code
+  - pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code

README.md

@@ -7,7 +7,7 @@
 
 |  |  |  |  |
 |---------|---------|---------|---------|
-|![Twisting turning and tilting](https://mithi.github.io/robotics-blog/robot-only-x1.gif)|<img src="https://mithi.github.io/robotics-blog/v2-hexapod-1.gif" width="550"/>|<img src="https://mithi.github.io/robotics-blog/v2-hexapod-2.gif" width="500"/>|![Adjusting camera view](https://mithi.github.io/robotics-blog/robot-only-x2.gif)|
+|![Twisting turning and tilting](https://mithi.github.io/robotics-blog/robot-only-x1.gif)|<img src="https://mithi.github.io/robotics-blog/v2-hexapod-1.gif" width="550"/>|<img src="https://mithi.github.io/robotics-blog/v2-hexapod-2.gif" width="500"/>|![Adjusting camera view](https://mithi.github.io/robotics-blog/robot-only-x3.gif)|
 
 | STATUS | FEATURE   | DESCRIPTION  |
 |---|-----------|--------------|
@@ -24,9 +24,9 @@
 
 ## 🕷️ Preview
 
-|![IK](https://mithi.github.io/robotics-blog/v2-ik-ui.gif)|![Kinematics](https://mithi.github.io/robotics-blog/v2-kinematics-ui.gif)|
+|![image](https://mithi.github.io/robotics-blog/v2-ik-ui.gif)|![image](https://mithi.github.io/robotics-blog/v2-kinematics-ui.gif)|
 |----|----|
-| ![IK](https://mithi.github.io/robotics-blog/UI-1.gif) | ![Kinematics](https://mithi.github.io/robotics-blog/UI-2.gif) |
+| ![image](https://mithi.github.io/robotics-blog/UI-1.gif) | ![image](https://mithi.github.io/robotics-blog/UI-2.gif) |
 
 ## 🕷️ Requirements
 
@@ -57,13 +57,15 @@ Running on http://127.0.0.1:8050/
 
 - Definitions
   - [`Linkage`](./hexapod/linkage.py)
-  - [`VirtualHexapod`](./hexapod/models.py#L94)
+  - [`VirtualHexapod`](./hexapod/models.py)
 - The [Inverse Kinematics Algorithm](./hexapod/ik_solver/README.md) used for this project
-- [How to find the ground contact points, tilt, and height of the hexapod](./hexapod/ground_contact_solver.py#L43)
-- [How to make the hexapod step on the correct target ground contacts](./hexapod/ik_solver/recompute_hexapod.py#L15)
+- How to find the orientation of the hexapod with respect to the ground given we know all the orientations of the six legs with respect to the robot's body.
+  - [Algorithm 1](./hexapod/ground_contact_solver/ground_contact_solver.py) when we know which of the three points of each leg could contact the ground
+  - [Algorithm 2](./hexapod/ground_contact_solver/ground_contact_solver2.py) when  we **don't** know which of points of which legs could be in contact with the ground
+- [How to make the hexapod step on the correct target ground contacts](./hexapod/ik_solver/recompute_hexapod.py)
 - How to determine if the hexapod should twist and by how much
-  - [`find_if_might_twist`](./hexapod/models.py#L242)
-  - [`find_twist_frame`](./hexapod/models.py#L267)
+  - [`find_if_might_twist`](./hexapod/models.py#L248)
+  - [`find_twist_frame`](./hexapod/models.py#L273)
 
 ## 🕷️ Screenshots
 
@@ -73,15 +75,18 @@ Running on http://127.0.0.1:8050/
 
 ## 🕷️ Notes
 
-- ❗Now live on https://hexapod-robot-simulator.herokuapp.com ! **BUT** (and a big one) I highly suggest that you run it locally. When run locally, it's pretty speedy! On the other hand, the link above is barely usable. Might convert this to to be a fully client-side Javascript app later, maybe?
+- Now live on https://hexapod-robot-simulator.herokuapp.com ! **BUT** (and a big one) I highly suggest that you run it locally. When run locally, it's pretty speedy! On the other hand, the link above is barely usable. Might convert this to to be a fully client-side Javascript app later, maybe?
 
-- ❗This implementation uses matrices, **NOT** quaternions. I'm aware that quaternions is far superior in every single way. In the (un)forseeable future, maybe?
+- This implementation uses matrices, **NOT** quaternions. I'm aware that quaternions is far superior in every single way. In the (un)forseeable future, maybe?
 
-- ❗Frankly, [My IK algorithm](https://github.com/mithi/hexapod-robot-simulator/blob/master/hexapod/ik_solver/README.md) isn't all that great, it's just something I came up with based on what I remember back in college plus browsing through the [Mathematics Stack Exchange](https://math.stackexchange.com/). It might not be the best, but it's the most intuitive that I can think of. If you want something closer to the state-of-the-art, maybe checkout [Unity's Fast IK](https://assetstore.unity.com/packages/tools/animation/fast-ik-139972) or [ROS IKFast](http://wiki.ros.org/Industrial/Tutorials/Create_a_Fast_IK_Solution).
+- Honestly, [My IK algorithm](./hexapod/ik_solver/README.md) is just something I came up with based on what I remember back in college plus browsing through the [Mathematics Stack Exchange](https://math.stackexchange.com/). It's just the most intuitive that I can think of. I'm open to hearing other ways to approach this. If you want something closer to the state-of-the-art, maybe checkout [Unity's Fast IK](https://assetstore.unity.com/packages/tools/animation/fast-ik-139972) or [ROS IKFast](http://wiki.ros.org/Industrial/Tutorials/Create_a_Fast_IK_Solution).
 
-- I believe that the idea that it's best to be kind to one another shouldn't be controversial. And I shouldn't be afraid to say that. [![Contributor Covenant](https://img.shields.io/badge/Contributor%20Covenant-v2.0%20adopted-ff69b4.svg)](https://www.contributor-covenant.org/)
+- I would also love to here new ideas on how to improve the [algorithms currently used](./hexapod/ground_contact_solver/) to find the orientation of the hexapod given we know the orientation of the legs with respect to the body.
+
+- I believe that the idea that it's best if we are kind to one another shouldn't be controversial. And I shouldn't be afraid to say that. [![Contributor Covenant](https://img.shields.io/badge/Contributor%20Covenant-v2.0%20adopted-ff69b4.svg)](https://www.contributor-covenant.org/)
 
 ## 🤗 Contributors
 
 - [@mithi](https://github.com/mithi/)
 - [@philippeitis](https://github.com/philippeitis/)
+- [@mikong](https://github.com/mikong/)

checker

@@ -3,4 +3,4 @@ black .
 python -m pytest
 python -m py_compile ./*/*.py ./*.py ./*/*/*.py
 flake8 ./*/*.py ./*.py ./*/*/*.py
-pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py,tests --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code
+pylint ./*.py */*.py */*/*.py --reports=y --ignore=page_kinematics.py,ik_solver.py --disable=broad-except,too-few-public-methods,attribute-defined-outside-init,too-many-locals,too-many-instance-attributes,too-many-arguments,bad-continuation,missing-class-docstring,missing-module-docstring,missing-function-docstring,invalid-name,duplicate-code

hexapod/ground_contact_solver.py …_contact_solver/ground_contact_solver.pyhexapod/ground_contact_solver.py renamed to hexapod/ground_contact_solver/ground_contact_solver.py hexapod/ground_contact_solver.py renamed to hexapod/ground_contact_solver/ground_contact_solver.py

@@ -1,4 +1,20 @@
 """
+❗❗❗A more general algorithm for computing the robot's orientation.
+This algorithm rests upon the assumption that it
+knows which point of the each leg is in contact with the ground.
+This assumption seems to be true for all possible cases for
+leg-patterns page and inverse-kinematics page.
+
+But this is not true for all possible
+angle combinations (18 angles) that can be defined in
+the kinematics page.
+
+This module is used for the leg-patterns page,
+and the inverse-kinematics page.
+
+The other module will be used for the kinematics page.
+❗❗❗
+
 This module is responsible for the following:
 1. determining which legs of the hexapod is on the ground
 2. Computing the normal vector of the triangle defined by at least three legs on the ground
@@ -9,12 +25,8 @@
 """
 from math import isclose
 from itertools import combinations
-from hexapod.points import (
-    Point,
-    dot,
-    get_normal_given_three_points,
-    is_point_inside_triangle,
-)
+from hexapod.points import dot, get_normal_given_three_points
+from hexapod.ground_contact_solver.helpers import is_stable
 
 
 def compute_orientation_properties(legs, tol=1):
@@ -24,20 +36,13 @@ def compute_orientation_properties(legs, tol=1):
       - Normal vector of the plane defined by these legs
       - Distance of this plane to center of gravity
     """
-    trio = three_ids_of_ground_contacts(legs)
+    trio, n, height = find_ground_plane_properties(legs)
 
     # This pose is unstable, The hexapod has no balance
     if trio is None:
         return [], None, None
 
-    p0, p1, p2 = get_corresponding_ground_contacts(trio, legs)
-    n = get_normal_given_three_points(p0, p1, p2)
-
-    # Note: using p0, p1 or p2 should yield the same result
-    # height from cog to ground
-    height = -dot(n, p0)
-
-    # Get all contacts of the same height
+    # get all the legs on the ground
     legs_on_ground = []
 
     for leg in legs:
@@ -48,17 +53,15 @@ def compute_orientation_properties(legs, tol=1):
     return legs_on_ground, n, height
 
 
-def three_ids_of_ground_contacts(legs, tol=1):
+def find_ground_plane_properties(legs, tol=1):
     """
     Return three legs forming a stable position from legs,
     or None if no three legs satisfy this requirement.
-
-    This function takes the legs of the hexapod
-    and finds three legs on the ground that form a stable position
-    returns the leg ids of those three legs
-    return None if no stable position found
+    It also returns the normal vector of the plane
+    defined by the three ground contacts, and the
+    computed distance of the hexapod body to the ground plane
     """
-    trios = set_of_trios_from_six()
+    trios = list(combinations(range(6), 3))
     ground_contacts = [leg.ground_contact() for leg in legs]
 
     # (2, 3, 5) is a trio from the set [0, 1, 2, 3, 4, 5]
@@ -67,7 +70,7 @@ def three_ids_of_ground_contacts(legs, tol=1):
     for trio, other_trio in zip(trios, reversed(trios)):
         p0, p1, p2 = [ground_contacts[i] for i in trio]
 
-        if not check_stability(p0, p1, p2):
+        if not is_stable(p0, p1, p2):
             continue
 
         # Get the vector normal to plane defined by these points
@@ -94,43 +97,18 @@ def three_ids_of_ground_contacts(legs, tol=1):
         # height should be the most largest since
         # the plane defined by this trio is on the ground
         # the other legs ground contact cannot be lower than the ground
-        condition_violated = False
-        for i in other_trio:
-            _height = -dot(n, ground_contacts[i])
-            if _height > height + tol:
-                # Wrong leg combination, check another
-                condition_violated = True
-                break
-
-        if not condition_violated:
-            return trio  # Found one!
+        other_points = [ground_contacts[i] for i in other_trio]
+        if no_other_legs_lower(n, height, other_points, tol):
+            # Found one!
+            return [p0, p1, p2], n, height
 
     # Nothing met the condition
     return None
 
 
-def get_corresponding_ground_contacts(ids, legs):
-    """
-    Given three leg ids and the list of legs get the points
-    contacting the ground of those three legs.
-    """
-    return [legs[i].ground_contact() for i in ids]
-
-
-def set_of_trios_from_six():
-    """
-    Get all combinations of a three-item-group given six items.
-    20 combinations total
-    """
-    return list(combinations(range(6), 3))
-
-
-def check_stability(a, b, c):
-    """
-    Check if the points a, b, c form a stable triangle.
-
-    If the center of gravity p (0, 0) on xy plane
-    is inside projection (in the xy plane) of
-    the triangle defined by point a, b, c, then this is stable
-    """
-    return is_point_inside_triangle(Point(0, 0, 0), a, b, c)
+def no_other_legs_lower(n, height, other_points, tol):
+    for point in other_points:
+        _height = -dot(n, point)
+        if _height > height + tol:
+            return False
+    return True

hexapod/ground_contact_solver/ground_contact_solver2.py

@@ -0,0 +1,111 @@
+"""
+❗❗❗This is a more general algorithm to account for the cases
+that are not handled correctly by the other ground_contact_solver.
+This will only be used by the kinematics-page of the app.
+This algorithm can be optimized or replaced if a more elegant
+algorithm is available.
+❗❗❗
+
+We have 18 points total.
+(6 legs, three possible points per leg)
+
+We have a total of 540 combinations
+- get three legs out of six (20 combinations)
+  - we have three possible points for each leg, that's 27 (3^3)
+  -  27 * 20 is 540
+
+For each combination:
+    1. Check if stable if not, next
+      - Check if the projection of the center of gravity to the plane
+        defined by the three points lies inside the triangle, if not, next
+    2. Get the height and normal of the height and normal of the triangle plane
+        (We need this for the next part)
+    3. For each of the three leg, check if the two other points on the leg is not a
+        lower height, if condition if broken, next. (6 points total)
+    4. For each of the three other legs, check all points (3 points of each leg)
+        if so, next. (9 points total)
+    5. If no condition is violated, then this is good, return this!
+        (height, n_axis, 3 ground contacts)
+"""
+from math import isclose
+from itertools import combinations
+from hexapod.ground_contact_solver.helpers import is_stable
+from hexapod.points import get_normal_given_three_points, dot
+
+TOL = 1.0
+
+
+def all_joint_id_combinations():
+    joints_combination_list = []
+    # joint coxia point 1, femur point 2, foot_tip 3
+    for i in range(3, 0, -1):
+        for j in range(3, 0, -1):
+            for k in range(3, 0, -1):
+                joints_combination_list.append([i, j, k])
+
+    return joints_combination_list
+
+
+OTHER_POINTS_MAP = {1: [2, 3], 2: [3, 1], 3: [1, 2]}
+
+
+def compute_orientation_properties(legs):
+    leg_trios = list(combinations(range(6), 3))
+    joint_trios = all_joint_id_combinations()
+
+    for leg_trio, other_leg_trio in zip(leg_trios, reversed(leg_trios)):
+
+        three_legs = [legs[i] for i in leg_trio]
+        other_three_legs = [legs[i] for i in other_leg_trio]
+
+        for joint_trio in joint_trios:
+
+            p0, p1, p2 = [legs[i].get_point(j) for i, j in zip(leg_trio, joint_trio)]
+
+            if not is_stable(p0, p1, p2):
+                continue
+
+            n = get_normal_given_three_points(p0, p1, p2)
+            height = -dot(n, p0)
+
+            if same_leg_joints_break_condition(three_legs, joint_trio, n, height):
+                continue
+
+            if other_leg_joints_break_condition(other_three_legs, n, height):
+                continue
+
+            legs_on_ground = find_legs_on_ground(legs, n, height)
+            return legs_on_ground, n, height
+
+    return [], None, None
+
+
+def other_leg_joints_break_condition(other_three_legs, n, height):
+    for leg in other_three_legs:
+        for i in range(1, 4):
+            height_to_test = -dot(n, leg.get_point(i))
+            if height_to_test > height + TOL:
+                return True
+    return False
+
+
+def same_leg_joints_break_condition(three_legs, three_point_ids, n, height):
+    for leg, point_id in zip(three_legs, three_point_ids):
+        for other_point_id in OTHER_POINTS_MAP[point_id]:
+            other_point = leg.get_point(other_point_id)
+            height_to_test = -dot(n, other_point)
+            if height_to_test > height + TOL:
+                return True
+    return False
+
+
+def find_legs_on_ground(legs, n, height):
+    legs_on_ground = []
+    for leg in legs:
+        for point in reversed(leg.all_points[1:]):
+            _height = -dot(n, point)
+            if isclose(height, _height, abs_tol=TOL):
+                legs_on_ground.append(leg)
+                break
+
+    return legs_on_ground

hexapod/ground_contact_solver/helpers.py

@@ -0,0 +1,37 @@
+from hexapod.points import (
+    Point,
+    dot,
+    cross,
+    vector_from_to,
+)
+
+
+# math.stackexchange.com/questions/544946/
+#   determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
+# gamedev.stackexchange.com/questions/23743/
+#   whats-the-most-efficient-way-to-find-barycentric-coordinates
+# en.wikipedia.org/wiki/Barycentric_coordinate_system
+def is_stable(p1, p2, p3, tol=0.001):
+    """
+    Determines stability of the pose.
+    Determine if projection of 3D point p
+    onto the plane defined by p1, p2, p3
+    is within a triangle defined by p1, p2, p3.
+    """
+    p = Point(0, 0, 0)
+    u = vector_from_to(p1, p2)
+    v = vector_from_to(p1, p3)
+    n = cross(u, v)
+    w = vector_from_to(p1, p)
+    n2 = dot(n, n)
+    beta = dot(cross(u, w), n) / n2
+    gamma = dot(cross(w, v), n) / n2
+    alpha = 1 - gamma - beta
+    # then coordinate of the projected point (p_) of point p
+    # p_ = alpha * p1 + beta * p2 + gamma * p3
+    min_val = -tol
+    max_val = 1 + tol
+    cond1 = min_val <= alpha <= max_val
+    cond2 = min_val <= beta <= max_val
+    cond3 = min_val <= gamma <= max_val
+    return cond1 and cond2 and cond3