def __init__(self, start_com, end_com, stance, radius, margin):
n = normalize(end_com - start_com)
t = array([0., 0., 1.])
t -= dot(t, n) * n
t = normalize(t)
b = cross(n, t)
cross_section = [
dx * t + dy * b
for (dx, dy) in [(+radius,
+radius), (+radius,
-radius), (-radius,
+radius), (-radius,
-radius)]
]
tube_start = start_com - margin * n
tube_end = end_com + margin * n
primal_vrep = [tube_start + s for s in cross_section] + \
[tube_end + s for s in cross_section]
primal_hrep = compute_polytope_hrep(primal_vrep)
dual_vrep = stance.compute_pendular_accel_cone(
com_vertices=primal_vrep)
dual_hrep = compute_polytope_hrep(dual_vrep)
self.dual_hrep = dual_hrep
self.dual_vrep = dual_vrep
self.primal_hrep = primal_hrep
self.primal_vrep = primal_vrep
def compute_dual_hrep(self):
"""Compute halfspaces of the dual cones."""
for (stance_id, cone_vertices) in enumerate(self.dual_vrep):
B, c = compute_polytope_hrep(cone_vertices)
self.dual_hrep.append((B, c))
def compute_primal_hrep(self):
"""Compute halfspaces of the primal tube."""
try:
self.full_hrep = (compute_polytope_hrep(self.full_vrep))
except RuntimeError as e:
raise Exception("Could not compute primal hrep: %s" % str(e))