❗Algorithm for computing hexapod orientation does not account for all cases
Opened this issue · 8 comments
❗❗❗
The old 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 current algorithm will still be used for the leg-patterns page, and the inverse-kinematics page. A more general algorithm will be used for the kinematics page
The following algorithm can be used for the leg-patterns page and inverse-kinematics page.
(How to find the ground contact points, tilt, and height of the hexapod)
But this can't be used in general.
This is because how we determine the ground contact ie Linkage.compute_ground_contact() doesn't always yield the correct result.
hexapod-robot-simulator/hexapod/linkage.py
Line 162 in 45acb5a
Here's the new algorithm that accounts for most cases
Examples:
This seems like it would choose the triangle most parallel to the hexapod body, but it seems like it has a few problems (eg. it ignores more angled planes that are also valid and or more stable). I think that a more robust option might be to do the following instead:
Take all sets of any three points and their corresponding triangle where:
- The center of gravity is above the triangle.
- the (0, 0, 0) check is incorrect in several ways - drawing a perpendicular line from the triangle plane to the center of gravity, and then checking that the line is inside the triangle would be appropriate here.
- No points exist under the plane (so the sign of all their heights relative to the plane is correct)
- This could possibly be used to search the problem space further (eg. take a point that is under the plane, and form a triangle with that point instead).
This could also iterate through all valid sets of points (using the yield keyword to return valid solutions), form a polygon from the ground contacts that occur when choosing the points, and then the user could choose a solution which:
- minimizes the distance between the center of gravity and the center of the polygon, and maximizes the distance from the center of gravity to the edge.
- minimizes the height.
- maximizes the area of the polygon (optional, this is somewhat tested above).
- achieves the previous goals to a satisfactory degree.
A score could be computed like so (a smaller score = better):
height = distance_above(center_of_gravity, polygon)
score = (
height *
(distance_from_center(center_of_gravity, polygon) + perturbation) /
(distance_from_edge(center_of_gravity, polygon)
)
distance_above should find the length of a vector perpendicular to the polygon which touches the c.o.g..
distance_from_center should project the c.o.g. onto the polygon, and find the distance from the projected point to the center.
distance_from_edge should project the c.o.g. onto the polygon, and find the minimum distance from the projected point to any edge in the polygon.
Being closer to the center of the polygon is rewarded, but being close to any edge is punished (eg. for small polygon, you might be very close to both the edge and center) - and we add a small perturbation to ensure that the hexapod is still stable even if it moves inside the polygon. We also reward the center of gravity being close to the ground, since this is also more stable.
@philippeitis Thanks for your input, I'll take note of it.
For documentation purposes, here's another WRONG algorithm
Given:
find the lowest point given 18 points
(6 legs, 3 points each leg) that's the first point
now we have to find the second point from
the 15 candidate points (3 points each leg from 5 remaining legs)
draw a vector from the first point to each
of the 15 candidate points
get the angle between each vector and the
plane defined the hexapod's body
get the two lowest angles,
the two points corresponding to these two angles are the second and third point
https://www.quora.com/How-do-we-find-the-angle-between-a-vector-and-a-vector-plane-which-itself-is-made-by-two-vectors
assert that given the plane defined by these three points,
no other points are lower when the hexapod is reoriented
New Algorithm I implemented
This might also be a wrong algorithm but right now it's the best one i can think of
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)
https://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
https://gamedev.stackexchange.com/questions/23743/whats-the-most-efficient-way-to-find-barycentric-coordinates
https://en.wikipedia.org/wiki/Barycentric_coordinate_system
The current algorithm produces this results:
The foot tip should be the ground contact (z=0) and NOT the femur point ( z= 26)
The algorithm in theory is correct but there is a bug somewhere because it works okay in the javascript implementation: https://github.com/mithi/hexapod/blob/master/src/hexapod/solvers/orient/orientSolverGeneral.js