Symbolic manipulator FK and Jacobian in Java

Today I’m posting my Java code for getting the symbolic forward kinematics (FK) and Jacobian for an arbitrary serial manipulator.
As symbolic computation libraries are sparse in Java, I’m invoking Python from the terminal and retrevies the output using I/O streams. As such, Python needs to present on the system.
The Python code is embedded in the Java code as a String.

The joint type strings are either RX,RY,RZ,PX,PY,PZ and FIXED, where R = Revolute, and P = Prismatic, and the X, Y or Z is the axis of rotation. FIXED is simply a translation matrix.
The relative position array is the description of each joint types relative position to the previous one. [x,y,z,x,y,z,…..]

Java dependencies:
Google gson

Python dependencies:
SymPy


import com.google.gson.Gson;
import java.io.BufferedReader;
import java.io.BufferedWriter;
import java.io.FileWriter;
import java.io.IOException;
import java.io.InputStreamReader;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.List;
import java.util.Map;
import java.util.logging.Level;
import java.util.logging.Logger;

/**
*
* @author Lars Ivar Hatledal
*/
public class SymbolicKinematicsRetriever {

private final static Logger LOG = Logger.getLogger(SymbolicKinematicsRetriever.class.getName());

Gson gson = new Gson();
private final String json;

public SymbolicKinematicsRetriever(String[] jointTypes, double[] relativePositions) {
this.json = new ManipulatorJson(jointTypes, relativePositions).toJsonString();
}

public String[] getFKAndJacobian() throws IOException {
Path tmpDir = Files.createTempDirectory(“VCPtmpDir”);
Path tmpFile = Files.createTempFile(tmpDir, “SymKine”, “.py”);

LOG.log(Level.FINE, “Created tmpDir {0}”, tmpDir);
LOG.log(Level.FINE, “Created tmpFile {0}”, tmpFile);

try (BufferedWriter bw = new BufferedWriter(new FileWriter(tmpFile.toFile()))) {
bw.write(getScript());
bw.flush();
}

ProcessBuilder pb = new ProcessBuilder(“python”, tmpFile.toString());
pb.redirectErrorStream(true);
Process p = pb.start();

String result = “”;
try (BufferedReader br = new BufferedReader(new InputStreamReader(p.getInputStream()))) {
String str;
while ((str = br.readLine()) != null) {
result += str;
}
}

Map msg = gson.fromJson(result, Map.class);

Map fk = gson.fromJson((String)msg.get(“FK”), Map.class);
Map j = gson.fromJson((String)msg.get(“Jacobian”), Map.class);

if (Files.deleteIfExists(tmpFile)) {
LOG.log(Level.FINE, “Deleted {0}”, tmpFile.toString());
}

if (Files.deleteIfExists(tmpDir)) {
LOG.log(Level.FINE, “Deleted {0}”, tmpDir.toString());
}

return new String[]{(String)fk.get(“mat”), (String)j.get(“mat”)};
}

private String getScript() {
return “import sys\n”
+ “import json\n”
+ “from sympy import *\n”
+ “\n”
+ “def matToStr(mat):\n”
+ ” s = \”\”\n”
+ ” for i in range(0,mat.rows):\n”
+ ” for j in range(0,mat.cols):\n”
+ ” s+=str(mat[i,j])\n”
+ ” if j != mat.cols-1:\n”
+ ” s+=’, ‘\n”
+ ” s+=’\\n’\n”
+ ” return s\n”
+ ” \n”
+ “class KinematicChain:\n”
+ ” \n”
+ ” def __init__(self):\n”
+ ” self.i = 1\n”
+ ” self.variables = []\n”
+ ” self.matrices = []\n”
+ ” \n”
+ ” def add(self, type, relPos):\n”
+ ” m = self.transMatrix(type, relPos);\n”
+ ” self.matrices.append(m)\n”
+ ” \n”
+ ” def fkMat(self):\n”
+ ” fk = eye(4)\n”
+ ” for mat in self.matrices:\n”
+ ” fk = fk * mat\n”
+ ” ans = simplify(fk)\n”
+ ” return ans\n”
+ ” \n”
+ ” def jacobian(self):\n”
+ ” fk = self.fkMat()\n”
+ ” f = Matrix([fk[0,3], fk[1,3], fk[2,3]])\n”
+ ” if (len(self.variables) < 1):\n" + " return eye(4)\n" + " else:\n" + " x = Matrix(self.variables)\n" + " ans = simplify(f.jacobian(x))\n" + " return ans\n" + " \n" + " def transMatrix(self, type, p):\n" + " if (type != \"FIXED\"):\n" + " s1 = \"a\" + str(self.i)\n" + " self.i += 1\n" + " a = symbols(s1)\n" + " self.variables.append(a)\n" + " \n" + " if (type == \"FIXED\"):\n" + " return Matrix([\n" + " [1, 0, 0, p[0]],\n" + " [0, 1, 0, p[1]],\n" + " [0, 0, 1, p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"RX\"):\n" + " return Matrix([\n" + " [1, 0, 0, p[0]],\n" + " [0, cos(a), -sin(a), p[1]],\n" + " [0, sin(a), cos(a), p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"RY\"):\n" + " return Matrix([\n" + " [cos(a), 0, sin(a), p[0]],\n" + " [0, 1, 0, p[1]],\n" + " [-sin(a), 0, cos(a), p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"RZ\"):\n" + " return Matrix([\n" + " [cos(a), -sin(a), 0, p[0]],\n" + " [sin(a), cos(a), 0, p[1]],\n" + " [0, 0, 1, p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"PX\"):\n" + " return Matrix([\n" + " [1, 0, 0, p[0] + a],\n" + " [0, 1, 0, p[1]],\n" + " [0, 0, 1, p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"PY\"):\n" + " return Matrix([\n" + " [1, 0, 0, p[0]],\n" + " [0, 1, 0, p[1] + a],\n" + " [0, 0, 1, p[2]],\n" + " [0, 0, 0, 1]])\n" + " elif (type == \"PZ\"):\n" + " return Matrix([\n" + " [1, 0, 0, p[0]],\n" + " [0, 1, 0, p[1]],\n" + " [0, 0, 1, p[2] + a],\n" + " [0, 0, 0, 1]])\n" + " else:\n" + " return eye(4)\n" + "\n" + "if __name__ == '__main__':\n" + " obj = json.loads(" + "\'" + json + "\'" + ")\n" + " \n" + " jointTypes = obj['jointTypes']\n" + " rel = obj['relativePositions']\n" + " \n" + " relativePositions = []\n" + " \n" + " x = 0\n" + " while x < len(rel):\n" + " relativePositions.append([rel[x], rel[x+1], rel[x+2]])\n" + " x += 3\n" + " \n" + " #print message\n" + " FK = KinematicChain()\n" + " for i in range(0, len(jointTypes)):\n" + " FK.add(jointTypes[i], relativePositions[i])\n" + " fk = FK.fkMat()\n" + " j = FK.jacobian()\n" + " print json.dumps({\n" + " 'FK':json.dumps({'rows':fk.rows, 'cols':fk.cols, 'mat':matToStr(fk)}),\n" + " 'Jacobian':json.dumps({'rows': j.rows, 'cols':j.cols, 'mat':matToStr(j)})})"; } private class ManipulatorJson { private final String[] jointTypes; private final double[] relativePositions; public ManipulatorJson(String[] jointTypes, double[] relativePositions) { this.jointTypes = jointTypes; this.relativePositions = relativePositions; } public String toJsonString() { return gson.toJson(this); } } } [/java] Standalone python script with example: [python] import sys import json from sympy import * def matToStr(mat): s = "" for i in range(0,mat.rows): for j in range(0,mat.cols): s+=str(mat[i,j]) if j != mat.cols-1: s+=', ' s+='\n' return s class KinematicChain: def __init__(self): self.i = 1 self.variables = [] self.matrices = [] def add(self, type, relPos): m = self.transMatrix(type, relPos); self.matrices.append(m) def fkMat(self): fk = eye(4) for mat in self.matrices: fk = fk * mat ans = simplify(fk) return ans def jacobian(self): fk = self.fkMat() f = Matrix([fk[0,3], fk[1,3], fk[2,3]]) if (len(self.variables) < 1): return eye(4) else: x = Matrix(self.variables) ans = simplify(f.jacobian(x)) return ans def transMatrix(self, type, p): if (type != "FIXED"): s1 = "a" + str(self.i) self.i += 1 a = symbols(s1) self.variables.append(a) if (type == "FIXED"): return Matrix([ [1, 0, 0, p[0]], [0, 1, 0, p[1]], [0, 0, 1, p[2]], [0, 0, 0, 1]]) elif (type == "RX"): return Matrix([ [1, 0, 0, p[0]], [0, cos(a), -sin(a), p[1]], [0, sin(a), cos(a), p[2]], [0, 0, 0, 1]]) elif (type == "RY"): return Matrix([ [cos(a), 0, sin(a), p[0]], [0, 1, 0, p[1]], [-sin(a), 0, cos(a), p[2]], [0, 0, 0, 1]]) elif (type == "RZ"): return Matrix([ [cos(a), -sin(a), 0, p[0]], [sin(a), cos(a), 0, p[1]], [0, 0, 1, p[2]], [0, 0, 0, 1]]) elif (type == "PX"): return Matrix([ [1, 0, 0, p[0] + a], [0, 1, 0, p[1]], [0, 0, 1, p[2]], [0, 0, 0, 1]]) elif (type == "PY"): return Matrix([ [1, 0, 0, p[0]], [0, 1, 0, p[1] + a], [0, 0, 1, p[2]], [0, 0, 0, 1]]) elif (type == "PZ"): return Matrix([ [1, 0, 0, p[0]], [0, 1, 0, p[1]], [0, 0, 1, p[2] + a], [0, 0, 0, 1]]) else: return eye(4) if __name__ == '__main__': obj = json.loads('{"jointTypes":["RY","RX","RX", "FIXED"],"relativePositions":[0, 0, 0, 0, 2.61, 0, 0, 0, 7.01, 0, 0, 3]}') jointTypes = obj['jointTypes'] rel = obj['relativePositions'] relativePositions = [] x = 0 while x < len(rel): relativePositions.append([rel[x], rel[x+1], rel[x+2]]) x += 3 FK = KinematicChain() for i in range(0, len(jointTypes)): FK.add(jointTypes[i], relativePositions[i]) fk = FK.fkMat() j = FK.jacobian() print json.dumps({ 'FK':json.dumps({'rows':fk.rows, 'cols':fk.cols, 'mat':matToStr(fk)}), 'Jacobian':json.dumps({'rows': j.rows, 'cols':j.cols, 'mat':matToStr(j)})}) [/python]

Leave a Reply

Your email address will not be published. Required fields are marked *