- aditya mehrotra.

# chip updates: the final inverse kinematic equations for chip - 3D this time! [updates]

Okay so today, we're going to be doing few posts based on the things we learned in 2.12 the past week. Also tomorrow, we'll be doing some design posts on the 2.12 robot arm design for the term project. But the next few Chip posts are going to be on the full Inverse Kinematic Equations, the Jacobian of the system and any Singularities. Then we can evaluate the design of the leg now that we've built it :)!

So this post will be on developing the actual inverse kinematics of the legs of chip. What we're going to do is develop a full-3D-Kinematic Equations set using the __Denavit-Hartenberg Convention__ we mentioned earlier. We're first going to do this based fully on variables, later we will add in actual lengths and etc. In this post we'll also develop where/how to ZERO the leg because of this - we'll develop the zeros AFTER we do Denavit-Hartenberg but BEFORE we do the kinematics.

So the first thing we need is a drawing...

This drawing roughly accurately describes what we have in terms of the Chip Leg System (we will refer to as CLS). The first thing we're going to do is develop the coordinate frame, and then we will go into the DH-Parameters and IK Equations.

## the CLS coordinate frame:

The CLS coordinate frame will be derived starting from DH parameters but also using what makes sense for our system. In DH convention, the Z-axis points along the motor's axis of rotation. Which will be one constraint when we choose the CLS coordinate frame. We also know that due to IK conventions, the base of the robot, or the frame in our case, is the origin. So each leg will have an origin starting where it connects to the frame.

According to this convention we developed before, the Shoulder Joint is the origin of the CLS at the location where it connects to the frame. But we also know the Z-Axis must point ALONG the shaft of the motor for DH parameters to work, therefore the leg coordinate system can be defined as follows if we use some logical thinking on the side-view of the robot.

I will remind you this is completely arbitrary...

As you can now see the orientation of the X-Y-Z axis and the origin of the entire robot frame. We can start developing the rotation matrices using the DH parameters, and then go to inverse kinematics.

## ah DH, matrices made standard (I refuse to say easy):

So you're probably aware (if you've been following along), 3D kinematics involves many rotation matrices that translate the system's coordinate frame in 3D so we can find the endpoint location in terms of the joint parameters. DH parameters/convention allow us to place these rotation matrices well/accurately while using an industry standard. Here we go.

Let's start by drawing the Z-Axis of all the actuators, and the X/Y Axis of the origin!

Now let's start with the DH parameters between Z1 and Z2. According to the diagram above, we can define the four DH parameters:

d1 - is ZERO because the two z-axis are on the same plane

theta - is -90 DEGREES to turn the x-axis in the direction we want (this rotation is around the y-axis and in the negative direction)

a - the length of the common normal, the distance the origin moves, is L1

alpha - in this case is also ZERO because the Z-axis are now aligned

Here's how this looks in pictures...

Now let's do this for Z2 to Z3! Once again, let's define the parameters. If you are having trouble following along, go watch the video linked above about DH parameters or look at our post about it.

d2 - is L2 to get the origin into the new correct plane

theta - is again -90 Degrees this time around the Z-Axis to get the X-Axis aligned with the common normal

a - is L3 which is sends the origin into the correct plane

alpha - is -90 Degrees about the X-Axis that aligns the z-Axis.

This is what that looks like:

Now we have THREE origins and their axis directions. And from O3, we always know the position of the end-effector from that origin. Which means we can now use inverse kinematics and rotation matrices to develop the kinematic equations but FIRST. We need the zeros!

## now let's develop some zeros:

Developing the zeros locations of the model is really important because that will allow us to gain the other important rotation parameters. Deciding these zeros will change the kinematic equations so that's what we are going to set as convention NOW.

By convention we will set the X-Axis as the zeros, and the positive direction of rotation with the right-hand rule.

Okay... so I started with this and then confused myself. Let's try doing this with a simplified stick diagram because that will help us visualize it more!!! Without the actuators.

This is how we are going to define zeros and thetas because that's what makes the most sense based on the DH parameters.

## finally getting to the kinematics, enough setup:

So now what we are going to do is develop the final inverse kinematic equations for the CLS based on what we developed above. So here's what we know, for a 3-DoF robotic system...

Xe = X12+R12*X23+R12*R23*X3E

That's the kinematic equation right? So now we need to start defining the variables in this equation. We're going to use MatLab along the way!

The first thing we are going to define is the vector X12 which is the vector that points between the origins O1 and O2. Take a look at the diagram and this is <0,0,L1>

X12 = <0,0,L1>

So this is what we have in Matlab so far...

Now we want to define the rest of the vectors X23 and X3E, and the reason for this is because it's just very easy to define them by looking at the diagram.

