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]