Search

So as we mentioned last time, the Jacobian of the Forwards Kinematic Equations for any robotics system actually gives us a lot of information about that system. The equation relates the desired velocity of the end-effector to the individual joint velocities. We will see later (tomorrow) that is can also be used for torque control but we aren't there yet.

This Jacobian, like we said before, doesn't just let us just compute what velocities we want from the leg, it also lets us compute things like the position of the leg in the environment. If we want to command leg position, we can use the control structure below, but insert an integrator which takes the velocity that the inverse Jacobian outputs and converts that to position commands. We'll need to design a control structure for this...

## anyways... here is how we are getting the Jacobian of the system

MATLAB!!! MATLAB as always. If we pull up Matlab, take the FK equations we developed before, and use the Jacobian function in Matlab we can get the Jacobian of the system!

J =

[ L5*(cos(theta_1)*cos(theta_3) - cos(theta_2)*sin(theta_1)*sin(theta_3)) + L6*(cos(theta_1)*sin(theta_3) + cos(theta_2)*cos(theta_3)*sin(theta_1)) + L2*cos(theta_1) - L3*cos(theta_2)*sin(theta_1) - L4*sin(theta_1)*sin(theta_2),

L4*cos(theta_1)*cos(theta_2) - L3*cos(theta_1)*sin(theta_2) + L6*cos(theta_1)*cos(theta_3)*sin(theta_2) - L5*cos(theta_1)*sin(theta_2)*sin(theta_3),

L6*(cos(theta_3)*sin(theta_1) + cos(theta_1)*cos(theta_2)*sin(theta_3)) - L5*(sin(theta_1)*sin(theta_3) - cos(theta_1)*cos(theta_2)*cos(theta_3))]

[ 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),

L4*cos(theta_2)*sin(theta_1) - L3*sin(theta_1)*sin(theta_2) + L6*cos(theta_3)*sin(theta_1)*sin(theta_2) - L5*sin(theta_1)*sin(theta_2)*sin(theta_3),

L5*(cos(theta_1)*sin(theta_3) + cos(theta_2)*cos(theta_3)*sin(theta_1)) - L6*(cos(theta_1)*cos(theta_3) - cos(theta_2)*sin(theta_1)*sin(theta_3))]

[0,

L6*cos(theta_2)*cos(theta_3) - L4*sin(theta_2) - L3*cos(theta_2) - L5*cos(theta_2)*sin(theta_3), - L5*cos(theta_3)*sin(theta_2) - L6*sin(theta_2)*sin(theta_3)]

Now that seems like a lot... we don't want to just take MATLAB's word for it so let's do it by hand. FK equations --> Jacobian!

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)

We're going to verify/calculate some of the matrix by hand and plug it into Matlab just for peace of mind.

dxe/dtheta_1 =

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

As we can see, verifying dxe/dtheta_1, we get the same answer by hand as we do in the Jacobian matrix Matlab gave us. Probably going to go through by hand and just make sure that everything makes sense you know. But looking at it, it does make sense that theta_1 doesn't have an affect on the z-velocity but the other two would.

So that's the Jacobian, we will likely use it later. First, I think, because we cannot seem to generate a solutions to the inverse kinematics for the previous set of 3D equations, we're going to massively simplify the model a little bit. To do this we're going to need to get creative and, well, we'll do that later today. OR what we will do is come up with an integral control structure that uses the Jacobian model. This might be better. OR we will do BOTH and verify. I think we will try the Jacobian model first because we can do MORE with that.

Hopefully later we can get some PID tuning going.