ZIPY : ZIPY Inverted Pendulum Yakshave

Wrote some blog posts about trying to use model predictive control for the cart pole. Got it working ok-ish in simulation in OpenAI Gym.

http://www.philipzucker.com/model-predictive-control-of-cartpole-in-openai-gym-using-osqp/

http://www.philipzucker.com/osqp-sparsegrad-fast-model-predictive-control-python-inverted-pendulum/

We've been spending a bit of time cleaning up the code in the git repository, refactoring common elements of different pieces into parent classes and generally trying to unify the interfaces between stuff. Going forward, we'd like to be able to mix and match the sensor sources and controllers, so hopefully this effort pays off.

We had success balancing the pole using entirely the new vision system as an angle measurement! In general, the swing-up from down position seems to have gotten finicky using both the rotary encoder and the vision system. An interesting next step is to see if we can balance a double inverted pendulum, using the rotary encoder for the first pole and the camera for the second.

I've gotten a couple posts on my blog about a project to use PyTorch (an automatic differentiation and machine learning library for python) for model predictive control. In model predictive control, you solve a trajectory optimization problem for every new command you give. In trajectory optimization, you set up the control problem as a big ole optimization problem.

PyTorch is useful because it is fast, and it gets us the gradients and hessians we need with minimal fuss.

Although not entirely tested, this approach looks promising so far. The trajectories and forces look reasonable and the iteration speed seems acceptable.

Here's the some of the latest blog posts about this :

http://www.philipzucker.com/pytorch-trajectory-optimization-part-5-cleaner-code-50hz/

http://www.philipzucker.com/pytorch-trajectory-optimization-3-plugging-hessian/

http://www.philipzucker.com/pytorch-trajectory-optimization-part-2-work-progress/

An update on our inverted pendulum project. The rotary encoder we used are great. They're accurate, easy, and we can read them quickly. We'd like to make a double pendulum, though. That would mean another encoder for the joint, but we don't know an easy way to make that work. We thought a vision system could free us from the wires necessary for the encoders. Phil wrote some notes on his blog.

Early on in this project, we used a webcam to try to track the colorful stickers attached to pole. It worked, but it was finicky. We've found a far better way. We bought some retroreflective tape and an IR light source source. Then, we ripped apart our PS Eye camera and removed the IR filter inside. We had to file down the lens holder base to bring the focus back to the right range. 

With the IR filter removed, the camera can see the illuminated tape very easily.

We used some bash commands called from our Python script to turn down the exposure, gain, etc.

command = "v4l2-ctl -d 1 -c white_balance_automatic=0 -c auto_exposure=1 -c gain_automatic=0"
output = subprocess.call(command, shell=True)
command = "v4l2-ctl -d 1 -c exposure=0 -c gain=0"
output = subprocess.call(command, shell=True)

The result was a nearly binary image. As you can see in the video below, it's basically perfect. We used some simple OpenCV code to threshold, make contours, and fit a red rectangle around the contours. 

ret, frame = cap.read()
imgray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, thresh = cv2.threshold(imgray, 0, 255, cv2.THRESH_BINARY+cv2.THRESH_OTSU)
im2, contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE , cv2.CHAIN_APPROX_SIMPLE)

rect = cv2.minAreaRect(contours[0])
box = cv2.boxPoints(rect)
box = np.int0(box)

cv2.drawContours(thresh,[box],0,(0,0,255),2)

Here's a link to the Git commit.

I wrote an overview of the project on my blog:

http://blog.benwiener.com/programming/2018/05/10/cart-pole.html

Here is a description of some of the math of the control

http://www.philipzucker.com/cartpole-maths/

And some more posts during the creation.

http://www.philipzucker.com/cartpole-workin-boyeee/

http://www.philipzucker.com/cart-pole-trajectory-optimization-using-cvxpy/

http://www.philipzucker.com/cart-pole-using-lyapunov-lqr-control-openai-gym/

Here is an attempt at creating an Iterative LQR controller. Quite Slow, questionably working, but interesting.

https://philzook58.github.io/reinforcement/learning/control/2017/12/07/LQR.html

MIT has a great course taught by Russ Tedrake (Lecture videos available online). This is where I learned about the control that we ended up using

http://underactuated.mit.edu/underactuated.html?chapter=acrobot

After many trials and tribulations, the goddamn thing works.