 # Inverse Kinematics of a 2 DOF Planar Manipulator

#1

Hi, I’m Charmaine and am currently taking Dan and Su’s fundamental’s class. My partner and I are having trouble with a processing sketch. Please refer to the link.

We’re trying to generate a triangle, from an origin and known coordinates (`mousePressed`), that keeps two fixed lengths constant (Left and Bottom side).

Questions:

1. How do we get the left side to stay constant?
2. Why is `println(degrees(aA));` displaying incorrect numbers?

The goal is to have the triangle to work so that we can calculate the (Left and Bottom angle), which will be sent to the arduino to control servo motors.

``````float a, b, c;
float aA, aB, aC;
float cosA, cosB, cosC;
PVector va, vb, vc;
float angle;

float tempA, tempO;
void setup() {
size(900, 600);

va = new PVector(-450, 0);
vb = new PVector(150, -10);
vc = new PVector(0, 0);
a = 250;
b = 250;

println(degrees(angle));
}

void draw() {
background(0);
translate(width/2, height);
if (mousePressed == true) {
vb.set(mouseX-width/2, mouseY-height, 0);
angle = degrees(PVector.angleBetween(va, vb));

tempO = abs(sin(angle-aA))*a;
tempA = abs(cos(angle-aA))*a;
vc.set(-1*tempA, -1 *tempO, 0);

stroke(255);
line(va.x+450, va.y, vb.x, vb.y);

c = dist(va.x+450, va.y, vb.x, vb.y);// now we know the length of all sides

cosA = (b*b+c*c -a*a)/2/b/c;

aA = acos(cosA);
aB = aA;
aB = 180 - aA*2;

println(degrees(aA));// angle for servo #1
}
triangle(va.x+450, va.y, vb.x, vb.y, vc.x, vc.y);
}

``````

Please let me know if I am being unclear. Thank you!

Processing Help
#3

Hey @seedeesee great to see you on the forum!

So to clarify the relationships you want to enforce:

`length of A` `length of B`

∴ what `angle AB` and `angle AC` and `angle BC` allow such equivalency to occur? #4

I took the liberty of changing your post’s name.

If I understand your question correctly you’ll find two solutions in sections 1 and 2 of this chapter.

http://www.eng.utah.edu/~cs5310/chapter5.pdf

Basically it is a Planar Inverse Kinematics problem – very common in robotics when a given X, Y position is desired, but the robot can only manipulate it’s “shoulder” and “elbow” (i.e. it has no concept of X and Y only … “All I know how to do is control the angle theta 1 of my shoulder and angle theta 2 of my elbow”).

I did write a quick implementation of some similar solutions recently (I am building some 2+ DOF planar writing robots myself) … so if you get really stuck I’ll be happy to share it with you … but you’re really close … and I don’t want to steal your mathematical victory Keep us posted!

@dander4

#5

By the way, just to add a note – all (I believe …) functions that take angles in Processing take radians for angles, not degrees. So, in this case `PVector.angleBetween()` is returning radians, but you are converting it to degrees with `degrees()` and then passing it into `sin()` and `cos()` in its converted (degree) form, while `aA` is stored in radian form (which is what `acos()` etc returns).

#6

Hi, I am Charmaine’s partner and I am currently reading the chapters that you linked to us and trying to understand the math. I am learning the first section where you find theta2 and I put in some numbers to find theta2 but I ended up getting a negative inside of the square root and you can’t take the square root of a negative number. So I"m confused how to implement this type of equation into our code if I know that at some points it’s going to result in a negative number inside of the square root.

#7

Hey there – for what values are you getting negative numbers?

A section of my math looks like this:

`````` // numerator and denominator are broken up
// here to make it a little easier to work with
float eq53_num = (a1 + a2) * (a1 + a2) - (hand.x * hand.x + hand.y * hand.y);
float eq53_den = (hand.x * hand.x + hand.y * hand.y) - (a1 - a2) * (a1 - a2);

