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])