def DoCalcVectorOutput(self, context, pendulum_state, unused, output):
xbar = copy(pendulum_state)
xbar[0] = wrap_to(xbar[0], 0, 2. * math.pi) - math.pi
# If x'Sx <= 2, then use the LQR controller
if (xbar.dot(self.S.dot(xbar)) < 2.):
output[:] = -self.K.dot(xbar)
else:
theta = pendulum_state[0]
thetadot = pendulum_state[1]
total_energy = TotalEnergy(pendulum_state, self.params)
desired_energy = TotalEnergy(UprightState().CopyToVector(),
self.params)
output[:] = (self.params.damping() * thetadot - .1 * thetadot *
(total_energy - desired_energy))
def DoCalcVectorOutput(self, context, pendulum_state, unused, output):
xbar = copy(pendulum_state)
xbar[0] = wrap_to(xbar[0], 0, 2.*math.pi) - math.pi
# If x'Sx <= 2, then use the LQR controller
if (xbar.dot(self.S.dot(xbar)) < 2.):
output[:] = -self.K.dot(xbar)
else:
theta = pendulum_state[0]
thetadot = pendulum_state[1]
total_energy = TotalEnergy(pendulum_state, self.params)
desired_energy = TotalEnergy(UprightState().CopyToVector(),
self.params)
output[:] = self.params.damping() * thetadot - \
.1 * thetadot * (total_energy - desired_energy)