Using Python to Simulate Mechanical Things
I like to build things out of Legos, but it is a real uphill battle to actually be able to control any of the things I build using actual control systems such as PID controllers. I therefore decided I should start trying to build little programs to simulate as much as possible.
To assist me in this project, I enlisted Python because of its bindings to ODE and SDL, and also because it is interpreted and object oriented. These attributes all combine to form a language that is very good for doing light-weight physical simulation.
Update! Files You'll Need
I decided to start working with some of these files again much later after I originally wrote them (in April of 2009) and found that I'd since lost my development environment.
More distressing was the fact that Windows support for all these packages has been left behind the current versions. Python is at version 3.something now, ODE is four or five versions newer, by pyODE hasn't been touched in a year.
But there is nothing to worry about. I have archived a working set of tools you can install (and that I can install in the future) to use these example programs:
- Python binary for Windows (32bit):
(md5: b4bbaf5a24f7f0f5389706d768b4d210 )
- PyODE binary (including ODE itself) for Windows (32bit):
(md5: 47f17cfbc197e72f7baf8ace03aac1d7 )
- Pygame binary for Windows (32bit) for Python 2.5:
(md5: 0664898f458e72edd76ca763cdaa84e7 )
- PyOpenGL binary for Windows (32bit):
(md5: bbfd65e68fe3f8949a854eac6a8b9bb6 )
Simple Pendulum Balancing
I didn't really know how to use Python, so I started with the second pyODE tutorial. It simulates a two dimensional system which tries to spin two weights on the end of levers using a constant velocity. The page for the tutorial has a nice screen shot of whats going on.
My first change was to create a "bang-bang" controller to change the commanded velocity of the center lever to try and keep it pointed upright. I also added some code to report the angle (position) of the lever and the error in the position of the lever.
The second program replaces the "bang-bang" controller with a proportional conroller. You may notice that it works much better than the previous controller. It doesn't seem to seek the desired position as quickly, but once it gets "locked on," it can keep the position very steady.
I also added some code to report the output of the control system, the "command velocity," and rounded all the displayed digits to three decimals to make them somehwat easier to read.
It is also interesting to report that the proportional controller only needed a maximum force of 20 units to achieve a steady state, while the bang-bang controller needed a maximum force of 22 units. The bang-bang controller could probably use less than 22 units of force if it were allowed to command a larger velocity, but that would lengthen the settling time considerably.
Here is the sourcecode for the proportional controler demo.
Finally, I worte a full PID controller, with limits to lessen integral windup. Rick Bickle's motor control presentation was very helpful, from a coding standpoint. I found that tuning the P, I, and D constants was rather challenging. For previous controllers I'd worked with, the D term was to be rather small, since it amplified noise. The I term was usually fairly large, since it was used to prevent changes. But in this controller, I had to set the I term to zero, and have a rather large D term! I don't pretend to be a PID guru, but I was somewhat surprised.
Part of the problem was how the system behaves. Due to the second lever swinging around freely, it was hard to do the traditional tuning method. The method I used in the past was to set all terms to zero. Then increase the P term until the system oscillates. Then increase the I term until the oscillation stops. Finally, increase the D term to get the transient response you desire.
I think this system my be described as "chaotic" (in the mathematical sense) or "non-linear". I will have to research a little more about the terms to say for sure.
Here is the sourceode for the full PID controller, with values that bring the pendulum to a balance. Feel free to twiddle around with the P, I, and D terms and the I term limits to see if you can get a better response!
Balancing Ball Robot
The next robot I wanted to simulate was a balancing robot, similar to David Anderson's nBot, but using a ball instead of wheels so that it had to balance on two axes.
I started with the third tutorial for pyODE. A lot of my code is from this example, but I typed it in by hand so as to force myself to understand each part of the code. I would recommend this to anyone using tutorials to learn a new language.
I added some code to render balls as well as blocks, and set up an ODE universal joint between a sphere (which would be the wheel) and a box (which would be the counterweight and object I wish to balance vertically).
Much of this part of the project was figuring out how ODE joints work and tweeking the physical parameters of the simulation. No control systems have been added yet, but it should end up being similar to the PID code of the pendulum balancer.
Here is a copy of the source code. Note that it uses pyOpenGL and GLUT, instead of pygame. This is somewhat of a pain to setup, especially on Windows. If I could find an easier way (installation-wise) for doing 3D work, I would probably switch.
Single Axis PID
It took me a few days to figure out a way to determine the tilt of the stick in such a way that I could derive control information for the ball. The problem was that most methods of representing angles are ambiguous as to whether the tilt error was negative or positive. The solution was to use a Euler axis/angle pair derived from the ODE rotation matrix of the stick.
With that out of the way, I put in a single PID controller on one axis of the sphere, which would balance the machine to a degree. Once perterbed, or given enough time for errors to accumulate, it would gyrate wildly around trying to control a two-axis system with only one axis of control. It almost seems like it could balance given tuning of the parameters.
The source code at this stage is full of four days of munging around with different rotational representations and is a real mess. But its worth a download to watch the machine swing around like an idiot for a while. This simulation is different than all the previous simulations in that the starting position of the second stick is slightly randomized. This means that each run of the simulation is different.
Dual Axis PID
After another period of geometry wrangling, I deduced the correct combination of orthonormal basis and dihedral angles to control the second axis of the rolling ball.
This is all fine and good, except that now I have discovered that I need to control a third parameter: the "spinning" of the machine. Right now, I can keep the machine oriented upwards, but in doing so, it will gradually accumulate a rotational inertia around it's axis.
The problem is that I have no more control variables to exercise. Imagine you had a ball that you and a friend were balancing on. You are facing right angles and can only run forwards or backwards. How would you turn the ball so that one of you were facing north or any other compass direction?
The short answer is that you can't directly control it. Neither you nor your friend can make the ball spin in place, since both of you are exerting forces which line up on the axis you want to spin around. You could spin, if you got clever, by allowing yourselves to lean a certain direction, and then roll the ball so that you "swung" the direction you wanted to the ball to spin.
This would actually happen in the normal course of trying to keep the ball balanced, so you would gradually accumulate some spin anyways. This is what is happening in my simulation now. A gradual, uncontrolled spin develops and eventually makes the whole thing runs away.
In the third version of the simulation, you can see this happening. I've added some debug marks to show the different axis of the system. I've also added another block for it to run into. I've not done too much in the way of PID constant tuning; in fact, I am using a simple P controller.
The problem I am now faced with is this: I have two controllable parameters, but three parameters to control.
I fiddled around some more with the code and added walls and a grid. This doesn't really change much. The machine will bounce off the walls. It makes screenshots a lot nicer. :) I also made the camera always point at the center of the ball. Here is a copy with these changes.
The last thing I tried on this system was adding a block which spun providing torque against the gradual accumulation of spin on the ball. The block sat on top of the first block like the top of the letter T. It had its own PID controller for how much it spun.
Suffice to say, throwing yet another body into this system, with its own PID controller, didn't do much for stability.