InputArmBase_copy copy of InputArmBase

addFileTest.groovy

//Your code here

DefaultDhSolver.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;

import com.neuronrobotics.kinematicschef.InverseKinematicsEngine
return InverseKinematicsEngine.instance
/*
return new DhInverseSolver() {
	
	@Override
	public double[] inverseKinematics(
		TransformNR target,
		double[] jointSpaceVector,
		DHChain chain
	) {
		ArrayList<DHLink> links = chain.getLinks();
		if (links.size() < 3) {
			return jointSpaceVector
		}
		
		// THis is the jacobian for the given configuration
		//Matrix jacobian =  chain.getJacobian(jointSpaceVector);
		Matrix taskSpacMatrix = target.getMatrixTransform();
		
		int linkNum = jointSpaceVector.length;
		double[] inv = new double[linkNum];

		// This is the d offset between the shoulder CoR and the wrist CoR
		// r is RED, d is GREEN
		double offsetFromShoulderCoRToWristCoR = links.get(1).getD() - links.get(2).getD();
		double shoulderHorizontalOffset = links.get(0).getR();

		// Length to the target from the origin in the XY plane
		double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(), 2) + Math.pow(target.getY(), 2));

		// Angle from link 0 to link 1
		double angleXYPlaneVect = Math.asin(target.getY() / lengthXYPlaneVect);
		
		double angleRectangleAdjustedXY = Math.asin(offsetFromShoulderCoRToWristCoR / lengthXYPlaneVect);
		double lengthRectangleAdjustedXY = lengthXYPlaneVect * Math.cos(angleRectangleAdjustedXY) - shoulderHorizontalOffset;
		
		double orientation = angleXYPlaneVect - angleRectangleAdjustedXY;
		if (Math.abs(Math.toDegrees(orientation)) < 0.01){
			orientation = 0;
		}

		// These become the wrist center
		double ySet = lengthRectangleAdjustedXY * Math.sin(orientation);
		double xSet = lengthRectangleAdjustedXY * Math.cos(orientation);
		double zSet = target.getZ() - links.get(0).getD();

		if (links.size() >= 6) {
			double tipAngulationSum = Math.toDegrees(
				links.get(1).getTheta() +
				links.get(2).getTheta() +
				links.get(5).getTheta()
			)
			
			if (tipAngulationSum == 90) {
				zSet += links.get(5).getD();
			} else {
				xSet -= links.get(5).getR() * Math.cos(orientation);
				ySet -= links.get(5).getR() * Math.sin(orientation);
			}
		} else if (links.size() >= 5) {
			double tipAngulationSum = Math.toDegrees(
				links.get(1).getTheta() +
				links.get(2).getTheta() +
				links.get(4).getTheta()
			)
			
			if (tipAngulationSum == 90) {
				zSet += links.get(4).getD();
			} else {
				xSet -= links.get(4).getR() * Math.cos(orientation);
				ySet -= links.get(4).getR() * Math.sin(orientation);
			}
		} else if (links.size() >= 4) {
			double tipAngulationSum = Math.toDegrees(
				links.get(1).getTheta() +
				links.get(2).getTheta() +
				links.get(3).getTheta()
			)
			
			if (tipAngulationSum == 90) {
				zSet += links.get(3).getR();
			} else {
				xSet -= links.get(3).getR() * Math.cos(orientation);
				ySet -= links.get(3).getR() * Math.sin(orientation);
			}
		}
		
		// Actual target for analytical solution is above the target minus the z offset
		TransformNR overGripper = new TransformNR(
			xSet, ySet, zSet,
			target.getRotation()
		);
		
		double l1 = links.get(1).getR();
		double l2 = links.get(2).getR();
		double l3 = links.get(3).getD();
		double l5 = links.get(5).getD();

		double lengthFromShoulderToWristCenter = Math.sqrt(
			xSet * xSet +
			ySet * ySet +
			zSet * zSet
		);

		if (lengthFromShoulderToWristCenter < 0) {
			def errorMsg = "lengthFromShoulderToWristCenter less than zero: " + lengthFromShoulderToWristCenter + "\n"
			print(errorMsg)
			throw new RuntimeException(errorMsg);
		} else if (lengthRectangleAdjustedXY < 0) {
			def errorMsg = "lengthRectangleAdjustedXY less than zero: " + lengthRectangleAdjustedXY + "\n"
			print(errorMsg)
			throw new RuntimeException(errorMsg);
		}
		
		//from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
		double a = l2;
		double b = l1;
		double c = lengthFromShoulderToWristCenter;
		double A = Math.acos((Math.pow(b, 2) + Math.pow(c, 2) - Math.pow(a, 2)) / (2.0 * b * c));
		double B = Math.acos((Math.pow(c, 2) + Math.pow(a, 2) - Math.pow(b, 2)) / (2.0 * a * c));
		double C = Math.PI - A - B; // rule of triangles
		double elevation = Math.asin(zSet / lengthFromShoulderToWristCenter);

		inv[0] = Math.toDegrees(orientation);
		inv[1] = -Math.toDegrees(A + elevation + links.get(1).getTheta());
		
		if ((int)links.get(1).getAlpha() == 180){
			inv[2] = Math.toDegrees(C) -
					180 - // interior angle of the triangle, map to external angle
					Math.toDegrees(links.get(2).getTheta());
		} else if((int)links.get(1).getAlpha() == 0){
			inv[2] = -Math.toDegrees(C) +
					Math.toDegrees(links.get(2).getTheta());
		}
		
		if (links.size() > 3) {
			// keep it parallel
			// the wrist twist will always be 0 for this model because we assume that 3 links won't have a twist
			inv[3] = -(inv[1] + inv[2]);
		} else if(links.size() > 4) {
			// keep the tool orientation parallel to the base
			// this link points in the same direction as the first link
			inv[4] = inv[0];
		}

		for (int i = 0; i < inv.length; i++){
			if (Math.abs(inv[i]) < 0.01){
				inv[i] = 0;
			}
		}

		for (int i = 0; i < linkNum; i++) {
			print(inv[i] + ", ")
		}
		return inv;
	}
};
*/

