Wednesday, June 28, 2017

Inverse Kinematics 3DOF Octopus Arm

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, θ) to pulses (p0, p1, p).

In this process, I thought a lot about precision and cost benefit of increased precision.  For example, it was clear that there was a tilt of the arm when viewed from the side.  This was about 2⁰ off the vertical.  I ended up ignoring this tilt because the geometry would have been much more complicated and the cost greatly outweighed the benefit of higher precision.

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, θ), 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.
Finally, a note about inverse kinematics.  To test the inverse kinematics, I entered the end effector positions which I had calculated for known values of the three angles into the inverse kinematics program.  I expected that it would return the three angles which were used to set the end effector position.  And yet, it didn't.  For example, (θ0, θ1, θ2) = (0,0,0) led to a calculated (x, y, z) = (3.3, 11, 4.3).  When I put (x, y, z) = (3.3, 11, 4.3) into the inverse kinematics program, it spit out (θ0, θ1, θ2) = (28,16,1) which is different from (0,0,0).  In general, there are multiple solutions which are possible to get an end effector into a particular location in space.  See the image below from "NXT Power Programming" by John Hansen.

For me, θ0 and θdefine a coordinate plane similar to that shown in Figure 16-4, and so I think there are two possible (θ01) 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).

Linear Regression for Least Squares Fit of 2D Data

This post describes a script which finds the best fit line for a data set.  My work was motivated by a desire to learn a bit more about artificial intelligence algorithms, and confusion about why the average of the slopes (y/x) of a dataset (when the x and y values are proportional) doesn't agree with the slope of a best fit line (with the y-intercept set to zero).

Artificial intelligence is a vast subject.  A subset of this subject relates to tasks that computers can be taught to complete.  Since computers operate much faster than humans, it is easy to give them simple tasks and have them iterate repeatedly.  This article from an online Stanford Computer Science course describes "supervised learning", where computers use a training dataset to "learn".  A very simple example is a best fit line; the computer determines a rule (y=mx + b) which predicts the output value (y) for a given input value (x) based on a training dataset of (x,y) data points.  This rule has two unknowns (m = slope and b = y-intercept) which can be independently varied to give the best fit of the training dataset.  Spreadsheet programs like Excel can fit data based on a wide variety of rule categories (linear, polynomial, exponential, etc).

A related and interesting observation relates to a spreadsheet analysis of a dataset wherein x and y should vary linearly.  For example, the heat released by salts when they dissolve in water follows a linear relationship with the amount of salt used.  A material property, the enthalpy of solution, ΔHsoln, is equal to the ratio of heat to mass (ΔHsoln=q/M) and is constant for a given temperature and pressure.  This means that a plot of heat (q) versus mass (M) is linear with a slope equal to the enthalpy of solution (q = ΔHsoln  M is in the form y = m x).  You can also determine the enthalpy by calculating q/M for each trial of a dataset and then averaging the results.  The picture below shows the results of a spreadsheet analysis for ammonium chloride dissolved in water.

What is interesting to me, is that the enthalpy as determined by the slope of the best fit line with zero intercept (283 J/g) is not the same as the enthalpy as determined by averaging the values of q/m for each trial (277 J/g).  To understand why they are not the same, I decided to learn a bit more about how Excel fits linear data.

Fitting of linear data can be done with a linear regression.  This basically means that we guess a linear function (y = mx + b) which could fit the dataset.  Then we change either b or m and see if the new function is a better or a worse fit.  If the fit is better, we "keep going" in the direction we picked.  For example, if we were making the slope bigger, then we keep increasing the slope.  If we made the fit worse, we want to change the parameter in the other direction.

As described, this is a "poor man's" version of a gradient descent protocol.  The difference is that in a gradient descent, we are not so much randomly changing and checking our parameters, but are looking at a mathematical function cost = f (m, b) and determining the direction (specified by a vector in the m, b plane) which leads to the largest decrease in cost.  We then change m and b concurrently so as to move in the direction of maximum decrease.  In multivariable calculus, the gradient is analogous to the derivative in 2D calculus.  Hence gradient descent is analogous to minimization problems done in introductory calculus classes.

From my point of view, doing a real gradient descent is not necessary as the computing power available from a standard laptop allows me to fit the data within seconds using a more rudimentary algorithm.

The basic algorithm is shown in the flowchart at right.  Code can be found online on github.

At the heart of the algorithm is a cost function which is the sum of the squares of the differences between the predicted y value (mxi + b) and the actual y value (yi) for each x value.  Suppose that your training dataset is depicted by a series of i data points (xi, yi).  Then

In python, this calculation is easily done by reading in the dataset

and then calculating the cost

assuming that your data is saved in a file called data.csv located in the same place as the python program, and that the data file contains x and y values separated by a comma with a different point on each line.

The rest of the "meat" of the program is in incrementing/decrementing m or b which is done by the following:

and in toggling whether we are incrementing/decrementing m and b, which is done by the following:
There are several factors in the program which determine how quickly the best fit line converges.  The first is how close the initial guess is.  The second is how much the slope or y-intercept changes each time through the iteration.  These latter "step sizes" are stored in the variables called "deltaSlope" and "deltaIntercept".

As a test, I fed in the enthalpy data discussed above into both Excel and this python program.  I chose an initial guess of m = 250, b = 0 and a step size of 1 for both slope and intercept.  The python program "converged" to m ~ 285 while b ranged between 0 and -20.

The Excel fit of the data gives m = 285 and b = -12, so I'm definitely in the right ballpark.

