Trobot Arm

DiBlasi.groovy

import java.util.ArrayList;

import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;

return new DhInverseSolver() {
	
	@Override
	public double[] inverseKinematics(TransformNR target,
			double[] jointSpaceVector,DHChain chain ) {
		ArrayList<DHLink> links = chain.getLinks();

		// This is the jacobian for the given configuration
		Matrix jacobian =  chain.getJacobian(jointSpaceVector);
		Matrix taskSpaceMatrix = target.getMatrixTransform();

		// Print dimensions of matrices.
  		println "Jacobian Rows: "+jacobian.getArray().length
        println "Jacobian Columns: "+jacobian.getArray()[1].length
  		println "Task Space Rows: "+taskSpaceMatrix.getArray().length
        println "Task Space Columns: "+taskSpaceMatrix.getArray()[1].length

        // Perform first half of Pseudo Jacobian calculations.
		int linkNum = jointSpaceVector.length;
		double [] inv = new double[linkNum];

		Matrix Jt
    	Matrix Jstore
    	Matrix Jp

    	Jt = jacobian.transpose()
    	Jstore = jacobian.times(Jt)
    	Jstore = Jstore.inverse()
    	Jp = Jt.times(Jstore)

		// Generate and fill 6x1 joint space matrix.
		x = target.getX()
        y = target.getY()
        z = target.getZ()
        RotationNR rot = target.getRotation();
        t = rot.getRotationTilt()
        e = rot.getRotationElevation()
        a = rot.getRotationAzimuth()

        double[][] vals = {{x.},{y.},{z.},{t.},{e.},{a.}};
        Matrix testMatrix = new Matrix(vals);

       	println "Test Rows: "+testMatrix.getArray().length
        println "Test Columns: "+testMatrix.getArray()[1].length


        testMatrix.set(0, 0, x)
        testMatrix.set(1, 0, y)
        testMatrix.set(2, 0, z)
        testMatrix.set(3, 0, t)
        testMatrix.set(4, 0, e)
        testMatrix.set(5, 0, a)
    	println"Before"

        // Complete Pseudo Jacobian calculations.
    	Jstore = Jp.times(testMatrix)

    	println"Success"


    	inv[0] = Jstore.get(0, 0)
    	inv[1] = Jstore.get(1, 0)
    	inv[2] = Jstore.get(2, 0)
    	inv[3] = Jstore.get(3, 0)
    	inv[4] = Jstore.get(4, 0)
    	inv[5] = Jstore.get(5, 0)

    	println inv

		return inv;
	}
};

TrobotLinks.xml

<root>
<mobilebase>

<driveType>none</driveType>
	<cadEngine>
		<gist>bcb4760a449190206170</gist>
		<file>ThreeDPrintCad.groovy</file>
	</cadEngine>
	<driveEngine>
		<gist>bcb4760a449190206170</gist>
		<file>WalkingDriveEngine.groovy</file>
	</driveEngine>

<name>TrobotArmGroup</name>
<appendage>

<name>TrobotArm</name>
	<cadEngine>
		<gist>bcb4760a449190206170</gist>
		<file>ThreeDPrintCad.groovy</file>
	</cadEngine>
	<kinematics>
		<gist>3b31eec7e2db7c50a1e5</gist>
		<file>DiBlasi.groovy</file>
	</kinematics>
<link>
	<name>basePan</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>11</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>235.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>235</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>13.0</Delta>
		<Theta>0.0</Theta>
		<Radius>32.0</Radius>
		<Alpha>-90.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>baseTilt</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>10</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>128.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>128</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>25.0</Delta>
		<Theta>-90.0</Theta>
		<Radius>93.0</Radius>
		<Alpha>180.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>elbow</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>9</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>121.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>121</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>11.0</Delta>
		<Theta>-90.0</Theta>
		<Radius>131.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>wrist2</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>7</index>
	<scale>-0.313</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>200.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>175</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>0.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>90.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>wrist3</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>6</index>
	<scale>0.3125</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>255.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>255</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<DHParameters>
		<Delta>98.0</Delta>
		<Theta>0.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>-90.0</Alpha>
	</DHParameters>

</link>

<ZframeToRAS
>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</ZframeToRAS>

<baseToZframe>
	<x>200.0</x>
	<y>200.0</y>
	<z>0.0</z>
	<rotw>0.7071067811865476</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>-0.7071067811865475</rotz>
</baseToZframe>

</appendage>

<ZframeToRAS>
	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</ZframeToRAS>

<baseToZframe>
	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz>
</baseToZframe>

</mobilebase>

</root>