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):
    def __init__(self, intopic, outtopic):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
        A = np.array([[0., 1, 0], [0,0,1], [0,0,0]])
        B = np.array([[0.],[0],[1]])
        Q = np.diag([1.,1,1])
        R = np.diag([1.])
        K, S, E = lqr(A,B,Q,R)
        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])]))


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


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()

Remove some unused code that unnecessarily introduced depends
Remove some unused code that unnecessarily introduced depends

This should be regarded as an update to 9aa5b02e12a2287642a285541c43790a10d6444f
that removes unnecessary dependencies for the lqr.py example.

In the example provided by 9aa5b02e12a2287642a285541c43790a10d6444f
Python code was introduced that led to dependencies on NumPy and
the Python Control System Library (control), yet the state-feedback
gains were hard-coded. We will re-introduce these dependencies in the
next changeset, but having this checkpoint without them seems useful.

  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):
      def __init__(self, intopic, outtopic):
          rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
          self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
-         A = np.array([[0., 1, 0], [0,0,1], [0,0,0]])
-         B = np.array([[0.],[0],[1]])
-         Q = np.diag([1.,1,1])
-         R = np.diag([1.])
-         K, S, E = lqr(A,B,Q,R)
-         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])]))
  
  
  if __name__ == "__main__":
      rospy.init_node("lqr", anonymous=True)
      lqrc = LQRController("input", "output")
      rospy.spin()
  
Remove some unused code that unnecessarily introduced depends
## Code Before:
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):
    def __init__(self, intopic, outtopic):
        rospy.Subscriber.__init__(self, outtopic, VectorStamped, self.read_state)
        self.intopic = rospy.Publisher(intopic, VectorStamped, queue_size=1)
        A = np.array([[0., 1, 0], [0,0,1], [0,0,0]])
        B = np.array([[0.],[0],[1]])
        Q = np.diag([1.,1,1])
        R = np.diag([1.])
        K, S, E = lqr(A,B,Q,R)
        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])]))


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

## Instruction:
Remove some unused code that unnecessarily introduced depends
## Code After:
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()