Obviously this a pretty rudimentary program.  It doesn't assess how close it's getting and it doesn't adjust the step sizes at all, so we don't see convergence.  Rather we see fluctuations around the actual values which would minimize the cost function.  More robust algorithms could adjust the values of "deltaSlope" and "deltaIntercept" perhaps in proportion to the relative value of the cost function.  Alternately, the program could just stop changing slope or intercept once changes lead to minimal cost change.  These are all tweaks that I'm sure have been thoroughly exploited in calculators and spreadsheet program. 

So back to the original motivating question... why is the slope of a best fit line not the same as the average of the y/x ratios for all the data points?  The best fit line uses a least squares minimization, so it's a nonlinear approach as opposed to a simple average.  So the results won't be the same.

Thursday, June 8, 2017

Playing with the Adafruit Huzzah

The Internet of Things (IoT) involves hooking up everyday objects via wi-fi (or cellular) so that they can send or receive information.   This technology allows items to be "smart": to respond to remote commands or to update a user on status regardless of proximity.  Data from multiple IoT items can be aggregated to give the status of buildings or a region.

The Adafruit Huzzah is one of several products which use the ESP8266 wifi chip.  The Huzzah is "packaged" nicely with lots of support and a lower barrier of entry than the 8266 chip by itself.

I played with the Huzzah to create a proof of concept in order to understand its capabilities and ease of use.  It's pretty simple.  Much of the hard work is hidden in layers of various websites such as Adafruit IO and IFTTT.

WiFi Test

For a simple test, I first hooked up the Huzzah and ran a test program which gets you onto the Internet.   The program sends a GET request to the host

GET /testwifi/index.html HTTP/1.1\r\nHost:\r\nConnection: close\r\n\r\n

If that GET request works, you will see the contents of that webpage written to the serial monitor.  

This sketch also has two levels of diagnostics.  First, while the wifi connection is being made, you will see “.”  printed twice a second until a connection is made.  Afterwards, it will show you your IP address.  This diagnostic tells you if you have successfully chatted with the router whose SSID you provided. 

The second diagnostic level tells you whether you can make a TCP connection.  The Huzzah attempts to connect as a client to the host  If it successfully connects, it send the previously mentioned GET request, parses and displays the information sent back from the server.  Otherwise, it tells us that the "connection failed".   With one router, the test worked fine.  With a second, it failed, producing a mixture of gibberish and HEX information from the “stack”.

Adafruit IO

Next I played with Adafruit IO.  This gives the ability to set up feeds and dashboards which access the info in those feeds.   Adafruit IO runs using the MQTT protocol, and they have written nice libraries for the Huzzah.  I setup four testing feeds shown in the image below:

Each of these can be linked to a block on the dashboard as shown below:
Finally, we setup a dashboard as shown below.  I used a slider for the PWM feed, toggle switches for the LED and Huzzah Status (the former sending data and the latter receiving data from the Huzzah), and a textbox for the status of the switch (which is pulled HIGH on the Huzzah).  The dashboard can be accessed from anyplace which has Internet access and is the user's interface to the Huzzah.  I tested from a laptop, an old iTouch using wifi, and an Android device using cellular service.

In the code on the Huzzah, we either subscribe to feeds (test-LED and test-PWM) or publish to feeds (test-switch) depending on whether we are reading and acting on information from the user (subscribe) or sending information to the user (publish).  So if we want to turn on an appliance, turn off an alarm, or setup the sprinklers, we subscribe to an associated feed.  If we want to display information from a sensor, we publish to a feed.  The basic syntax is shown in the picture below:

In the setup function, we subscribe to to changes in the feeds that we have aliased above:

Then we see if a message has come on our subscription, determine whether its from the test-LED or test-PWM feed, parse the message to convert it to a number, and then act accordingly.  For the test-LED feed, this means turning on or off an LED.  For the test-PWM feed, this means analogWrite - ing to an LED to vary its brightness.

To publish our data, we use the .publish method for the class Adafruit_MQTT_Publish as shown below.
I played around with using a "will", which basically sends a final command if the connection terminates for some reason, but did not get consistent results with this protocol.

The following video shows the use of a laptop and an iTouch to control two LEDs.  The blue LED on the Huzzah is controlled by the test-LED feed and is either on or off.  The red LED is controlled by the slider linked to the test-PWM feed, which adjusts the LED brightness using PWM.  The Green wire shorts one of the Huzzah's inputs to ground which shows up in the feed test-switch and is displayed as a 1 or 0 under the heading Switch ON?  The switch updated consistently on my laptop and Android, but not on the iTouch.

Huzzah and IFTTT

The last experiment I did was to link up the Huzzah to a Twitter account to have it control some change in the physical world when a new tweet was received.  The connections are shown in the diagram below:

Everytime that something happens with the RedShiftFTC account, the Adafruit IO feed RedShiftTweeted is updated by IFTTT.  The Huzzah is monitoring this feed, and whenever the feed changes, the Huzzah just increments a variable called tweetsReceived.   I hooked up a LED matrix (LinkSprite LED Matrix kit using the Max7219 chip) and programmed a heart and a check symbol for the LED matrix.  The Huzzah causes the LED matrix to display the heart and check in succession, changing for the number of times that corresponds to the number of tweets received.  Full code is here.

Of minor interest (mostly for my records): The Huzzah runs at 3v3 while the LED matrix runs at 5V.  So I needed a level shifter between the two.  I originally worked with the 74LVC245 level shifter but found that it did not work reliably.  An oscilloscope trace showed that one of the outputs was behaving erratically.

I switched to the Sparkfun BOB-12009 level shift and got reliable results.