Algorithm 3 Derivation / Explanation?

[jfrabosi]

Hi,

I've been working on modifying this repository to control four hexapod platforms at once, and I've been trying to understand how the inverse kinematics algorithms work. I've been looking at the source code but am struggling to find how Algorithm 3 is derived. Do you have any source documents that explain it? I've tried to derive it myself using the law of cosines and other trigonometry identities, but am struggling to get it to match your Algorithm 3.

double i1 = -ARM_LENGTH4 - ROD_LENGTH4 - a4 - b4 - c4
+ 2 * (ARM_LENGTH2 * (ROD_LENGTH2 + a2 - b2 + c2)
+ ROD_LENGTH2 * a2 + ROD_LENGTH2 * (b2 + c2)
- a2 * (b2 + c2) - b2 * c2);
if (i1 < 0)
{
movOK = -5;
break;
}
i1 = sqrt(i1);
i1 = (2 * ARM_LENGTH * c - i1) /
(ARM_LENGTH2 + 2 * ARM_LENGTH * a -
ROD_LENGTH2 + BP2);
i1 = 2 * atan(i1); // ~10 µs
new_servo_angles[sid].rad = i1;

Any help is greatly appreciated!

Jakob

[jfrabosi]

I figured it out. For anyone in the future who reads this, here's how Algorithm 3 works:

First, write out these two equations. They relate the servo arm BA and the rod arm AP, as defined in the stewart-platform-esp32/doc/hexapod-parameters.png image.

(x-a)^2 + b^2 + (z-c)^2 = L^2
x^2 + z^2 = R^2

where:
point B is located at (0, 0, 0)
A is located at (x, 0, z) (servo arm can only rotate in XZ plane)
P is located at (a, b, c)
the servo arm has length L
the rod arm has length R

Then, expand the first equation and substitute in the second equation for the x^2 + z^2 term to get our third equation.

We can draw a triangle with adjacent side x, opposite side z, hypotenuse L, and closest angle theta. We want to solve theta to determine what the angle of the servo should be. From this triangle, we get:

tan(theta) = x / z

We'll be working with the half angle tangent identities, so we'll create a variable t to help us out.

t = tan(theta/2) = x/2z

Use the following trig identities:

sin(theta) = 2t / (1+t^2)
cos(theta) = (1 - t^2) / (1 + t^2)

Then, substitute these identities for x and z:

x = Lcos(theta) = L * (1 - t^2) / (1 + t^2)
z = Lsin(theta) = L * 2t / (1+t^2)

Substitute these for x and z into the third equation from further above. Expand all of the terms and get it in the form below:

At^2 + Bt + c = 0

then use the quadratic formula, using the minus half of the plus/minus sign:

t = (-B - sqrt(B^2 - 4AC)) / 2A

Going back to the code now, the first line double i1 = ... is everything inside the square root term. So, for the first line,

i1 = B^2 - 4AC

if you expand the terms, you'll see that they all match up. Technically, you should expand all the terms first since the 2 in the denominator does some work inside the square root, canceling some 2s, so do that first. If this term is negative, there's no solution.

Finally, we put that i1 term into the quadratic formula equation after taking its square root, so the next i1 line becomes:

i1 = (-B - i1) / 2A

Then, since we said t = tan(theta/2), we can easily solve for theta.

Sorry for the long-winded explanation. Hopefully this helps someone in the future!

[NicHub]

Nice job, thank you for sharing.

Don’t hesitate to share also about your “four-hexapod-platform”. That will be awesome to see.

Note that I’ve updated the drawing as I replaced DraftSight with QCAD: a8fb838

[jfrabosi]

It's actually part of my Master's thesis - I've included a picture below. Thank you so much for making this project open-source - my project wouldn't be possible without your work! You're properly credited throughout the thesis and my code, of course.

[NicHub]

It looks so cool! Do you have a video of it? What's the subject of your Master's thesis?

[jfrabosi]

Thank you! Yes, here's one showing the full range-of-motion about the roll and pitch axes.
https://www.youtube.com/shorts/3j4FkxVK0gE

My Master's thesis subject is designing an acoustic positioning system for autonomous submarines. The windmill-looking attachment on the end effector is an acoustic array, with four ultrasonic microphones on it. I have a transmitter a set distance away, and I record an acoustic pulse on each of the receivers. Doing some signal processing and sensor fusion, I can get an estimate for where the acoustic array is relative to the transmitter. In the video below, you can see the audio signals shift in time when the hexapod changes position/orientation.
https://www.youtube.com/watch?v=QgJyHS0l2EE

The thesis is an above-water proof-of-concept. The hexapod bot is simulating the movements of a submarine. In the real version, the acoustic array would be fixed on a submarine and the transmitter would be on a buoy, projecting sound down towards the ocean floor / submarine. Above-water is easier to work with since nothing needs to be waterproofed and the speed of sound is 4.5x slower (however, sound propagates much less efficiently in air, so my range is quite limited!)

I'll be posting the code on my GitHub account soon - I'm almost done with writing the chapter about the hexapod positioner (which I call the Fo-SHIP, Four-Stacked Hexapod Isometric Platform, pronounced "faux-ship")

[NicHub]

Exciting project! The videos are amazing. I’m curious to see your code.

I once worked for a manufacturer of ultrasonic car detection systems for parking lots. I know that it can be difficult to filter unwanted reflections and other pesky sources of errors.

By the way, why did you choose a “Four-Stacked Hexapod Isometric Platform” design? Wouldn’t a simple off-the-shelf robotic arm have done the job?

[jfrabosi]

Thank you! I just finished the chapter on the hexapod, if you're curious I've attached it as a PDF.

2 Ground-Truth Positioning System.pdf

And here's the repository with the code

Thankfully, I'm only considering pretty head-on transmissions - noise from the environment and reflections can cause issues, but they tend to get averaged out over time. I also implement a Kalman filter to help de-noise the system.

I went with the Fo-SHIP because I thought it was neat! I'd only seen one other stacked hexapod platform before (from some NASA research) and thought it might be interesting to give it a shot. I proposed my thesis to my committee, and so the budget for the thesis is out of my own pocket - designing my own actuator kept the costs down significantly! Overall, I think it cost around $300 to build, and it has an average positional accuracy of 7.35mm (better for smaller angles, worse for larger). Would definitely have preferred a pre-made actuator to get better accuracy, but it wasn't in the cards for me. Plus, I wanted to make something unique.

Also: please let me know if I've represented you correctly in the chapter! Here's relevant excerpts:
Much of the inverse kinematics work for this mechanism were derived from previous
authors. Notably, a paper by an unknown author from the Wokingham U3A Math
Group details an inverse kinematics solution for a hexapod platform controlled by
six rotary servos [6]. An art installation named “memememe” improved upon this
solution and implemented the design into a single hexapod platform for controlling
the motion of a smartphone attached to the end effector [7]. Lastly, an open-source
implementation of a single hexapod platform using an ESP32 was posted online by
Nicolas Jeanmonod [8] [9]. This thesis has modified Jeanmonod’s implementation
to work with a stacked hexapod platform and some new features and functions
were added, but his work is the backbone of the hexapod platform controller. The
details of the inverse kinematics of the platform will be described later in Section
2.5.5, along with their code implementation.

[8] N. Jeanmonod, “Plateforme de stewart pilot´ee avec un esp32,”
https://ouilogique.com/plateforme-de-stewart-esp32/, 2019.
[9] ——, “Stewart platform on esp32,” https://github.com/NicHub/stewartplatform-esp32/tree/master, 2019.