Inverse Kinematics
From Lofaro Lab Wiki
The inverse Kinematics algorithm used was the Inverse Jacobian. We used the Inverse Jacobian because it is highly parallizable which was a key aspect of our project because we wanted to distribute the calculations across different controllers. This algorithm also allows us easily preform inverse kinematics with a joint locked because we can remove that column from the Jacobian matrix. After the Jacobian is computed the thetas are changed in small iterations. In the code all that needs to be done is change the desired target position's x,y,z,roll,pitch,yaw coordinate.
Inverse Kinematics Code
import java.io.IOException; import java.net.URISyntaxException; import org.ejml.simple.SimpleMatrix; import org.jblas.DoubleMatrix; import Jama.Matrix; public class soloIKLimits { public static void main(String[] args) throws IOException, ClassNotFoundException, InterruptedException, URISyntaxException { // TODO Auto-generated method stub double thetas [] = new double[6]; //inputting a starting position in terms of x,y,z,roll pitch yaw and a goal position with //that same structure pattern thetas = inverseK(2.27,-2.27,-1.5,1.27,0.0,-.37,-1.144,2.45,.149,-2.26,-.015); double[] thetav = inverseK(thetas[0],thetas[1],thetas[2],thetas[3],thetas[4],-.37,-1.144,2.55,-.119,-2.26,.012); } public static double[] inverseK(double th1,double th2,double th3,double th4,double th5,double targ1,double targ2,double targ3,double targ4, double targ5, double targ6) throws InterruptedException { // broken joint //set equal to 1 for broken joint double broken1 = 0; double broken2 = 1; double broken3 = 0; double broken4 = 0; double broken5 = 0; //healthy joints in our 5DOF robot //this will be the number of columns in our jacobian matrix int numHealthy = 5 - (int)(broken1+broken2+broken3+broken4+broken5); boolean broken = false; //the number of rows in our jacobian matrix, it will change to 3 if any joints broken int jacobSize = 6; //array of which joints are broken, 1 for broken 0, for healthy double robotJoints[] = {broken1, broken2, broken3, broken4, broken5}; //theta limits for each joint in the robot double maxtv1 = 5.41; double maxtv2 = .61; double maxtv3 = 2.27; double maxtv4 = 2.27; double maxtv5 = 2.27; double mintv1 = 0; double mintv2 = -2.27; double mintv3 = -2.27; double mintv4 = -2.27; double mintv5 = -2.27; //array of min and max limits Double []maxRow = {maxtv1, maxtv2, maxtv3, maxtv4, maxtv5}; Double[]minRow = {mintv1, mintv2, mintv3, mintv4, mintv5}; //thetas taken from input double theta1 = th1; double theta2 = th2; double theta3 = th3; double theta4 = th4; double theta5 = th5; //ranges used later in the code int [] rang6 = {0,1,2,3,4,5}; int[] rang3 = {0,1,2}; //array of the angles of each joint double thetaRow[] = {theta1,theta2,theta3,theta4,theta5}; //current position //finds the current position given the initial angles given; forward kinematics DoubleMatrix cPos = fK(theta1,theta2,theta3,theta4,theta5); //real 6 DOF position, in case any joints are broken //if any joints broken cPos becomes 3DOF DoubleMatrix rPos = cPos; //if a single joint is broken, we stop using 6DOF space and //turn to 3DOF space because with broken joints there are extreme //limits of what is reachable at a certain roll pitch yall orientation //we found their to only be one solution with 1 specific roll pitch yaw orientation if (broken1 == 1 || broken2 == 1|| broken3 == 1 || broken4 == 1|| broken5 == 1) { cPos = cPos.get(rang3, 0); broken = true; jacobSize = 3; } //inputting the target position DoubleMatrix tPos = new DoubleMatrix(6,1); tPos.put(0, 0, targ1); tPos.put(1, 0, targ2); tPos.put(2, 0, targ3); tPos.put(3, 0, targ4); tPos.put(4, 0, targ5); tPos.put(5, 0, targ6); //real 6 DOF error in case any joints are broken //if any joints broken error becomes 3DOF DoubleMatrix trueError = tPos.sub(rPos); //sets the roll pitch yaw values to 0 if any joints broken if (broken1 == 1 || broken2 == 1|| broken3 == 1 || broken4 == 1|| broken5 == 1) { tPos = tPos.get(rang3, 0); trueError.put(3,0,0); trueError.put(4,0,0); trueError.put(5,0,0); jacobSize = 3; } //finds error of the system //which is the difference from the current position //and the goal position DoubleMatrix error = tPos.sub(cPos); //in the while loop below, we recaclucalte the error, //we also divide it by a scaled factor to move towards //our goal position in small steps double eScale = 1; //the small change in theta we apply to each angle double dtheta = .01; //the matrices we use DoubleMatrix jacobian = new DoubleMatrix(jacobSize,numHealthy); DoubleMatrix jFull = new DoubleMatrix(6,5); DoubleMatrix jinv = new DoubleMatrix(5,6); SimpleMatrix dthetab = new SimpleMatrix(6,1); SimpleMatrix derror = new SimpleMatrix(6,1); int n = 0; int counter = 0; //loop until our error is small enough while ( Math.abs(trueError.get(0,0)) > .05 || Math.abs(trueError.get(1,0)) > .05 || Math.abs(trueError.get(2,0)) > .05 || Math.abs(trueError.get(3,0)) > .05 || Math.abs(trueError.get(4,0)) > .05 || Math.abs(trueError.get(5,0)) > .05 ) { //find the forward kinematics (current position) given a small change //dtheta in each individual joint in our robot DoubleMatrix fk1 = fK(theta1 + dtheta,theta2,theta3,theta4,theta5); DoubleMatrix fk2 = fK(theta1,theta2+ dtheta,theta3,theta4,theta5); DoubleMatrix fk3 = fK(theta1,theta2,theta3+ dtheta,theta4,theta5); DoubleMatrix fk4 = fK(theta1,theta2,theta3,theta4+ dtheta,theta5); DoubleMatrix fk5 = fK(theta1,theta2,theta3,theta4,theta5+ dtheta); int k; //construct full jacobian for(k = 0; k< 6; k++) { jFull.put(k,0,(fk1.get(k,0) - rPos.get(k,0))/dtheta); jFull.put(k,1,(fk2.get(k,0) - rPos.get(k,0))/dtheta); jFull.put(k,2,(fk3.get(k,0) - rPos.get(k,0))/dtheta); jFull.put(k,3,(fk4.get(k,0) - rPos.get(k,0))/dtheta); jFull.put(k,4,(fk5.get(k,0) - rPos.get(k,0))/dtheta); } //loop values int l = 0; int o = 0; //construct adjusted jacobian, only changes from full jacobian //if any joint is broken while (l < 5) { if (robotJoints[l] == 0) { if (broken == false) { jacobian.put(rang6,o,jFull.get(rang6,l)); } else { jacobian.put(rang3,o,jFull.get(rang3,l)); } o++; } l++; } //used SimpleMatrix because its pinv is better than DoubleMatrix //and compatible with Apache Spark. found this out after the fact //so simply converted SimpleMatrix simpleJac = new SimpleMatrix(jacobian.toArray2()); //finding inverse of jacobian SimpleMatrix simpleIn = simpleJac.pseudoInverse(); //dividng our error by a scalar value derror = new SimpleMatrix(error.div(eScale).toArray2()); //finding the vector dethetabar by multiplying the inverse times the error //this vector tells us how much we need to move each joint to get closer to //the desired position dthetab = simpleIn.mult(derror); //loop values int h = 0; int g = 0; //divide if dthetabar spits values that are too big while (h < 5) { while (Math.abs(dthetab.get(g,0)) > .69) { dthetab.set(g,0,dthetab.get(g,0)/10); g++; } h++; } //loop values int index = 0; int z = 0; //move theta by small amount and enfore theta limits for (index=0; index < 5; index++) { if (robotJoints[index] == 0) { thetaRow[index] = (thetaRow[index] + dthetab.get(z,0)); if (thetaRow[index] > maxRow[index]){ thetaRow[index] = thetaRow[index] - dthetab.get(z,0); dthetab.set(z,0,0); } else if (thetaRow[index] < minRow[index]){ thetaRow[index] = thetaRow[index] - dthetab.get(z,0); dthetab.set(z,0,0); } z++; } } //update the thetas theta1 = thetaRow[0]; theta2 = thetaRow[1]; theta3 = thetaRow[2]; theta4 = thetaRow[3]; theta5 = thetaRow[4]; // //update real position rPos = fK(thetaRow[0],thetaRow[1],thetaRow[2],thetaRow[3],thetaRow[4]); //update current position accordingly if (broken == false) { cPos.put(rang6,0,rPos.get(rang6,0)); } else { cPos.put(rang3,rPos.get(rang3,0)); } error = tPos.sub(cPos); System.out.println(cPos.toString()); n = n + 1; counter++; //make true error to enable entry in loop //essentially makes our error in 3DOF space instead of 6DOF //if a joint is broken if (broken == true) { trueError.put(0, 0,error.get(0,0)); trueError.put(1, 0,error.get(1,0)); trueError.put(2, 0,error.get(2,0)); trueError.put(3, 0,0); trueError.put(4, 0,0); trueError.put(5, 0,0); } } //once loop is done return the thetas that get us to the goal position double[] list = {theta1,theta2,theta3,theta4,theta5}; return list; } public static DoubleMatrix fK(double theta1,double theta2,double theta3,double theta4,double theta5) { //the Denavit–Hartenberg parameters for our robot double alpha1 = - (Math.PI/2); double alpha2 = 0; double alpha3 = 0; double alpha4 = Math.PI/2; double alpha5 = 0; //distance in the x direction from frame to frame double r1 = 0; double r2 = 0.729; double r3 = 0.729; double r4 = 0; double r5 = 0; double d1 = 1.145; double d2 = 0; double d3 = 0; double d4 = 0; double d5 = .354; //find transformation matrix for each joint //given each Denavit–Hartenberg parameter Matrix nA1 = new Matrix(findT(theta1,r1,d1,alpha1).toArray2()); Matrix nA2 = new Matrix(findT(theta2,r2,d2,alpha2).toArray2()); Matrix nA3 = new Matrix(findT(theta3,r3,d3,alpha3).toArray2()); Matrix nA4 = new Matrix(findT(theta4,r4,d4,alpha4).toArray2()); Matrix nA5 = new Matrix(findT(theta5,r5,d5,alpha5).toArray2()); //find complete homogenious transformation matrix from the transformaitom matrices Matrix homoGen1 =(((((nA1.times(nA2)).times(nA3)).times(nA4)).times(nA5))); //convert from class Matrix to DoubleMatrix DoubleMatrix homoGen = (new DoubleMatrix(homoGen1.getArray())); DoubleMatrix cPos = new DoubleMatrix(6,1); //fill the current position with its x,y,z, roll,pitch yaw values cPos.put(0,0,homoGen.get(0,3)); cPos.put(1,0,homoGen.get(1,3)); cPos.put(2,0,homoGen.get(2,3)); cPos.put(3,0, Math.atan2(-homoGen.get(2,0),(Math.sqrt(homoGen.get(2,1)*homoGen.get(2,1) + homoGen.get(2,2)*homoGen.get(2,2))))); cPos.put(4,0,Math.atan2(homoGen.get(1,0), homoGen.get(0,0))); cPos.put(5,0,Math.atan2(homoGen.get(2,1), homoGen.get(2,2))); return cPos; } public static DoubleMatrix findT(double theta, double r, double d, double alpha) { //finding transofmration matrix give the parameters DoubleMatrix A1 = new DoubleMatrix(4,4); A1.put(0, 0, Math.cos(theta)); A1.put(0, 1, -Math.sin(theta)*Math.cos(alpha)); A1.put(0, 2, Math.sin(theta)*Math.sin(alpha)); A1.put(0, 3, r*Math.cos(theta)); A1.put(1, 0, Math.sin(theta)); A1.put(1, 1, Math.cos(theta)*Math.cos(alpha)); A1.put(1, 2, -Math.cos(theta)*Math.sin(alpha)); A1.put(1, 3, r*Math.sin(theta)); A1.put(2, 0, 0); A1.put(2, 1, Math.sin(alpha)); A1.put(2, 2, Math.cos(alpha)); A1.put(2, 3, d); A1.put(3, 0, 0); A1.put(3, 1, 0); A1.put(3, 2, 0); A1.put(3, 3, 1); return A1; } }
