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)