Skip to content

Commit

Permalink
Improve ground contact solver + some small changes
Browse files Browse the repository at this point in the history
In ground contact solver:
* No longer return trio since it isn't being used anyway
* Improve ground contact solver algorithm
* Prefer combinations where the legs aren't adjacent to each other
* Introduce some randomness so we are not bias in choosing on stable position over another
* Refactor shared code for both ground contact solver and ground contact solver 2

In points:
* Add tolerance optional parameter when testing point equality

In tests:
* Make test (doesn't assume ground targets) its own function
* Add tolerance when asserting points in tests

* Update README
  • Loading branch information
mithi committed Apr 23, 2020
1 parent 8bc5f9f commit ee3c1dd
Show file tree
Hide file tree
Showing 7 changed files with 123 additions and 83 deletions.
7 changes: 3 additions & 4 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,8 @@ Running on http://127.0.0.1:8050/

## ⚠️ Known Issues

- [ ][Priorities](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3APRIORITY)
- [ ][Priority](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3APRIORITY)
- [ ][Good First Issue](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3A%22good+first+issue%22)
- [ ][Help Wanted](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3A%22help+wanted%22)
- [ ][Bugs](https://github.com/mithi/hexapod-robot-simulator/issues?q=is%3Aissue+is%3Aopen+label%3Abug)
- [ ][All](https://github.com/mithi/hexapod-robot-simulator/issues)
Expand Down Expand Up @@ -79,9 +80,7 @@ Running on http://127.0.0.1:8050/

- This implementation uses matrices, **NOT** quaternions. I'm aware that quaternions is far superior in every single way. In the (un)forseeable future, maybe?

- 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 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.
- 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. 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 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/)

Expand Down
47 changes: 21 additions & 26 deletions hexapod/ground_contact_solver/ground_contact_solver.py
Original file line number Diff line number Diff line change
Expand Up @@ -23,51 +23,45 @@
3. Determining the height of the center of gravity (cog) wrt to the ground.
ie this height is distance between the cog and the plane defined by ground contacts.
"""
from math import isclose
from itertools import combinations
from hexapod.points import dot, get_normal_given_three_points
from hexapod.ground_contact_solver.helpers import is_stable
from hexapod.ground_contact_solver.shared import (
is_stable,
is_lower,
find_legs_on_ground,
LEG_TRIOS,
)


def compute_orientation_properties(legs, tol=1):
def compute_orientation_properties(legs):
"""
Returns:
- Which legs are on the ground
- Normal vector of the plane defined by these legs
- Distance of this plane to center of gravity
"""
trio, n, height = find_ground_plane_properties(legs)
n, height = find_ground_plane_properties(legs)

# This pose is unstable, The hexapod has no balance
if trio is None:
# this pose is unstable, The hexapod has no balance
if n is None:
return [], None, None

# get all the legs on the ground
legs_on_ground = []
return find_legs_on_ground(legs, n, height), n, height

for leg in legs:
_height = -dot(n, leg.ground_contact())
if isclose(height, _height, abs_tol=tol):
legs_on_ground.append(leg)

return legs_on_ground, n, height


def find_ground_plane_properties(legs, tol=1):
def find_ground_plane_properties(legs):
"""
Return three legs forming a stable position from legs,
or None if no three legs satisfy this requirement.
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 = 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]
# the corresponding other_trio of (2, 3, 5) is (0, 1, 4)
# order is not important ie (2, 3, 5) is the same as (5, 3, 2)
for trio, other_trio in zip(trios, reversed(trios)):
for trio in LEG_TRIOS:
p0, p1, p2 = [ground_contacts[i] for i in trio]

if not is_stable(p0, p1, p2):
Expand All @@ -94,21 +88,22 @@ def find_ground_plane_properties(legs, tol=1):
# using p0, p1 or p2 should yield the same result
height = -dot(n, p0)

# height should be the most largest since
# height should be the highest since
# the plane defined by this trio is on the ground
# the other legs ground contact cannot be lower than the ground
other_trio = [i for i in range(6) if i not in trio]
other_points = [ground_contacts[i] for i in other_trio]
if no_other_legs_lower(n, height, other_points, tol):
if no_other_legs_lower(n, height, other_points):
# Found one!
return [p0, p1, p2], n, height
return n, height

# Nothing met the condition
return None
return None, None


def no_other_legs_lower(n, height, other_points, tol):
def no_other_legs_lower(n, height, other_points):
for point in other_points:
_height = -dot(n, point)
if _height > height + tol:
if is_lower(point, height, n):
return False

return True
85 changes: 39 additions & 46 deletions hexapod/ground_contact_solver/ground_contact_solver2.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,38 +27,45 @@
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
import random
from hexapod.ground_contact_solver.shared import (
is_stable,
is_lower,
find_legs_on_ground,
SOME_LEG_TRIOS,
ADJACENT_LEG_TRIOS,
)
from hexapod.points import get_normal_given_three_points, dot

TOL = 1.0
OTHER_POINTS_MAP = {1: (2, 3), 2: (3, 1), 3: (1, 2)}


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]}
JOINT_TRIOS = []
for i in range(3, 0, -1):
for j in range(3, 0, -1):
for k in range(3, 0, -1):
JOINT_TRIOS.append((i, j, k))


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]
"""
Returns:
- Which legs are on the ground
- Normal vector of the plane defined by these legs
- Distance of this plane to center of gravity
"""
# prefer leg combinations where legs are not adjacent to each other
# introduce some randomness so we are not bias in
# choosing on stable position over another
shuffled_some_leg_trios = random.sample(SOME_LEG_TRIOS, len(SOME_LEG_TRIOS))
leg_trios = shuffled_some_leg_trios + ADJACENT_LEG_TRIOS