X23 = <0,-L3,L2>

X3E = <L5,-L6,L4>

Now, it's time to define the rotation matrices between each of the rotational transforms *both* due to DH-parameters *and *due to the theta rotation of the actuators themselves.

**A MISTAKE:**

So R12 (or just R1) has a rotation of theta_1 about the Z-Axis and then -90d about the Y-Axis.

R23 (R2) has a rotation of +90d around the *original* Y-Axis, and -90d+theta_2 around the Z-Axis.

R3E (R3) has a simple rotation of theta_3 around the Z-Axis.

Now that we know what the rotations *are*, it's time to define the actual matrices R1, R2, R3. And off we go to Matlab.

^^ OKAY THAT'S WHAT WE THOUGHT AND IT DIDN'T WORK LET's RETRY IT AGAIN. WE KNOW IT DIDN'T WORK BECAUSE WE ENDED UP WITH THREE ROTATION MATRICES AND WE SHOULD'VE ENDED UP WITH TWO.

**THE ACTUAL ROTATION MATRICES:**

So R1 is a rotation of -90d about the Y-Axis, theta_1 about the original Z-Axis, and -theta_2 about the original X-Axis! (we need to account for ALL of these in the next rotation)

R2 is a rotation of +90d around the original Y-Axis of origin 2, -90d around the Z-Axis of origin 2, and +theta_3 around the original X-Axis. (The rotation of theta_2 is accounted for in the previous rotation matrix!)

***NOTE FOR THESE MATRICES, ALL ANGLES ARE WITH RESPECT TO THE ORIGINAL X-Y-Z AXIS

Knowing this...

The Matlab script can now output the equations for the inverse kinematics for the legged robot system.

xe == - L2*cos(theta_2) - L3*sin(theta_2) - L5*sin(theta_2) - L6*cos(theta_2)*cos(theta_3) - L4*cos(theta_2)*sin(theta_3)

ye == L4*(cos(theta_3)*sin(theta_1) + cos(theta_1)*sin(theta_2)*sin(theta_3)) - L6*(sin(theta_1)*sin(theta_3) - cos(theta_1)*cos(theta_3)*sin(theta_2)) - L3*cos(theta_1)*cos(theta_2) - L5*cos(theta_1)*cos(theta_2) + L2*cos(theta_1)*sin(theta_2)

ze == L1 + L4*(cos(theta_1)*cos(theta_3) - sin(theta_1)*sin(theta_2)*sin(theta_3)) - L6*(cos(theta_1)*sin(theta_3) + cos(theta_3)*sin(theta_1)*sin(theta_2)) + L3*cos(theta_2)*sin(theta_1) + L5*cos(theta_2)*sin(theta_1) - L2*sin(theta_1)*sin(theta_2)

Ah yes... ridiculous set of equations isn't it.

## and that was just forward kinematics... how about inverse:

So to solve the inverse kinematic equations, what we want to do is use the half tangent formulas to solve this system.

We're going to create these for theta_1, theta_2, and theta_3. FYI t=tan(theta/2)! We're going to substitute in these t1s, t2s, and t3s!

So we went through all of that and now have many possible values for the angles for all t1, t2, and t3, and we will have to optimize. The script, by the way, takes forever to run. So we'll just hard-code the final equations we will obtain from it.

Here are the *simplified equations for t3...*

*ze1 =*

* *