theta2 = 2 * atan(sqrt(eq53_num / eq53_den));
// (eq 5.3) if 2, right handed solution, if -2, left handed solution

float phi = atan2(hand.y, hand.x); // (eq.  5.5)
float psi = atan2(a2 * sin(theta2), a1 + a2 * cos(theta2)); // (eq 5.6)

theta1 = phi - psi; // (eq 5.4)
``````

There will be some values of hand position for which there is no solution. On a practical level these are all values that it is physically impossible for the hand to reach (because of the fixed arm lengths). In those cases, you will get invalid inverse solutions.

If you look at the end of the chapter (figure 5.11) you can see how to calculate a valid workspace for the arm. Programmatically, you might simply reject any “hand x/y” values that are not “inside” the workspace, because the arm “can’t go there”.

Keep me posted! I’m excited to se you solution #8

Okay thanks, good to know! I guess one major part I’m stuck on is which part of the code isn’t working. I can try to figure out the math on paper, but I’m having a hard time converting the language into Processing. Once I am able to translate it, I don’t know where in the code it’s not working because I don’t understand a few of the lines in code and how they work together as a whole. Do you mind just telling me which part is keeping me from getting the triangle to do what we want?

#9

`angle = degrees(PVector.angleBetween(va, vb));`

is problematic because you are feeding that angle (still in degrees) into sin/cos below – (see the comment above – I mentioned it before). If you get rid of degrees here and use the radians value, it definitely helps for some values of X/Y. One problem is that you are currently allowing calculations to be done outside of a “valid” workspace (as mentioned the post above). For x/y values you will definitely get undefined (and visually strange) results.

… but since I don’t have have access to your full on paper mathematical derivation, it’s a bit difficult to comment on a good solution past that. There is a very thorough derivation in the paper, which is what I used to make mine work. The core math is copied and pasted from my code, so I know it works for finding theta1 and theta2, given valid x / y.

#10

For these equations:
(in 5.1.1)
x = a1cθ1 + a2c(θ1 + θ2)
y = a1sθ1 + a2s(θ1 + θ2)

what do c and s stand for? At first I assumed cos and sin but then I realized that they would just write cos and sin if that’s what is supposed to be used in the math.

#11

