class Stance(ContactSet):
"""
Stances extend contact sets with COM locations.
"""
def __init__(self, com, left_foot=None, right_foot=None, left_hand=None,
right_hand=None, label=None, duration=None):
"""
Create a new stance.
INPUT:
- ``com`` -- coordinates or Point object
- ``left_foot`` -- (optional) left foot Contact
- ``right_foot`` -- (optional) right foot Contact
- ``left_hand`` -- (optional) left hand Contact
- ``right_hand`` -- (optional) right hand Contact
- ``label`` -- (optional) string label, e.g. phase in walking FSM
- ``duration`` -- (optional) timing information
"""
contacts = {}
if not issubclass(type(com), Point):
com = Point(com, visible=False)
if left_foot:
contacts['left_foot'] = left_foot
if left_hand:
contacts['left_hand'] = left_hand
if right_foot:
contacts['right_foot'] = right_foot
if right_hand:
contacts['right_hand'] = right_hand
self.com = com
self.duration = duration
self.label = label
self.left_foot = left_foot
self.left_hand = left_hand
self.right_foot = right_foot
self.right_hand = right_hand
super(Stance, self).__init__(contacts)
self.compute_stability_criteria()
def compute_stability_criteria(self):
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
def dist_to_sep_edge(self, com):
"""
Algebraic distance to the edge of the static-equilibrium polygon
(positive inside, negative outside).
"""
A, b = self.sep.hrep_pair
alg_dists = (b - dot(A, com[:2])) / self.sep_norm
return min(alg_dists)
def _expand_reduced_pendular_cone(reduced_hull, zdd_max=None):
g = -gravity[2] # gravity constant (positive)
zdd = +g if zdd_max is None else zdd_max
vertices_at_zdd = [
array([a * (g + zdd), b * (g + zdd), zdd])
for (a, b) in reduced_hull
]
return Polytope(vertices=[gravity] + vertices_at_zdd)
def __init__(self,
com,
left_foot=None,
right_foot=None,
left_hand=None,
right_hand=None,
label=None,
duration=None):
contacts = filter(None, [left_foot, right_foot, left_hand, right_hand])
super(Stance, self).__init__(contacts)
if not issubclass(type(com), Point):
com = Point(com, visible=False)
self.com = com
self.duration = duration
self.label = label
self.left_foot = left_foot
self.left_hand = left_hand
self.right_foot = right_foot
self.right_hand = right_hand
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
class Stance(ContactSet):
"""
Stances extend contact sets with COM locations.
Parameters
----------
com : array or Point
COM given by coordinates or a Point object.
left_foot : Contact, optional
Left foot contact.
right_foot : Contact, optional
Right foot contact.
left_hand : Contact, optional
Left hand contact.
right_hand : Contact, optional
Right hand contact.
label : string, optional
Label for the current contact phase.
duration : double, optional
Timing information.
Note
----
Stances proactively computed their contact wrench cone and
static-equilibrium polygon when they are instanciated. The Stance class is
therefore best-suited for planning; under time constraints, consider using
bare ContactSets instead.
"""
def __init__(self,
com,
left_foot=None,
right_foot=None,
left_hand=None,
right_hand=None,
label=None,
duration=None):
contacts = filter(None, [left_foot, right_foot, left_hand, right_hand])
super(Stance, self).__init__(contacts)
if not issubclass(type(com), Point):
com = Point(com, visible=False)
self.com = com
self.duration = duration
self.label = label
self.left_foot = left_foot
self.left_hand = left_hand
self.right_foot = right_foot
self.right_hand = right_hand
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
@property
def contact(self):
"""Unique contact if the Stance is single-support."""
assert self.nb_contacts == 1
for contact in self.contacts:
return contact
def dist_to_sep_edge(self, com):
"""
Algebraic distance to the edge of the static-equilibrium polygon
(positive inside, negative outside).
"""
A, b = self.sep.hrep_pair
alg_dists = (b - dot(A, com[:2])) / self.sep_norm
return min(alg_dists)
def compute_stability_criteria(self):
self.cwc = self.compute_wrench_face([0, 0, 0]) # calls cdd
self.sep = Polytope(vertices=self.compute_static_equilibrium_polygon())
self.sep.compute_hrep()
A, _ = self.sep.hrep_pair
self.sep_norm = array([norm(a) for a in A])