for leg_trio in leg_trios:

other_leg_trio = [i for i in range(6) if i not in leg_trio]
other_three_legs = [legs[i] for i in other_leg_trio]
three_legs = [legs[i] for i in leg_trio]

for joint_trio in joint_trios:
for joint_trio in JOINT_TRIOS:

p0, p1, p2 = [legs[i].get_point(j) for i, j in zip(leg_trio, joint_trio)]

Expand All @@ -80,32 +87,18 @@ def compute_orientation_properties(legs):
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:
point = leg.get_point(other_point_id)
if is_lower(point, height, n):
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
def other_leg_joints_break_condition(other_three_legs, n, height):
for leg in other_three_legs:
for point in leg.all_points[1:]:
if is_lower(point, height, n):
return True
return False
Original file line number Diff line number Diff line change
@@ -1,10 +1,40 @@
from math import isclose
from hexapod.points import (
Point,
dot,
cross,
vector_from_to,
)

# Prioritize legs that are not adjacent to each other
SOME_LEG_TRIOS = [
(0, 1, 3),
(0, 1, 4),
(0, 2, 3),
(0, 2, 4),
(0, 2, 5),
(0, 3, 4),
(0, 3, 5),
(1, 2, 4),
(1, 2, 5),
(1, 3, 4),
(1, 3, 5),
(1, 4, 5),
(2, 3, 5),
(2, 4, 5),
]

ADJACENT_LEG_TRIOS = [
(0, 1, 2),
(1, 2, 3),
(2, 3, 4),
(3, 4, 5),
(0, 4, 5),
(0, 1, 5),
]

LEG_TRIOS = SOME_LEG_TRIOS + ADJACENT_LEG_TRIOS


# math.stackexchange.com/questions/544946/
# determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
Expand Down Expand Up @@ -35,3 +65,20 @@ def is_stable(p1, p2, p3, tol=0.001):
cond2 = min_val <= beta <= max_val
cond3 = min_val <= gamma <= max_val
return cond1 and cond2 and cond3


def is_lower(point, height, n, tol=1):
_height = -dot(n, point)
return _height > height + tol


def find_legs_on_ground(legs, n, height, tol=1):
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
6 changes: 4 additions & 2 deletions hexapod/points.py
Original file line number Diff line number Diff line change
Expand Up @@ -52,10 +52,12 @@ def __repr__(self):
def __str__(self):
return repr(self)

def __eq__(self, other):
def __eq__(self, other, percent_tol=0.0075):
if not isinstance(other, Point):
return False
equal_val = np.allclose(self.vec, other.vec, atol=0.01)

tol = length(self) * percent_tol
equal_val = np.allclose(self.vec, other.vec, atol=tol)
equal_name = self.name == other.name
return equal_val and equal_name

Expand Down
8 changes: 4 additions & 4 deletions tests/helpers.py
Original file line number Diff line number Diff line change
Expand Up @@ -18,20 +18,20 @@ def msg(a, b):
return f"Unequal Points\nexpected: {a}\n....found: {b}\n(case: {description})"

for point_a, point_b in zip(correct_body_points, hexapod.body.all_points):
assert point_a == point_b, msg(point_a, point_b)
assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b)

for leg_set, leg in zip(correct_leg_points, hexapod.legs):
for point_a, point_b in zip(leg_set, leg.all_points):
assert point_a == point_b, msg(point_a, point_b)
assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b)


def assert_two_hexapods_equal(hexapod1, hexapod2, description):
def msg(a, b):
return f"Unequal Points\n1: {a}\n2:{b}\n(case: {description})"

for point_a, point_b in zip(hexapod1.body.all_points, hexapod2.body.all_points):
assert point_a == point_b, msg(point_a, point_b)
assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b)

for leg_a, leg_b in zip(hexapod1.legs, hexapod2.legs):
for point_a, point_b in zip(leg_a.all_points, leg_b.all_points):
assert point_a == point_b, msg(point_a, point_b)
assert point_a.__eq__(point_b, percent_tol=0.0075), msg(point_a, point_b)
6 changes: 5 additions & 1 deletion tests/test_ik.py
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,13 @@ def test_sample_ik():
assert_ik_solver(ik_solver.inverse_kinematics_update, case)


def test_points_ik2():
def test_points_ik2_assume_ground_targets():
for case in CASES:
assert_ik_points(case, True)


def test_points_ik2_dont_assume_ground_targets():
for case in CASES:
assert_ik_points(case, False)


Expand Down

0 comments on commit ee3c1dd

Please sign in to comment.