def compute_orientation_properties(legs, tol=1):
"""
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 = three_ids_of_ground_contacts(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
legs_on_ground = []
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 three_ids_of_ground_contacts(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
"""
trios = set_of_trios_from_six()
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)):
p0, p1, p2 = [ground_contacts[i] for i in trio]
if not check_stability(p0, p1, p2):
continue
# Get the vector normal to plane defined by these points
# ❗IMPORTANT: The normal is always pointing up
# because of how we specified the order of the trio
# (and the legs in general)
# starting from middle-right (id:0) to right back (id:5)
# always towards one direction (ccw)
n = get_normal_given_three_points(p0, p1, p2)
# p0 is vector from cog (0, 0, 0) to ground contact
# dot product of this and normal we get the
# hypothetical (negative) height of ground contact to cog
#
# cog * ^ (normal_vector) ----
# \ | |
# \ | -height
# \| |
# V p0 (foot_tip) ------
#
# using p0, p1 or p2 should yield the same result
height = -dot(n, p0)
# 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!
# Nothing met the condition
return None
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
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
"""
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 in LEG_TRIOS:
p0, p1, p2 = [ground_contacts[i] for i in trio]
if not is_stable(p0, p1, p2):
continue
# Get the vector normal to plane defined by these points
# ❗IMPORTANT: The normal is always pointing up
# because of how we specified the order of the trio
# (and the legs in general)
# starting from middle-right (id:0) to right back (id:5)
# always towards one direction (ccw)
n = get_normal_given_three_points(p0, p1, p2)
# p0 is vector from cog (0, 0, 0) to ground contact
# dot product of this and normal we get the
# hypothetical (negative) height of ground contact to cog
#
# cog * ^ (normal_vector) ----
# \ | |
# \ | -height
# \| |
# V p0 (foot_tip) ------
#
# using p0, p1 or p2 should yield the same result
height = -dot(n, p0)
# 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):
# Found one!
return n, height
# Nothing met the condition
return None, None
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 = Vector(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
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
"""
# prefer leg combinations where legs are not adjacent to each other
# introduce some randomness so we are not bias in
# choosing one 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:
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 is_lower(point, height, n, tol=1):
_height = -dot(n, point)
return _height > height + tol