InputArmBase_copy.xml

<root>
<mobilebase>
	<cadEngine>
		<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
		<file>LinkedCadEngine.groovy</file>
	</cadEngine>
	<driveEngine>
		<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
		<file>WalkingDriveEngine.groovy</file>
	</driveEngine>

<name>InputArmBase_copy1</name>
<appendage>

<name>InputArm</name>
	<cadEngine>
		<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
		<file>LinkedCadEngine.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
		<file>DefaultDhSolver.groovy</file>
	</kinematics>
<link>
	<name>basePan</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>14</index>
	<scale>-0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>614.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>baseTilt</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>15</index>
	<scale>0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>526.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>elbow</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>20</index>
	<scale>-0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>148.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

	<DHParameters>
		<Delta>11.0</Delta>
		<Theta>90.0</Theta>
		<Radius>24.0</Radius>
		<Alpha>90.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>wrist1</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>9</index>
	<scale>-0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>458.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>wrist2</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>10</index>
	<scale>0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>603.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>wrist3</name>
	<deviceName>dyio1</deviceName>
	<type>dummy</type>
	<index>11</index>
	<scale>0.2200489</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>450.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

	<DHParameters>
		<Delta>25.0</Delta>
		<Theta>90.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>0.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>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>

</appendage>
<appendage>

<name>PUMA</name>
	<cadEngine>
		<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
		<file>LinkedCadEngine.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://github.com/madhephaestus/carl-the-hexapod.git</git>
		<file>DefaultDhSolver.groovy</file>
	</kinematics>
<link>
	<name>basePan</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>0</index>
	<scale>0.33</scale>
	<upperLimit>255.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>128.43283582089552</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>235</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>baseTilt</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>1</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>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

	<DHParameters>
		<Delta>14.9</Delta>
		<Theta>0.0</Theta>
		<Radius>43.2</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>elbow</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>2</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>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>wrist1</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>3</index>
	<scale>1.0</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>false</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

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

</link>
<link>
	<name>wrist2</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>4</index>
	<scale>1.0</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>false</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

	<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>5</index>
	<scale>1.0</scale>
	<upperLimit>100000.0</upperLimit>
	<lowerLimit>-100000.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>false</isLatch>
	<indexLatch>0</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>standardMicro</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>standardMicro1</id>
		</vitamin>

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

	<DHParameters>
		<Delta>5.6</Delta>
		<Theta>0.0</Theta>
		<Radius>0.0</Radius>
		<Alpha>0.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>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>

