from __future__ import print_function

import roslib; roslib.load_manifest('dynamaestro')
import rospy
from dynamaestro.msg import VectorStamped


class LQRController(rospy.Subscriber):
    def __init__(self, intopic, outtopic):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)

    def read_state(self, vs):
        self.intopic.publish(VectorStamped(point=[-(vs.point[0] + 2.4142*vs.point[1] + 2.4142*vs.point[2])]))


if __name__ == "__main__":
    rospy.init_node("lqr", anonymous=True)
    lqrc = LQRController("input", "output")
    rospy.spin()

from __future__ import print_function

import roslib; roslib.load_manifest('dynamaestro')
import rospy
from dynamaestro.msg import VectorStamped

from control import lqr
import numpy as np


class StateFeedback(rospy.Subscriber):
    def __init__(self, intopic, outtopic, K=None):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
        self.K = K

    def read_state(self, vs):
        self.intopic.publish(VectorStamped(point=[-np.dot(self.K, np.asarray(vs.point))]))

class LQRController(StateFeedback):
    def __init__(self, intopic, outtopic,
                 A=None, B=None, Q=None, R=None):
        if A is None and B is None:
            A = np.array([[0., 1, 0],
                          [0,  0, 1],
                          [0,  0, 0]])
            B = np.array([[0.], [0], [1]])
        if Q is None and R is None:
            Q = np.diag([1.,1,1])
            R = np.diag([1.])
        K, S, E = lqr(A,B,Q,R)
        StateFeedback.__init__(self, intopic, outtopic, K)


if __name__ == "__main__":
    rospy.init_node("lqr", anonymous=True)

    n = 1  # Number of output dimensions
    m = 3  # Number of derivatives

    A = np.diag(np.ones(m-1), k=1)
    B = np.zeros((m, 1))
    B[-1,0] = 1.0
    Q = np.diag(np.ones(m))
    R = np.diag([1.])

    lqrc = LQRController("input", "output", A, B, Q, R)
    rospy.spin()

Improve LQR example for integrator_chains domain
Improve LQR example for integrator_chains domain

  from __future__ import print_function
  
  import roslib; roslib.load_manifest('dynamaestro')
  import rospy
  from dynamaestro.msg import VectorStamped
  
+ from control import lqr
+ import numpy as np
  
- class LQRController(rospy.Subscriber):
+ 
+ class StateFeedback(rospy.Subscriber):
-     def __init__(self, intopic, outtopic):
+     def __init__(self, intopic, outtopic, K=None):
          rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
          self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
+         self.K = K
  
      def read_state(self, vs):
-         self.intopic.publish(VectorStamped(point=[-(vs.point[0] + 2.4142*vs.point[1] + 2.4142*vs.point[2])]))
+         self.intopic.publish(VectorStamped(point=[-np.dot(self.K, np.asarray(vs.point))]))
+ 
+ class LQRController(StateFeedback):
+     def __init__(self, intopic, outtopic,
+                  A=None, B=None, Q=None, R=None):
+         if A is None and B is None:
+             A = np.array([[0., 1, 0],
+                           [0,  0, 1],
+                           [0,  0, 0]])
+             B = np.array([[0.], [0], [1]])
+         if Q is None and R is None:
+             Q = np.diag([1.,1,1])
+             R = np.diag([1.])
+         K, S, E = lqr(A,B,Q,R)
+         StateFeedback.__init__(self, intopic, outtopic, K)
  
  
  if __name__ == "__main__":
      rospy.init_node("lqr", anonymous=True)
+ 
+     n = 1  # Number of output dimensions
+     m = 3  # Number of derivatives
+ 
+     A = np.diag(np.ones(m-1), k=1)
+     B = np.zeros((m, 1))
+     B[-1,0] = 1.0
+     Q = np.diag(np.ones(m))
+     R = np.diag([1.])
+ 
-     lqrc = LQRController("input", "output")
+     lqrc = LQRController("input", "output", A, B, Q, R)
      rospy.spin()
  
Improve LQR example for integrator_chains domain
## Code Before:
from __future__ import print_function

import roslib; roslib.load_manifest('dynamaestro')
import rospy
from dynamaestro.msg import VectorStamped


class LQRController(rospy.Subscriber):
    def __init__(self, intopic, outtopic):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)

    def read_state(self, vs):
        self.intopic.publish(VectorStamped(point=[-(vs.point[0] + 2.4142*vs.point[1] + 2.4142*vs.point[2])]))


if __name__ == "__main__":
    rospy.init_node("lqr", anonymous=True)
    lqrc = LQRController("input", "output")
    rospy.spin()

## Instruction:
Improve LQR example for integrator_chains domain
## Code After:
from __future__ import print_function

import roslib; roslib.load_manifest('dynamaestro')
import rospy
from dynamaestro.msg import VectorStamped

from control import lqr
import numpy as np


class StateFeedback(rospy.Subscriber):
    def __init__(self, intopic, outtopic, K=None):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
        self.K = K

    def read_state(self, vs):
        self.intopic.publish(VectorStamped(point=[-np.dot(self.K, np.asarray(vs.point))]))

class LQRController(StateFeedback):
    def __init__(self, intopic, outtopic,
                 A=None, B=None, Q=None, R=None):
        if A is None and B is None:
            A = np.array([[0., 1, 0],
                          [0,  0, 1],
                          [0,  0, 0]])
            B = np.array([[0.], [0], [1]])
        if Q is None and R is None:
            Q = np.diag([1.,1,1])
            R = np.diag([1.])
        K, S, E = lqr(A,B,Q,R)
        StateFeedback.__init__(self, intopic, outtopic, K)


if __name__ == "__main__":
    rospy.init_node("lqr", anonymous=True)

    n = 1  # Number of output dimensions
    m = 3  # Number of derivatives

    A = np.diag(np.ones(m-1), k=1)
    B = np.zeros((m, 1))
    B[-1,0] = 1.0
    Q = np.diag(np.ones(m))
    R = np.diag([1.])

    lqrc = LQRController("input", "output", A, B, Q, R)
    rospy.spin()
