Sunday, October 23, 2011

Pythagoras: The Drawing Delta Robot: Math and Design

I built this robot when I saw this delta robot on YouTube. The speed and precision amazed me, and the three arms working in coordination intrigued me.

Coincidentally, I have always wanted to make some sort of non-Cartesian plotter. So I thought, why not a drawing delta robot?


The Math

A note on convention: The "upper arms" of the robot are the sections of each of the three arms that are directly and rigidly attached to the motors. The "lower arms" are the sections of the three arms that are directly attached to the manipulator through ball joints.

First and foremost is the kinematics of the robot. I can control the upper arms of the robot, attached to the motors, but the lower arms are free moving on ball joints. The range of the lower arms makes nice spheres in 3-D space. With the three arms, any given motor angles will give exactly 1-2 possible manipulator positions (assuming the lower arms are .

There are the two directions of kinematics to solve for the robot:
  • Forward: Given the motor positions, where is the manipulator?
  • Inverse: Given a manipulator, what are the three motor positions needed to achieve that location?
In my robot, I only use inverse kinematics, but its good to understand the forward kinematics too.

Forward Kinematics

Since the angle of the upper arms are known, the endpoints of the upper arms are known through some trigonometry.

However, to put these coordinates in terms of the entire robot, instead of the motor, a few offsets need to be added. First is the motor's offset from the robot's origin. This changes the Y and Z terms. Second is the rotation of each motor.
We can calculate the rotation using a rotation matrix, which given an angle theta and an X and Y coordinate, it will give the rotated X and Y coordinate.
And so for each arm we can construct a sphere of every position the lower arm can be, centered between the ends of the lower arms where they meet the upper arms.

And by solving for the intersection of all three spheres. The intersection of two spheres will give you a circle in 3-D space, adding the third will give you 1-2 points in 3-D space. And then you always choose the lower point.
The problem I had with this is the offsets in the hand.
I never got it quite right. My best guess was to bring the three spheres closer equal to the size of the offset in the manipulator. However, since my algorithm didn't need forward kinematics, I never fully implemented it.

Inverse Kinematics

Inverse kinematics is a tad bit easier, solving for the intersection of circles, instead of spheres.

Each lower arm creates a sphere of possible locations, offset by a constant amount by the manipulator. Since the upper arms can only move change their Y and Z coordinates by rotating the arm, they have a fixed X. This reduces the sphere to a circle and the inverse kinematics problem to a circle-circle intersection rather than a circle-sphere (marked in green). 
However, this simplification comes at the cost of unreachable positions. Checks must be implemented to make sure that a given point has a solution before trying to find it.

Anyways, I found the intersection of the circles by taking the line made by the midpoints of the circles and mathing them, as described in this website.

The actual code snippet that does the inverse kinematics follows. The actual "meat" of computation is relatively small.

An important note

When I made the code, I decided to flip the coordinate axes so I could use positive Z for the manipulator position instead of negative Z. As it turns out, this reverses your image and makes visualizing the robot tricky. If I were to redo the code, I would use negative Z, to keep it consistent with real life.


Design Considerations

The robot's goal is to draw on US letter paper (8.5" x 11"). With this in mind, I guessed the length of the lower arms, simulating the arms in SymPy (symbolic math for Python) and then modeling them in SolidWorks. The only guideline I had was to keep the upper arms shorter than the lower arms for the best range and keep the two rods of each lower arm as far apart as possible for stability. I learned a lesson from that.

In version one of the robot, the upper arms were quite long, relative to the lower arm. This made for really great range in the Z dimension. For a drawing robot, this is useless. Since the arms were so long, small movements in the motors created large movements in the manipulator. Given the low precision of the servos, I got poor results in the accuracy of the drawing. I later cut the arms shorter with a hacksaw to get better results.

For the best range of motion for drawing, when the pen is centered on the paper, the upper arms should be near horizontal, relative to the robot, as pictured. 

Appendix

2 comments:

  1. Aaron - You were pretty close on the fwd kinematics. I think this will solve it exactly.

    I assume you are comfortable with vector addition in 3D.

    Let's work on solving the 3D point where motor1's downlinkage attaches to the end-effector. Call it LinkagePt1. You'll solve for LinkagePt2 and LinkagePt3 the same way, then solve for the end-effector centerpoint as the vector mean of these 3 Pts.

    We start by computing a 3D vector that goes from LinkagePt2 --> Linkage Pt1. Call it OffsetLinkage2_1. This is easy, since they are fixed points on the end-effector. You also need to compute OffsetLinkage3_1.

    The linkage arms are all length r. I assume you can compute cartesian coords {x y z ] for the linkage endpts of the 3 drivearms, based on the motor angles. Call these DriveArmTip1, DriveArmTip2, DriveArmTip3. You're intuition to use the intersection of 3 spheres is great.

    The accomodation you have to make is that, whatever sphere is carved out by the motor2's lower-linkage-tip (LinkagePt2), that sphere needs to be offset by vector OffsetLinkage2_1 when you go to intersect it with Sphere1. Why? because the point LinkagePt1 is the intersection of these 3 circles:

    LinkagePt1 = intersectionOf3Spheres (
    ( center = DriveArmTip1, radius = r ),
    (center DriveArmTip2 + OffsetLinkage2_1, radius = r ),
    (center DriveArmTip3 + OffsetLinkage3_1, radius = r ) )

    Same approach to compute LinkagePt2 and LinkagePt3.

    The end-effector center is then the vector mean
    (LinkagePt1 + LinkagePt2 + LinkagePt3 ) / 3

    ReplyDelete
  2. I hope to use stepper motor for the project then how I change the codes?

    ReplyDelete