def draw_supporting_forces(self, wrench, point, force_scale=0.0025,
**kwargs):
support = self.find_supporting_forces(wrench, point, **kwargs)
if support is None:
warn("draw_supporting_support(): there are no supporting forces")
return
handles = [draw_force(c, fc, force_scale) for (c, fc) in support]
return handles
def on_tick(self, sim):
qd = self.robot.qd
qdd_disc = (qd - self.qd_prev) / sim.dt
self.qd_prev = qd
qdd = qdd_disc + exp(-1. / self.delay) * (self.qdd_prev - qdd_disc)
com = self.robot.com
Pd = self.robot.mass * self.robot.compute_com_acceleration(qdd)
self.handle = draw_force(com, Pd)
def run_force_thread(self, com, mass, sleep):
while self.force_lock:
with self.force_lock:
cf = self.find_static_supporting_forces(com.p, mass)
if cf is not None:
self.force_handles = [draw_force(c, fc) for (c, fc) in cf]
else: # no force
self.force_handles = []
sleep()
def on_tick(self, sim):
"""Find supporting contact forces at each COM acceleration update."""
support = self.contact_set.find_static_supporting_forces(
self.com.p, self.com.mass)
if not support:
self.handles = []
sim.viewer.SetBkgndColor(self.KO_COLOR)
self.last_bkgnd_switch = time()
else:
self.handles = [
draw_force(c, fc, self.force_scale) for (c, fc) in support]
if self.last_bkgnd_switch is not None \
and time() - self.last_bkgnd_switch > 0.2:
# let's keep epilepsy at bay
sim.viewer.SetBkgndColor(self.OK_COLOR)
self.last_bkgnd_switch = None
def on_tick(self, sim):
"""Find supporting contact forces at each COM acceleration update."""
com = self.com
comdd = com.pdd # needs to be stored by the user
gravity = sim.gravity
wrench = hstack([com.mass * (comdd - gravity), zeros(3)])
support = self.contact_set.find_supporting_forces(
wrench, com.p, com.mass, 10.)
if not support:
self.handles = []
sim.viewer.SetBkgndColor(self.KO_COLOR)
self.last_bkgnd_switch = time()
else:
self.handles = [
draw_force(c, fc, self.force_scale) for (c, fc) in support]
if self.last_bkgnd_switch is not None \
and time() - self.last_bkgnd_switch > 0.2:
# let's keep epilepsy at bay
sim.viewer.SetBkgndColor(self.OK_COLOR)
self.last_bkgnd_switch = None
def show_comd(self):
"""Show an arrow representing COM velocity."""
self.__show_comd = True
self.__comd_handle = draw_force(self.com, self.comd, scale=1.)