def compute_dual_vrep(self):
"""Compute vertices of the dual cones."""
if len(self.primal_vrep) == 1:
dual = self.start_stance.compute_pendular_accel_cone(
com=self.primal_vrep[0])
self.dual_vrep = [dual.vertices]
return
# now, len(self.primal_vrep) == 2
ds_then_ss = len(self.primal_vrep[0]) > 1
if ds_then_ss:
ds_vertices_2d = self.start_stance.compute_pendular_accel_cone(
com=self.full_vrep, reduced=True)
ss_vertices_2d = self.next_stance.compute_pendular_accel_cone(
com=self.primal_vrep[1], reduced=True)
else: # SS, then DS
ss_vertices_2d = self.start_stance.compute_pendular_accel_cone(
com=self.primal_vrep[0], reduced=True)
ds_vertices_2d = self.next_stance.compute_pendular_accel_cone(
com=self.full_vrep, reduced=True)
ss_vertices_2d = intersect_polygons(ds_vertices_2d, ss_vertices_2d)
ds_cone = ContactSet._expand_reduced_pendular_cone(ds_vertices_2d)
ss_cone = ContactSet._expand_reduced_pendular_cone(ss_vertices_2d)
if ds_then_ss:
self.dual_vrep = [ds_cone.vertices, ss_cone.vertices]
else: # SS, then DS
self.dual_vrep = [ss_cone.vertices, ds_cone.vertices]
def compute_dual_vrep(self):
"""Compute vertices of the dual cones."""
if len(self.primal_vrep) == 1:
dual = self.start_stance.compute_pendular_accel_cone(
com=self.primal_vrep[0])
self.dual_vrep = [dual.vertices]
return
# now, len(self.primal_vrep) == 2
ds_then_ss = len(self.primal_vrep[0]) > 1
if ds_then_ss:
ds_vertices_2d = self.start_stance.compute_pendular_accel_cone(
com=self.full_vrep, reduced=True)
ss_vertices_2d = self.next_stance.compute_pendular_accel_cone(
com=self.primal_vrep[1], reduced=True)
else: # SS, then DS
ss_vertices_2d = self.start_stance.compute_pendular_accel_cone(
com=self.primal_vrep[0], reduced=True)
ds_vertices_2d = self.next_stance.compute_pendular_accel_cone(
com=self.full_vrep, reduced=True)
ss_vertices_2d = intersect_polygons(ds_vertices_2d, ss_vertices_2d)
ds_cone = ContactSet._expand_reduced_pendular_cone(ds_vertices_2d)
ss_cone = ContactSet._expand_reduced_pendular_cone(ss_vertices_2d)
if ds_then_ss:
self.dual_vrep = [ds_cone.vertices, ss_cone.vertices]
else: # SS, then DS
self.dual_vrep = [ss_cone.vertices, ds_cone.vertices]
def on_tick(self, sim):
if wpg.is_in_double_support:
self.__time_ds += 1
elif self.__time_nonqs is not None:
try: # the robot is in single support
cs = ContactSet([wpg.support_contact])
p = pendulum.com_state.p
cs.find_static_supporting_wrenches(p, robot.mass)
except ValueError:
self.__time_nonqs += 1
def recompute(self, contact, comdd, am):
assert type(contact) in [Contact, ContactSet]
if type(contact) is Contact:
self.contact_set = ContactSet([contact])
else: # type(contact) is ContactSet
self.contact_set = contact
self.point_mass.pdd = comdd
self.am = am if am is not None else zeros(3)
super(WrenchDrawer, self).on_tick(sim)
def on_tick(self, sim):
com = pendulum.com_state.p
comdd = pendulum.omega2 * (com - pendulum.zmp_state.p) + gravity
self.point_mass.set_pos(com)
self.point_mass.set_vel(pendulum.com_state.pd)
self.contact_set = ContactSet([wpg.support_contact] + (
[wpg.swing_start] if wpg.is_in_double_support else []))
self.point_mass.pdd = comdd
super(WrenchDrawer, self).on_tick(sim)
if self.handles:
self.handles[0][-1].SetShow(False)
def __init__(self):
point_mass = PointMass(robot.com, robot.mass, visible=False)
contact_set = ContactSet([wpg.support_contact])
super(WrenchDrawer, self).__init__(point_mass, contact_set)
self.am = zeros(3)