I'm working through the Inverse Kinematic example for the Unimation PUMA 560 from Introduction to Robotics by Craig. In it he specifies the IK equations like so:
In my software program I have three sliders on the screen that will give me the rotation of the end point in x, y, z like so (this is in Unity):
Each one of these sliders will control a float variable in the code (C#) and I can read these into my script (Using Unity 5). I am trying to replicate the inverse kinematic solution for this PUMA robot inside Unity, so that for a given position and rotation of the end effector the link rotations will update accordingly. I have already written out the IK equations that Craig specified in the example to calculate theta(i), but how do I "read" the slider values and "input" them to these equations? If I am not making any sense I apologize, I have been chipping away at this for some time and hit a mental blank wall. Any advice appreciated.
Edit: So in my near-delirious state I have not posited my question properly. So far, these are the equations I have written so far in code:
public class PUMA_IK : MonoBehaviour {
GameObject J1, J2, J3, J4, J5, J6;
public Vector3 J2J3_diff, J3J4_diff;
public Slider px_Slider;
public Slider py_Slider;
public Slider pz_Slider;
public Slider rx_Slider;
public Slider ry_Slider;
public Slider rz_Slider;
public float Posx, Posy, Posz, Rotx, Roty, Rotz;
float a1, a2, a3, a4, a5, a6; //Joint twist
float r1, r2, r3, r4, r5, r6; //Mutual perpendicular length
float d1, d2, d3, d4, d5, d6; //Link offset
public float t1, t2, t23, t3, t4, t5, t6; //Joint angle of rotation
public float J1Rot, J2Rot, J3Rot, J4Rot, J5Rot, J6Rot;
float r11, r21, r31, r12, r22, r32, r13, r23, r33, c23, s23, Px, Py, Pz, phi, rho, K;
int pose; //1 - left hand, 2 = right hand
// Use this for initialization
void Start ()
{
pose = 1;
J1 = GameObject.FindGameObjectWithTag("J1");
J2 = GameObject.FindGameObjectWithTag("J2");
J3 = GameObject.FindGameObjectWithTag("J3");
J4 = GameObject.FindGameObjectWithTag("J4");
J5 = GameObject.FindGameObjectWithTag("J5");
J6 = GameObject.FindGameObjectWithTag("J6");
J2J3_diff = J3.transform.position - J2.transform.position;
J3J4_diff = J4.transform.position - J3.transform.position;
//Init modified DH parameters
//Joint twist
a1 = 0;
a2 = -90;
a3 = 0;
a4 = -90;
a5 = 90;
a6 = -90;
//Link length
r1 = 0;
r2 = Mathf.Abs(J2J3_diff.x);
r3 = Mathf.Abs(J3J4_diff.x);
r4 = 0;
r5 = 0;
r6 = 0;
//Link offset
d1 = 0;
d2 = 0;
d3 = Mathf.Abs(J2J3_diff.z);
d4 = Vector3.Distance(J4.transform.position, J3.transform.position);
d5 = 0;
d6 = 0;
}
void Update ()
{
Posx = px_Slider.value;
Posy = py_Slider.value;
Posz = pz_Slider.value;
Rotx = rx_Slider.value;
Roty = ry_Slider.value;
Rotz = rz_Slider.value;
Px = Posx;
Py = Posy;
Pz = Posz;
c23 = ((cos(t2)*cos(t3)) - (sin(t2)*sin(t3)));
s23 = ((cos(t2)*sin(t3)) + (sin(t2)*cos(t3)));
rho = Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2));
phi = Mathf.Atan2(Py, Px);
if (pose == 1)
{
t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
}
if (pose == 2)
{
t1 = Mathf.Atan2(Py, Px) - Mathf.Atan2(d3, -Mathf.Sqrt(Mathf.Pow(Px, 2) + Mathf.Pow(Py, 2) - Mathf.Pow(d3, 2)));
}
K = (Mathf.Pow(Px, 2)+ Mathf.Pow(Py, 2) + Mathf.Pow(Px, 2) - Mathf.Pow(a2, 2) - Mathf.Pow(a3, 2) - Mathf.Pow(d3, 2) - Mathf.Pow(d4, 2)) / (2 * a2);
if (pose == 1)
{
t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
}
if (pose == 2)
{
t3 = Mathf.Atan2(a3, d4) - Mathf.Atan2(K, -Mathf.Sqrt(Mathf.Pow(a2, 2) + Mathf.Pow(d4, 2) - Mathf.Pow(K, 2)));
}
t23 = Mathf.Atan2(((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) * (d4 - (a2 * sin(t3))), ((((a2 * sin(t3)) - a4) * Pz) - ((a3 + (a2 * cos(t3))) * ((cos(t1) * Px) + (sin(t1) * Py)))));
t2 = t23 - t3;
if (sin(t5) != 0) //Joint 5 is at zero i.e. pointing straight out
{
t4 = Mathf.Atan2((-r13 * sin(t1)) + (r23 * cos(t1)), (-r13 * cos(t1) * c23) + (r33 * s23));
}
float t4_detection_window = 0.00001f;
if ((((-a3 - (a2 * cos(t3))) * Pz) - ((cos(t1) * Px) + (sin(t1) * Py)) < t4_detection_window) && (((-r13 * cos(t1) * c23) + (r33 * s23)) < t4_detection_window))
{
t4 = J4Rot;
}
float t5_s5, t5_c5; //Eqn 4.79
t5_s5 = -((r13 * ((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4)))) + (r23 * ((sin(t1) * c23 * cos(t4)) - (cos(t1) * sin(t4)))) - (r33 * (s23 * cos(t4))));
t5_c5 = (r13 * (-cos(t1) * s23)) + (r23 * (-sin(t1) * s23)) + (r33 * -c23);
t5 = Mathf.Atan2(t5_s5, t5_c5);
float t5_s6, t5_c6; //Eqn 4.82
t5_s6 = ((-r11 * ((cos(t1) * c23 * sin(t4)) - (sin(t1) * cos(t4)))) - (r21 * ((sin(t1) * c23 * sin(t4)) + (cos(t1) * cos(t4)))) + (r31 * (s23 * sin(t4))));
t5_c6 = (r11 * ((((cos(t1) * c23 * cos(t4)) + (sin(t1) * sin(t4))) * cos(t5)) - (cos(t1) * s23 * sin(t5)))) + (r21 * ((((sin(t1) * c23 * cos(t4)) + (cos(t1) * sin(t4))) * cos(t5)) - (sin(t1) * s23 * sin(t5)))) - (r31 * ((s23 * cos(t4) * cos(t5)) + (c23 * sin(t5))));
t6 = Mathf.Atan2(t5_s6, t5_c6);
//Update current joint angle for display
J1Rot = J1.transform.localRotation.eulerAngles.z;
J2Rot = J2.transform.localRotation.eulerAngles.y;
J3Rot = J3.transform.localRotation.eulerAngles.y;
J4Rot = J4.transform.localRotation.eulerAngles.z;
J5Rot = J5.transform.localRotation.eulerAngles.y;
J6Rot = J6.transform.localRotation.eulerAngles.z;
}
void p(object o)
{
Debug.Log(o);
}
float sin(float angle)
{
return Mathf.Rad2Deg * Mathf.Sin(angle);
}
float cos(float angle)
{
return Mathf.Rad2Deg * Mathf.Cos(angle);
}
}
The issue is not with the mathematics of what is going on per se, I am just confused at how I interface the three values of the X, Y, and Z rotation for the sliders (which represent the desired orientation) with these equations. For the translation component it is easy, the slider values are simply equal to Px, Py and Pz in the IK equation set. But in his equations he references r11, r23, etc, which are the orientation components. I am unsure how to replace these values (r11, r12, etc) with the slider values.
Any ideas?
Edit 2: I should also say that these sliders would be for positioning the tool center point. The XYZ sliders will give the translation and the others would give the orientation, relative to the base frame. I hope this all makes sense. The goal is to be able to use these sliders in a similar fashion to how one would jog a real robot in world mode (as opposed to joint mode). I then pass these calculated angle values to the transform.rotation component of each joint in Unity.
So what I am really asking is given the three numbers that the rotation sliders produce (XRot, YRot and ZRot), how do I plug those three numbers into the IK equations?