def compute_dual_hrep(self):
"""
Compute halfspaces of the dual cones.
NB: not optimized, we simply call cdd here.
"""
for (stance_id, cone_vertices) in enumerate(self.dual_vrep):
B, c = Polytope.hrep(cone_vertices)
# B, c = (B.astype(float64), c.astype(float64)) # if using pyparma
self.dual_hrep.append((B, c))
def compute_primal_hrep(self):
"""
Compute halfspaces of the primal tube.
NB: not optimized, we simply call cdd here.
"""
try:
self.full_hrep = (Polytope.hrep(self.full_vrep))
except RuntimeError as e:
raise TubeError("Could not compute primal hrep: %s" % str(e))
def compute_primal_hrep(self):
"""
Compute halfspaces of the primal tubes (contrary to COMTube, there are
two tubes here).
NB: not optimized, we simply call cdd here.
"""
for (stance_id, vertices) in enumerate(self.primal_vrep):
try:
self.primal_hrep.append(Polytope.hrep(vertices))
except RuntimeError as e:
raise TubeError("Could not compute primal hrep: %s" % str(e))
def compute_stability_criteria(self):
def update_contacts(X, Y, mu):
for contact in self.contacts:
contact.X = X
contact.Y = Y
contact.friction = mu
X = self.contact_dict.values()[0].X
Y = self.contact_dict.values()[0].Y
mu = self.contact_dict.values()[0].friction
topp_scale = 0.75
update_contacts(topp_scale * X, topp_scale * Y, topp_scale * mu)
self.cwc_topp = self.compute_wrench_cone([0, 0, 0])
update_contacts(X, Y, mu)
self.sep = self.compute_static_equilibrium_polygon()
self.sep_hrep = Polytope.hrep(self.sep)
def compute_stability_criteria(self):
self.cwc = self.compute_wrench_cone([0, 0, 0]) # calls cdd
# self.cwc = compute_cwc_pyparma(self, [0, 0, 0])
m = RobotModel.mass # however, the SEP does not depend on this
self.sep = self.compute_static_equilibrium_polygon()
self.sep_hrep = Polytope.hrep(self.sep)