</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>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<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></centerOfMassFromCentroid>
	<imuFromCentroid>	<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></imuFromCentroid>

<vitamins>

</vitamins>

</mobilebase>

</root>

LinkedCadEngine.groovy

import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import javafx.scene.transform.Affine;
import com.neuronrobotics.bowlerstudio.physics.TransformFactory;

return new ICadGenerator(){

	private CSG reverseDHValues(CSG incoming,DHLink dh ){
		println "Reversing "+dh
		TransformNR step = new TransformNR(dh.DhStep(0))
		Transform move = TransformFactory.nrToCSG(step)
		return incoming.transformed(move)
	}
	
	private CSG moveDHValues(CSG incoming,DHLink dh ){
		TransformNR step = new TransformNR(dh.DhStep(0)).inverse()
		Transform move = TransformFactory.nrToCSG(step)
		return incoming.transformed(move)
		
	}
	
	@Override 
	public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
		ArrayList<DHLink> dhLinks = d.getChain().getLinks()
		DHLink dh = dhLinks.get(linkIndex)
		// Transform used by the UI to render the location of the object
		Affine manipulator = dh.getListener();

		CSG rLink =new Cube(dh.getRadius(),5,5).toCSG().toXMax()
		//rLink = moveDHValues(rLink, dh)
		rLink.setColor(javafx.scene.paint.Color.RED)
		
		CSG dLink = new Cube(5, 5, dh.getDelta()).toCSG().toZMin()
		dLink = moveDHValues(dLink, dh)
		dLink.setColor(javafx.scene.paint.Color.GREEN)
		
		ArrayList<CSG> allCad=[dLink, rLink];

		for(CSG cad:allCad){
			cad.setManipulator(manipulator)
		}
		//If you want you can add things here
		//allCad.add(myCSG);
		return allCad;
	}
	@Override 
	public ArrayList<CSG> generateBody(MobileBase b ) {
		println "Making Base "
		ArrayList<CSG> allCad=[]; //new Cube(10).toCSG()
		// Transform used by the UI to render the location of the object
		Affine manipulator = b.getRootListener();
		
		for(CSG cad:allCad){
			cad.setManipulator(manipulator)
		}
		//If you want you can add things here
		//allCad.add(myCSG);
		return allCad;
	}
};

WalkingDriveEngine.groovy

import java.time.Duration;

import java.util.ArrayList;

import javafx.application.Platform;

import org.reactfx.util.FxTimer;

import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.MobileBase;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.util.ThreadUtil;
import com.neuronrobotics.sdk.addons.kinematics.IDriveEngine;


double stepOverHeight=5;
long stepOverTime=200;
Double zLock=5;
Closure calcHome = { DHParameterKinematics leg -> 
		TransformNR h=leg.calcHome() 
 		TransformNR  legRoot= leg.getRobotToFiducialTransform()
		TransformNR tr = leg.forwardOffset(new TransformNR())
		tr.setZ(zLock)
		//Bambi-on-ice the legs a bit
		if(legRoot.getY()>0){
			//tr.translateY(-5)
		}else{
			//tr.translateY(5)
		}
		
		return h;

}
boolean usePhysicsToMove = true;
long stepCycleTime =5000
long walkingTimeout =stepCycleTime*2
int numStepCycleGroups = 2
double standardHeadTailAngle = -20
double staticPanOffset = 10
double coriolisGain = 1
boolean headStable = false
double maxBodyDisplacementPerStep = 20
double minBodyDisplacementPerStep = 15
def ar =  [stepOverHeight,
stepOverTime,
zLock,
calcHome,
usePhysicsToMove,
stepCycleTime,
numStepCycleGroups,
standardHeadTailAngle,
staticPanOffset,
coriolisGain,
headStable,
maxBodyDisplacementPerStep,
minBodyDisplacementPerStep,
	walkingTimeout]


return ScriptingEngine
          .gitScriptRun(
            "https://github.com/keionbis/SmallKat.git", // git location of the library
            "Bowler/DynamicWalking.groovy" , // file to load
            ar
         )