This project began as an idea to do biomimicry animatronics with a robotics elective class. I tasked the class with coming up with a piece of animal anatomy which we could mimic using servos and pneumatics. Here is a link to the powerpoint I used with them.
The students came up with the idea of an octopus arm, which obviously has a huge number of degrees of freedom. I had some suction cups leftover from another project and so we built a robotic arm with a suction cup end effector. I did the CAD design given time restrictions for the class. CAD files are here.Students did the mechanical assembly, pneumatics, and electronics wiring, as well as initial testing of motion controlled by the VSA program. The project content can be found in this powerpoint. The actual arm has a pneumatic cylinder and only three servos on the top (one less than in the original CAD design).
The brains of this operation is the Lynxmotion SSC-32U USB Servo Controller Board which has the ability to drive 32 servos or relays, has digital/analog IO, and USB, Serial (UART), and Xbee support. We wired up the three servos to pins 0 through 3 and three relays to pins 4 - 6. Pins 4 and 5 control the double acting solenoid for the pneumatic cylinder. Pin 6 controls a solenoid which turns on or off the suction going to the end effector.
The solenoids are all driven by relays, so I designed a simple interface between the solenoids and relays. The relays are protected by diodes across the solenoid contacts.
After the school year had ended, I decided that I wanted to go further with this arm. It seemed like an ideal platform to try out some inverse kinematics, which is a topic that I've read about in detail and even setup a project for a student, but had never done myself. This link goes to a project which I setup for a student using an Arduino and a Lego 2DOF arm along with a NXTMMX and a couple Arduino Libraries. See this link for more details on those libraries. I have also uploaded a folder with the Arduino libraries and the student's inverse kinematics code... untested.
Anyhow, for this project I wanted to use Python because doing the inverse kinematics calculations with an Atmega328 just doesn't make sense for a stationary arm which could easily be connected to a computer. I had been too intimidated to use Python for serial connections, but it turns out to be easy with PySerial. I had to install it but there are good directions online.
Here's a sample program which was able to control the SSC-32U via USB. In Windows, you just need to know the COM port. The format for serial data sent to the SSC-32U is clearly described on pg 24 in the documentation linked above... essentially you send (in ASCII form) the pin number, the pulse width in microseconds, and optional extras such as timing of the move. The message must end with a carriage return (\r in Python). See this post for a discussion of b' ' which was necessary for me to get this to work. I am using Python 3.5.2.
With communications working, the next task was to quantify the geometry of the arm. Kinematics involves knowing the geometry of your arm so that you can predict where the end effector will be located if you know the angles of all the rotational joints (for this analysis I left out the pneumatic cylinder, locking it into one position). It involves a bunch of geometry to relate several different coordinate systems to each other and ultimately to an absolute coordinate system. Inverse kinematics involves figuring out the angles which will get your end-effector to a particular location in space. Both kinematics and inverse kinematics involve the same equations, but in the former case you calculate position (x, y, z) as a function of angles (θ0, θ1, θ2), while in the latter case you calculate the angles as a function of the position.
I defined the absolute origin at the corner of the block of wood on which everything was mounted and then determined the fixed and variable geometries. I defined angles θ0, θ1, θ2 for servos 0, 1 and 2, each relative to the next servo. So θ0 is the position of servo 0 relative to servo 1. θ2 relates the second servo to the absolute coordinate system. By daisy chaining coordinates of the end effector first relative to the angle of the nearest servo, and working backwards, I wrote equations for the end effector location relative to the absolute origin.
Next I needed to relate the abstract geometries to some real world distances, and to calibrate the servo pulses to angles. I measured distances which are displayed in the picture at right.
I setup a camera and recorded each servo moving through the range of angles corresponding to pulses of 800 to 2200 microseconds. I used pictures of the servos in the starting and ending positions to measure the real-world angles, and determined linear functions relating angles (θ0, θ1, θ2 ) to pulses (p0, p1, p2 ).
With all the geometry taken care of, it was time for coding. The coding of the kinematics was quite simple. At the beginning of the program, you specify the three angles for the servos, and then the program calculates x, y, and z for the end effector, and sends off the appropriate pulses to the SSC-32U. I included a check to make sure that the angles were physically possible prior to sending the serial commands out.
I learned a couple tricks here: one is that displaying and formatting values in Python 3.x is different from Python 2.x. The syntax below works... you embed the variable names and the desired appearance within the print statement, and then use .format(**locals()) so that it knows what the variable names refer to.
Second, when sending the serial commands to the SSC-32U, I needed a way to include variables as part of the ASCII being sent out, because the pulse widths were no longer constants. The syntax below works; we create a string which includes text and pulse width variables cast as strings. Then the .encode('utf-8') does the equivalent of b' ' to turn the string into an array of bytes.
The kinematics were tested by comparing the calculated position with the actual position measured in the real world using a jig (see below). In general, we can see that the calculated and actual positions are close (see the column "Differences" to see how far off they were from each other). There are several likely causes of the differences which include the aforementioned tilt, play in the mechanical systems, and uncertainty in measurements.The inverse kinematics was a little bit harder, mostly because while I had functions for (x, y, z) = f(θ0, θ1, θ2 ), the inverse functions (θ0, θ1, θ2) = f(x,y,z) are not possible to solve for in a closed format (see this article for why some functions do not have closed form inverses). However, with a computer, a closed form solution is really not necessary; we can numerically determine the angle values which lead to a desired end effector position. I briefly looked into fsolve as a way to solve a system of nonlinear equations (search for something like "systems of nonlinear equations python fsolve") but then decided that the linear regression algorithm that I had worked on to fit 2D linear data could easily be extended for use here.
The inverse kinematics code can be found here. One trick that was necessary for this program was to recurrently call methods which would update the radian theta values and update the predicted end effector location (x,y,z). Unlike the kinematics sketch, where calculations are only done once and you can get away with defining everything at the beginning, here you need to be recalculate these values during each iteration as you have changed one of the angle values.
The "cost" function which is being minimized here is just the sum of the squared differences between the components of the desired end effector position denoted by (x₀, y₀, z₀) in the code, and the predicted end effector position (x, y, z) calculated from the kinematic equations for the current theta values.
The video at the beginning of this sketch shows a demonstration of the two pieces of code in action. I created a "jig" to allow me to semi-accurately measure the coordinates of the end effector in space. The jig is made from Rev extrusion with a linear motion kit. The desired coordinates are put into the inverse kinematics sketch which outputs the possible servo angles. Those angles are then put into the kinematics sketch which calculates the corresponding pulses and sends them to the SSC-32U.
 |
| Jig to find x,y,z coordinates of end effector. |
 |
| Suction mechanism. |
 |
| Pneumatic cylinder. |
For me, θ0 and θ1 define a coordinate plane similar to that shown in Figure 16-4, and so I think there are two possible (θ0,θ1) pairs for most end effector positions. There is only one possible value for θ2 because this angle corresponds to a rotation of the whole arm; the axis of rotation for θ2 is orthogonal to the other two axes.
The bullets below shows several other redundant solutions. The following picture illustrates that the same basic end effector position is calculated using the kinematics equations for each set of angles.
- (θ0, θ1, θ2) = (30,30,45) and (θ0, θ1, θ2) = (17,22,44) for (x, y, z) = (0.6, 12.9, -1.2)
- (θ0, θ1, θ2) = (-30,30,-45) and (θ0, θ1, θ2) = (33,66,-44) for (x, y, z) = (-1.1, 16.8, 8.1)
 |
| Redundant angles: each of the two boxed angle sets leads to approximately the same end effector location (underlined). |
