def display_contact_forces(self, q, qd, qdd):
contacts, f = self.check_contact_forces(q, qd, qdd)
for i in xrange(8):
for s in ["Contact%d" % i, "Force%d" % i]:
kinbody = self.env.GetKinBody(s)
if kinbody is not None:
self.env.Remove(kinbody)
for i, c in enumerate(contacts):
cur_f = f[3 * i:3 * (i + 1)]
display_box(self.env, c, box_id="Contact%d" % i)
display_force(self.env, c, cur_f, "Force%d" % i)
def display_zmp(self, q, qd, qdd):
zmp = self.compute_zmp(q, qd, qdd)
zmp[2] = 0.
display_box(self.env, zmp, box_id="ZMP", thickness=0.02)
def display_floor_com(self, q):
com = self.compute_com(q)
com[2] = 0.
box_id = "ZMP" # krooooooooooon
display_box(self.env, com, box_id=box_id, thickness=0.04)
def display_com(self, q):
com = self.compute_com(q)
display_box(self.env, com, box_id="COM", thickness=0.04)
