Difference between revisions of "Inverse Kinematics"
Line 4: | Line 4: | ||
+ | <nowiki> | ||
+ | import java.io.IOException; | ||
+ | import java.net.URISyntaxException; | ||
+ | |||
+ | import org.ejml.simple.SimpleMatrix; | ||
+ | import org.jblas.DoubleMatrix; | ||
+ | |||
+ | import Jama.Matrix; | ||
public class soloIKLimits { | public class soloIKLimits { | ||
Line 368: | Line 376: | ||
} | } | ||
+ | </nowiki> |
Revision as of 01:03, 16 May 2017
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.
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;
}
}