In chapter 2 (http://www.eng.utah.edu/~cs5310/chapter2.pdf, near eq 2.49) the author notes that cθ == cos(θ) and sθ == sin(θ) … it’s just shorthand.

#12

Hi, Lexi and Charmaine

I went to the basement to see you guys last Thursday after critique, but I could not see you guys. Sorry I did not realize you guys are still struggling - But I am glad to know Chris is helping you guys! Thanks, Chris!

Thanks for pointing out that sin function takes radians, Chris. That messed up things (just fixed - )
The code above was done when we did not know the two sides have the same length.
I just tried to take an advantage of the face that two arms have the same length. It looks better. But still giving us an weird result at certain areas. (But I think those areas are where the arms cannot reach, referencing your experiments with pen on paper - Does it looks like it?) aA and aC, which we need to know to apply that to Servo motors (Servo #1 : angle - aA, Servo #2 : aC), return the correct numbers. But still glitch when angle get higher number than 90. - which we expected, so we talked about that writing two different code with if function (one for angle < 90, the other for angle >90)

Still need to be debugged, but I am just sharing this to discuss with you guys. If anybody can see how we can improve this, let me know.

``````float a, b, c;
float aA, aB, aC, servoA;
float cosA, cosB, cosC;
PVector va, vb, vc;
float angle;

float tempA, tempO;
void setup() {
size(900, 600);

va = new PVector(-450, 0);
vb = new PVector(150, -10);
vc = new PVector(0, 0);
a = 450;
b = 450;

println(degrees(angle));
}

void draw() {
background(0);
translate(width/2, height);
if (mousePressed == true) {
vb.set(mouseX-width/2, mouseY-height, 0);
angle = degrees(PVector.angleBetween(va, vb));

vc.set(-1*tempA, -1 *tempO, 0);

stroke(255);
line(va.x+450, va.y, vb.x, vb.y);

c = dist(va.x+450, va.y, vb.x, vb.y);// now we know the length of all sides

cosA = (b*b+c*c -a*a)/2/b/c;

aA = degrees(acos(c/900));
aB = aA;
aC = 180 - aA*2;
servoA = angle - aA;
println("Servo #1 : " + servoA + "   Servo #2 : " + aC);

}
triangle(va.x+450, va.y, vb.x, vb.y, vc.x, vc.y);
}

``````

#13

Hey all, here’s my full solution.

``````// derivation http://www.eng.utah.edu/~cs5310/chapter5.pdf

float a1 = 100; // arm length 1
float a2 = 50; // arm length 2

float theta1; // shoulder angle
float theta2; // elbow angle

float alpha = .95; // smoothing alpha

PVector handPosition = new PVector(0, 0);
PVector lastHandPosition = new PVector(0, 0);
PVector shoulderPosition = new PVector(0, 0);

// workspace calculation
float maxRadius = a1 + a2;
float minRadius = max(a2 - a1, a1 - a2);

boolean leftHanded = true;

void setup()
{
size(400, 400);
smooth();

// place it in the middle
shoulderPosition = new PVector(width / 2, height / 2);

}

void draw()
{
background(0);

PVector currentPosition = new PVector(mouseX, mouseY);
currentPosition.sub(shoulderPosition); // normalize

// here we use a simple smoothing algorithm because it looks better and
// appears more like a robot ...
handPosition.x = (1 - alpha) * currentPosition.x + (alpha) * lastHandPosition.x;
handPosition.y = (1 - alpha) * currentPosition.y + (alpha) * lastHandPosition.y;

// the magnitude is the distance from the origin (0,0) to the point represented by the PVector.
float radius = handPosition.mag();

// our workspace color will reflect the validity of our hand position
color statusColor = color(255, 0, 0); // default red

// only do our calculations for hand positions inside workspace
{

float numerator = (a1 + a2) * (a1 + a2) - (handPosition.x * handPosition.x + handPosition.y * handPosition.y);
float denominator = (handPosition.x * handPosition.x + handPosition.y * handPosition.y) - (a1 - a2) * (a1 - a2);

theta2 = 2 * atan(sqrt(numerator / denominator));

if (!leftHanded)
{
theta2 *= -1;
}

float phi = atan2(handPosition.y, handPosition.x); // eq.  (5.5)
float psi = atan2(a2 * sin(theta2), a1 + a2 * cos(theta2)); // eq 5.6

theta1 = phi - psi;

// set a color for the workspace (green = good, red = bad)
statusColor = color(0, 255, 0);
}

// draw everything
pushMatrix();
translate(shoulderPosition.x, shoulderPosition.y);

// draw the outer workspace ring
stroke(statusColor, 80);
fill(statusColor, 40);
ellipse(0, 0, 2 * maxRadius, 2 * maxRadius);

// draw the inner workspace ring
stroke(statusColor, 80);
fill(0);
ellipse(0, 0, 2 * minRadius, 2 * minRadius);

// draw the arm
stroke(255);

// shoulder to elbow
rotate(theta1);
line(0, 0, a1, 0);

// elbow hand
translate(a1, 0);
rotate(theta2);
line(0, 0, a2, 0);

popMatrix();

// save our smoothed hand position so we can use it for smoothing next time
lastHandPosition.set(handPosition.x, handPosition.y);

fill(255);
if (leftHanded)
{
text("Press Any Key to Toggle\nHandedness: Left-handed",10,15);
}
else
{
text("Press Any Key to Toggle\nHandedness: Right-handed",10,15);
}
}

void keyPressed()
{
// toggle handedness
leftHanded = !leftHanded;
}
``````

#14

Hey @leximidkiff and @seedeesee it would be super inspirational if you all could post some documentation on your finished project so people can see how this kind of math gets used. You all any photos or videos or links you could add?