def create_polyhedron(self, contacts):
self.handle = None
self.nr_iter = 0
try:
self.polyhedron = stab.StabilityPolygon(robot.mass,
dimension=3,
radius=1.5)
stab_contacts = []
for contact in contacts:
hmatrix = matrixFromPose(contact.pose)
X, Y = contact.shape
displacements = [
array([[X, Y, 0]]).T,
array([[-X, Y, 0]]).T,
array([[-X, -Y, 0]]).T,
array([[X, -Y, 0]]).T
]
for disp in displacements:
stab_contacts.append(
stab.Contact(
contact.friction,
hmatrix[:3, 3:] + hmatrix[:3, :3].dot(disp),
hmatrix[:3, 2:3]))
self.polyhedron.contacts = stab_contacts
self.polyhedron.select_solver(self.method)
self.polyhedron.make_problem()
self.polyhedron.init_algo()
self.polyhedron.build_polys()
vertices = self.polyhedron.polyhedron()
self.handle = draw_polyhedron([(x[0], x[1], x[2])
for x in vertices])
except Exception as e:
print("SupportPolyhedronDrawer: {}".format(e))
def refine_polyhedron(self):
try:
self.polyhedron.next_edge()
vertices = self.polyhedron.polyhedron()
self.handle = draw_polyhedron([(x[0], x[1], x[2])
for x in vertices])
except Exception as e:
print("SupportPolyhedronDrawer: {}".format(e))
def update_cone(self):
self.handle = None
try:
vertices = self.stance.compute_pendular_accel_cone()
vscale = [self.stance.com.p + self.scale * acc for acc in vertices]
self.handle = draw_polyhedron(vscale, 'r.-#')
except Exception as e:
print("AccelConeDrawer: {}".format(e))
def draw_primal(self, tube):
self.poly_handles = []
colors = [(0.5, 0.5, 0., 0.3), (0., 0.5, 0.5, 0.3)]
if tube.start_stance.label.startswith('SS'):
colors.reverse()
for (i, vertices) in enumerate(tube.primal_vrep):
color = colors[i]
if len(vertices) == 1:
self.poly_handles.append(
draw_point(vertices[0], color=color, pointsize=0.01))
else:
self.poly_handles.extend(
draw_polyhedron(vertices, '*.-#', color=color))
def draw(self, acc_scale=0.05):
from pymanoid.gui import draw_cone, draw_polyhedron
handles = []
cyan, yellow = (0., 0.5, 0.5, 0.3), (0.5, 0.5, 0., 0.3)
handles.extend(draw_polyhedron(self.primal_vrep, '*.-#', color=cyan))
origin = self.primal_vrep[0]
apex = origin + [0., 0., acc_scale * -9.81]
vscale = [acc_scale * array(v) + origin for v in self.dual_vrep]
handles.extend(
draw_cone(apex=apex,
axis=[0, 0, 1],
section=vscale[1:],
combined='r-#',
color=yellow))
return handles