*(L3 + L5 + (L2^2*t3^4 + 2*L2^2*t3^2 + L2^2 + 4*L2*L4*t3^3 + 4*L2*L4*t3 - 2*L2*L6*t3^4 + 2*L2*L6 + L3^2*t3^4 + 2*L3^2*t3^2 + L3^2 + 2*L3*L5*t3^4 + 4*L3*L5*t3^2 + 2*L3*L5 + 4*L4^2*t3^2 - 4*L4*L6*t3^3 + 4*L4*L6*t3 + L5^2*t3^4 + 2*L5^2*t3^2 + L5^2 + L6^2*t3^4 - 2*L6^2*t3^2 + L6^2 - t3^4*xe^2 - 2*t3^2*xe^2 - xe^2)^(1/2) + L3*t3^2 + L5*t3^2)^2 + (L2 + L6 - xe + 2*L4*t3 + L2*t3^2 - L6*t3^2 - t3^2*xe)^2 ~= 0 & L3 + L5 + L3*t3^2 + L5*t3^2 + 2*L6*t2*t3^2 ~= ye + 2*L2*t2 + 2*L6*t2 + L3*t2^2 + L5*t2^2 + t2^2*ye + t3^2*ye + L3*t2^2*t3^2 + L5*t2^2*t3^2 + t2^2*t3^2*ye + 4*L4*t2*t3 + 2*L2*t2*t3^2 & 4*L2*(L3 + L5 + (... Output truncated. Text exceeds maximum line length for Command Window display.*

So... I'm either doing something horribly wrong. Or this is NOT GOING TO WORK.

Let's try something else!

## take 2 at solving the IK equations:

Here are the final IK equations, as a reminder:

xe == - L2*cos(theta_2) - L3*sin(theta_2) - L5*sin(theta_2) - L6*cos(theta_2)*cos(theta_3) - L4*cos(theta_2)*sin(theta_3)

ye == L4*(cos(theta_3)*sin(theta_1) + cos(theta_1)*sin(theta_2)*sin(theta_3)) - L6*(sin(theta_1)*sin(theta_3) - cos(theta_1)*cos(theta_3)*sin(theta_2)) - L3*cos(theta_1)*cos(theta_2) - L5*cos(theta_1)*cos(theta_2) + L2*cos(theta_1)*sin(theta_2)

ze == L1 + L4*(cos(theta_1)*cos(theta_3) - sin(theta_1)*sin(theta_2)*sin(theta_3)) - L6*(cos(theta_1)*sin(theta_3) + cos(theta_3)*sin(theta_1)*sin(theta_2)) + L3*cos(theta_2)*sin(theta_1) + L5*cos(theta_2)*sin(theta_1) - L2*sin(theta_1)*sin(theta_2)

So what I'm first going to do, based on the equations above, is let's sanity-check these equations by plugging in some reasonable values for theta_1, theta_2, theta_3, L1-L6!

theta_1 = 45d

theta_2 = 0d

theta_3 = -90d

L1 = 1.5"

L2 = 1.5"

L3 = 9"

L4 = 0.5"

L5 = 0.5"

L6 = 12"

Let's use Matlab to solve for xe, ye, ze based on these values and see if we get something useful. Careful, in Matlab sin, cos, tan need the angles in radians, but rotz, rotx, roty need them in degrees.

We get...

xe = -1

ye= 1.7678

ze= 16.7028

Now this seems... very wrong. Oops. It's time to go back and find out where we made our mistake. We should expect a small x value, a large - y value and a small + z value. That is, if I am understanding the IK equations correctly.

So I found this: __https://www.mathworks.com/matlabcentral/fileexchange/25855-plot-and-animate-robot__

We're going to download this Matlab tool to see if our DH parameters are even correct and then try to implement the FK and IK in Matlab.

So that one isn't running: let's try this one - __https://www.mathworks.com/matlabcentral/fileexchange/56917-implementation-of-the-denavit-hartenberg-dh-parameters-in-matlab__

That one works, let's plug in the parameters we got and see what is up:

And so I don't even know what's happening here, but this doesn't seem right based on our initial intuition. What I want to do now is get rid of DH and try the kinematics just using standard rotation matrices.

## checking our work, because it was wrong:

So let's try this again, from the top, with standard rotation matrices. Let's not use DH parameters, re-draw our diagram with our own zeros and everything else. Let's make a new Matlab script as well.

I have an intuition that what we messed up last time was the ZEROs position last time. I would like to note the LEG origin itself is staying the same. Check out the below diagram, if we zero like the following system, then the position of the leg is as follows according to the diagram.

r0 = <-L2-L6, -L3-L5, L1+L4>

We've also defined all joint movements as rotations around the Z-Axis. Let's also draw a rotated version and a homed version of the system.

Looking at this diagram again, I think another reason for the issue is I kept adding (-) signs in-front of the thetas. What I want to do is check the signs of the thetas first, and then get back to this if that does NOT work.

**ACTUAL ROTATION MATRICIES MAYBE:**

R1 - So let's multiply three matrices together, let's first rotate -90d around the Y-Axis. Then take that and rotate by theta_1 around the X-Axis, then rotate by theta_2 around the Z-Axis.

R2 - now we have a +90d rotation about the Y-Axis of the new coordinate frame, then we rotate -90d around the Z-axis, then +theta_3 around the Z-Axis.

***NOTE FOR THESE MATRICES, WE'RE DOING ROTATIONS IN STEPS. In this case ORDER MATTERS.

The vectors, X12, X23, and X3E will not change because that much confidence I have in myself. Once again, let's remind ourselves:

R1 = roty(-90)*[1,0,0;0,cos(theta_1),-sin(theta_1);0,sin(theta_1),cos(theta_1)]*[cos(theta_2),-sin(theta_2),0;sin(theta_2),cos(theta_2),0;0,0,1]

R2 = roty(90)*rotz(-90)**[cos(theta_3),-sin(theta_3),0;sin(theta_3),cos(theta_3),0;0,0,1]

Now we are going to plug in the same things we did above for a sanity check and see what we get as xe, ye, ze!

xe = -3.5355

ye = 0.7071

ze = 2

But now I think what needs to happen is we need to add the ZERO position of the leg. Let's add r0 = <-L2-L6, -L3-L5, L1+L4>...

And this doesn't work either...

## let's just start from the beginning:

The only thing we have not changed is the origin of the CLS. Now let's go completely back to the drawing board. Let me do these calculations by hand, focus on that instead of writing this, and then post them here with the equations. I'm not sure why I'm having so much trouble with this sorry!!!!

Okay let's use this picture now...

Okay and NOW, if we run these equations with the test parameters we placed in the Matlab script above. We get...

xe=-2.8284

ye=-16.2635

ze=2

Which makes *much more sense in inches. *Which means we are ready to write down the kinematic equations. Note the ZERO positions of the motors and the POSITIVE direction of the thetas! That is defined in the drawing above.

**FINAL FORWARDS KINEMATIC EQUATIONS:**

xe=L5*(cos(theta_3)*sin(theta_1) + cos(theta_1)*cos(theta_2)*sin(theta_3)) + L6*(sin(theta_1)*sin(theta_3) - cos(theta_1)*cos(theta_2)*cos(theta_3)) + L2*sin(theta_1) + L3*cos(theta_1)*cos(theta_2) + L4*cos(theta_1)*sin(theta_2)

ye=L3*cos(theta_2)*sin(theta_1) - L6*(cos(theta_1)*sin(theta_3) + cos(theta_2)*cos(theta_3)*sin(theta_1)) - L2*cos(theta_1) - L5*(cos(theta_1)*cos(theta_3) - cos(theta_2)*sin(theta_1)*sin(theta_3)) + L4*sin(theta_1)*sin(theta_2)

ze=L1 + L4*cos(theta_2) - L3*sin(theta_2) + L6*cos(theta_3)*sin(theta_2) - L5*sin(theta_2)*sin(theta_3)

Now the last check we want to run is substitute the zero positions for the thetas and see if we get the r0 we predict of r0=<L3-L6,-L2-L5,L1+L4>

CHECK =

L3 - L6

- L2 - L5

L1 + L4

AND WE DO. WHICH MEANS OUR FORWARD KINEMATIC EQUATIONS ARE RIGHT. I think we major overcomplicated this nonsense with the DH parameters. We're just going to use the model we currently/just developed. It's time to solve the inverse kinematic equations.

## solving the inverse kinematic equations, hopefully one last time:

Now we are onto solving the IK equations hopefully for the last time tonight. It will be super helpful because then we'll be able to solve the joint positions in terms of the desired position of the robot foot.

We will use Matlab to generate the IK equations, in Matlab, there's a toolbox that allows us to model a robot and generate the IK equations! __https://www.mathworks.com/help/robotics/ref/rigidbodytree.html__

We're going to work on this later when we know how to do it.

## summary of information:

**FINAL FORWARDS KINEMATIC EQUATIONS:**

xe=L5*(cos(theta_3)*sin(theta_1) + cos(theta_1)*cos(theta_2)*sin(theta_3)) + L6*(sin(theta_1)*sin(theta_3) - cos(theta_1)*cos(theta_2)*cos(theta_3)) + L2*sin(theta_1) + L3*cos(theta_1)*cos(theta_2) + L4*cos(theta_1)*sin(theta_2)

ye=L3*cos(theta_2)*sin(theta_1) - L6*(cos(theta_1)*sin(theta_3) + cos(theta_2)*cos(theta_3)*sin(theta_1)) - L2*cos(theta_1) - L5*(cos(theta_1)*cos(theta_3) - cos(theta_2)*sin(theta_1)*sin(theta_3)) + L4*sin(theta_1)*sin(theta_2)

ze=L1 + L4*cos(theta_2) - L3*sin(theta_2) + L6*cos(theta_3)*sin(theta_2) - L5*sin(theta_2)*sin(theta_3)

**FINAL INVERSE KINEMATIC EQUATIONS:**

WILL COME LATER (probably tomorrow)

**ZERO POSITION AND POSITIVE THETA DIRECTIONS:**

The zero position of the actuators is shown in this diagram, so are the direction of the angles. Note the zero position of the leg, theta_1 is a rotation around Z, theta_2 is a rotation around Y, and theta_3 is again about Z. All thetas are in the positive direction of rotation of that axis.

We'll finish this up tomorrow or Thursday. Right now, I'm exhausted and sick of matrices. Good night y'all.

#chip #scout #updates #robotics #inverse_kinemaics #engineers_do_math #we_bad_at_math #omgrobots #tenb_robo_lab