-
Notifications
You must be signed in to change notification settings - Fork 112
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
❗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
- Loading branch information
Showing
13 changed files
with
249 additions
and
99 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
111 changes: 111 additions & 0 deletions
111
hexapod/ground_contact_solver/ground_contact_solver2.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 |
Oops, something went wrong.