def __init__(self, bodies, ctrl, *args, **kwargs):
""" TODO.
"""
dTwistTask.__init__(self, *args, **kwargs)
self.comObs = CoMObserver(bodies)
self._bodies = bodies
self._ctrl = ctrl
for b in self._bodies:
assert(isinstance(b, Body))
assert(isinstance(ctrl, dTwistCtrl))
if self._cdof == []:
self._cdof = arange(3)
else:
assert(len(self._cdof) <= 3)
assert(all([cd < 3 for cd in self._cdof]))
class CoMTask(dTwistTask):
""" TODO.
"""
def __init__(self, bodies, ctrl, *args, **kwargs):
""" TODO.
"""
dTwistTask.__init__(self, *args, **kwargs)
self.comObs = CoMObserver(bodies)
self._bodies = bodies
self._ctrl = ctrl
for b in self._bodies:
assert(isinstance(b, Body))
assert(isinstance(ctrl, dTwistCtrl))
if self._cdof == []:
self._cdof = arange(3)
else:
assert(len(self._cdof) <= 3)
assert(all([cd < 3 for cd in self._cdof]))
def init(self, world, LQP_ctrl):
"""
"""
dTwistTask.init(self, world, LQP_ctrl)
self._ctrl.init(world, LQP_ctrl)
self.comObs.init(world)
def _update_matrices(self, rstate, dt):
"""
"""
self.comObs.update(dt)
#com_gpos, J, dJ = com_properties(self._bodies)
com_gpos = self.comObs.get_CoMPosition()
J = self.comObs.get_CoMJacobian()
dJ = self.comObs.get_CoMdJacobian()
com_gvel = dot(J, rstate['gvel'])
cmd = self._ctrl.update(com_gpos, com_gvel, rstate, dt)
self._J[:] = J[self._cdof, :]
self._dJ[:] = dJ[self._cdof, :]
self._dVdes[:] = cmd[self._cdof]
