Design a LQR controller for Cart Pole system in ROS/Gazebo
based on https://github.com/linZHank/invpend_experiment
To execute, go to catkin_ws, run source and catkin build
in one terminal, run roslaunch invpend_control load_invpend.launch
in a new one terminal, run rosrun rosrun invpend_control lqr_CartPole.py
mPole = 2 mCart = 20 g = 9.81 lPole = 0.5
A = np.matrix([[0, 1, 0, 0], [0, 0, (-12 * mPole * g) / ((12 * mCart) + mPole), 0], [0, 0, 0, 1], [0, 0, (12 * g * (mCart + mPole)) / (lPole * ((13 * mCart) + mPole)), 0] ])
B = np.matrix([0, 13 / ((13 * mCart) + mPole), 0, -12 / (lPole * ((13 * mCart) + mPole))]).T
Q = np.diag([1, 10, 1, 1]) R = np.diag([0.1])
K, S, E = control.lqr(A, B, Q, R)
