HephaestusWorkCell_copy copy of HephaestusWorkCell

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;

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 taskSpacMatrix = target.getMatrixTransform();
		
		int linkNum = jointSpaceVector.length;
		double [] inv = new double[linkNum];
		// this is an ad-hock kinematic model for d-h parameters and only works for specific configurations
		
		double d = links.get(1).getD()- links.get(2).getD();
		double r = links.get(0).getR();

		double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(),2)+Math.pow(target.getY(),2));
		double angleXYPlaneVect = Math.asin(target.getY()/lengthXYPlaneVect);
		
		double angleRectangleAdjustedXY =Math.asin(d/lengthXYPlaneVect);
		
		double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
		
		
		double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
		if(Math.abs(Math.toDegrees(orentation))<0.01){
			orentation=0;
		}
		double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
		double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
	
		
		double zSet = target.getZ() - links.get(0).getD();
		if(links.size()>4){
			zSet+=links.get(4).getD();
		}
		// Actual target for anylitical solution is above the target minus the z offset
		TransformNR overGripper = new TransformNR(
				xSet,
				ySet,
				zSet,
				target.getRotation());


		double l1 = links.get(1).getR();// First link length
		double l2 = links.get(2).getR();

		double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
		/*
		println ( "TO: "+target);
		println ( "Trangular TO: "+overGripper);
		println ( "lengthXYPlaneVect: "+lengthXYPlaneVect);
		println( "angleXYPlaneVect: "+Math.toDegrees(angleXYPlaneVect));
		println( "angleRectangleAdjustedXY: "+Math.toDegrees(angleRectangleAdjustedXY));
		println( "lengthRectangleAdjustedXY: "+lengthRectangleAdjustedXY);
		println( "r: "+r);
		println( "d: "+d);
		
		println( "x Correction: "+xSet);
		println( "y Correction: "+ySet);
		
		println( "Orentation: "+Math.toDegrees(orentation));
		println( "z: "+zSet);
	*/
		

		if (vect > l1+l2 ||  vect<0 ||lengthRectangleAdjustedXY<0 ) {
			throw new RuntimeException("Hypotenus too long: "+vect+" longer then "+l1+l2);
		}
		//from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
		double a=l2;
		double b=l1;
		double c=vect;
		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/vect);

/*
		println( "vect: "+vect);
		println( "A: "+Math.toDegrees(A));
		println( "elevation: "+Math.toDegrees(elevation));
		println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
		println( "l2 from l1: "+Math.toDegrees(C));
		*/
		inv[0] = Math.toDegrees(orentation);
		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());// offset for kinematics
		}
		if((int)links.get(1).getAlpha() ==0){
			inv[2] = -(Math.toDegrees(C))+Math.toDegrees(links.get(2).getTheta());// offset for kinematics
		}
		if(links.size()>3)
			inv[3] =-(inv[1] + inv[2]);// keep it parallell
			// We know the wrist twist will always be 0 for this model
		if(links.size()>4)
			inv[4] = inv[0];//keep the tool orentation paralell from the base
		
		for(int i=0;i<inv.length;i++){
			if(Math.abs(inv[i]) < 0.01){
				inv[i]=0;
			}
//			println( "Link#"+i+" is set to "+inv[i]);
		}
		int i=3;
		if(links.size()>3)
			i=5;
		//copy over remaining links so they do not move
		for(;i<inv.length && i<jointSpaceVector.length ;i++){
			inv[i]=jointSpaceVector[i];
		}
		return inv;
	}
};

HephaestusWorkCell_copy.xml

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

<name>HephaestusWorkCell_copy</name>
<appendage>

<name>HephaestustArm</name>
	<cadEngine>
		<git>https://gist.github.com/bd0d8cb9d659abaa503edb96af30dda7.git</git>
		<file>seaLinkGen.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://gist.github.com/bd0d8cb9d659abaa503edb96af30dda7.git</git>
		<file>DefaultDhSolver.groovy</file>
	</kinematics>
<link>
	<name>basePan</name>
	<deviceName>dyio</deviceName>
	<type>servo-rotory</type>
	<index>11</index>
	<scale>0.1</scale>
	<upperLimit>2550.0</upperLimit>
	<lowerLimit>-2550.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>235</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>towerProMG91</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>tproSG90_1</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>90.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>10</index>
	<scale>0.1</scale>
	<upperLimit>2550.0</upperLimit>
	<lowerLimit>-2550.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>128</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>towerProMG91</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>tproSG90_1</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>166.0923641158874</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

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

	<vitamins>
		<vitamin>
			<name>electroMechanical</name>
			<type>hobbyServo</type>
			<id>towerProMG91</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>tproSG90_1</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>90.0</Theta>
		<Radius>200.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>110.0</z>
	<rotw>1.0</rotw>
	<rotx>-2.7755575615628907E-17</rotx>
	<roty>-2.775557561562889E-17</roty>
	<rotz>8.326672684688674E-17</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>

seaLinkGen.groovy

import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import eu.mihosoft.vrl.v3d.parametrics.*;
import com.neuronrobotics.bowlerstudio.vitamins.Vitamins;
import javafx.scene.paint.Color;
import eu.mihosoft.vrl.v3d.Transform;
import com.neuronrobotics.bowlerstudio.physics.TransformFactory;
import eu.mihosoft.vrl.v3d.Transform;
import javafx.scene.transform.Affine;


Vitamins.setGitRepoDatabase("https://github.com/madhephaestus/Hardware-Dimensions.git")
CSGDatabase.clear()
return new ICadGenerator(){
	boolean showVitamins = true
	boolean showRightPrintedParts = true
	boolean showLeftPrintedParts = true
	
	HashMap<String , HashMap<String,ArrayList<CSG>>> map =  new HashMap<>();
	HashMap<String,ArrayList<CSG>> bodyMap =  new HashMap<>();
	LengthParameter thickness 				= new LengthParameter("Material Thickness",5.1,[10,1])
	LengthParameter printerOffset 			= new LengthParameter("printerOffset",0.5,[1.2,0])
	StringParameter boltSizeParam 			= new StringParameter("Bolt Size","M5",Vitamins.listVitaminSizes("capScrew"))
	StringParameter bearingSizeParam 			= new StringParameter("Encoder Board Bearing","R8-60355K505",Vitamins.listVitaminSizes("ballBearing"))
	StringParameter gearAParam 			 	= new StringParameter("Gear A","HS36T",Vitamins.listVitaminSizes("vexGear"))
	StringParameter gearBParam 				= new StringParameter("Gear B","HS84T",Vitamins.listVitaminSizes("vexGear"))
	//StringParameter gearBParam 				= new StringParameter("Gear B","HS60T",Vitamins.listVitaminSizes("vexGear"))
	//StringParameter gearBParam 				= new StringParameter("Gear B","HS84T",Vitamins.listVitaminSizes("vexGear"))
	//StringParameter gearBParam 				= new StringParameter("Gear B","HS36T",Vitamins.listVitaminSizes("vexGear"))
	//StringParameter gearBParam 				= new StringParameter("Gear B","HS12T",Vitamins.listVitaminSizes("vexGear"))
     //String springType = "Torsion-9271K133"
     //HashMap<String, Object>  springData = Vitamins.getConfiguration("torsionSpring",springType)
	HashMap<String, Object>  bearingData = Vitamins.getConfiguration("ballBearing",bearingSizeParam.getStrValue())			
	HashMap<String, Object>  boltMeasurments = Vitamins.getConfiguration( "capScrew",boltSizeParam.getStrValue())
	HashMap<String, Object>  nutMeasurments = Vitamins.getConfiguration( "nut",boltSizeParam.getStrValue())
	HashMap<String, Object>  gearAMeasurments = Vitamins.getConfiguration( "vexGear",gearAParam.getStrValue())
	HashMap<String, Object>  gearBMeasurments = Vitamins.getConfiguration( "vexGear",gearBParam.getStrValue())
	double workcellSize = 760
	double cameraLocation =(workcellSize-20)/2
	TransformNR cameraLocationNR = new TransformNR(cameraLocation,0,cameraLocation,new RotationNR(0,-180,-45))
	Transform cameraLocationCSG =TransformFactory.nrToCSG(cameraLocationNR)
	double gearDistance  = (gearAMeasurments.diameter/2)+(gearBMeasurments.diameter/2) +2.75
	//println boltMeasurments.toString() +" and "+nutMeasurments.toString()
	double springHeight = 26
	
	double boltDimeMeasurment = boltMeasurments.get("outerDiameter")
	double boltHeadThickness =boltMeasurments.headHeight
	double boltHeadKeepaway = boltMeasurments.headDiameter*1.25
	double nutDimeMeasurment = nutMeasurments.get("width")
	double nutThickMeasurment = nutMeasurments.get("height")
	//pin https://www.mcmaster.com/#98381a514/=16s6brg
	// PN: 98381a514		
	double pinRadius = ((3/16)*25.4+printerOffset.getMM())/2
	double pinLength = (1.5*25.4) + (printerOffset.getMM()*2)
	// bushing
	//https://www.mcmaster.com/#6391k123/=16s6one
	//double brassBearingRadius = ((1/4)*25.4+printerOffset.getMM())/2
	double brassBearingRadius = pinRadius
	double brassBearingLength = (5/8)*25.4
	
	double linkMaterialThickness = pinLength/2-3
	// #8x 1-5/8 wood screw
	double screwDrillHole=((boltMeasurments.outerDiameter-1.2)+printerOffset.getMM())/2
	double screwthreadKeepAway= (boltMeasurments.outerDiameter+printerOffset.getMM()+0.25)/2
	double screwHeadKeepaway =boltMeasurments.headDiameter/2 + printerOffset.getMM()
	double screwLength = 200 //1-5/8 
	
	//Encoder Cap mesurments
	double encoderCapRodRadius =7
	double cornerRadius = 1
	double capPinSpacing = gearAMeasurments.diameter*0.75+encoderCapRodRadius
	double pinOffset  =gearBMeasurments.diameter/2+encoderCapRodRadius*2
	double mountPlatePinAngle 	=Math.toDegrees(Math.atan2(capPinSpacing,pinOffset))
	double bearingDiameter = bearingData.outerDiameter
	double encoderToEncoderDistance = (springHeight/2)+linkMaterialThickness
	
	
	
	DHParameterKinematics neck=null;
	CSG gearA = Vitamins.get( "vexGear",gearAParam.getStrValue())
				.movey(-gearDistance)
	CSG gearB = Vitamins.get( "vexGear",gearBParam.getStrValue());
	CSG bolt = Vitamins.get( "capScrew",boltSizeParam.getStrValue());
	//CSG spring = Vitamins.get( "torsionSpring",springType)	
	//			.movez(-springHeight/2)
	CSG previousServo = null;
	CSG previousEncoder = null
	CSG encoderCapCache=null
	CSG encoderServoPlate=null;
	HashMap<Double,CSG> springLinkBlockLocal=new HashMap<Double,CSG>();
	HashMap<Double,ArrayList<CSG>> sidePlateLocal=new HashMap<Double,ArrayList<CSG>>();
	
	
       	
	CSG encoderSimple = (CSG) ScriptingEngine
					 .gitScriptRun(
            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
            "encoderBoard.groovy" , // file to load
            null// no parameters (see next tutorial)
            )
      List<CSG> nucleo = (List<CSG>) ScriptingEngine
					 .gitScriptRun(
			            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
			            "nucleo-144.groovy" , // file to load
			            null
			            ) 
     CSG encoderKeepaway = (CSG) ScriptingEngine
					 .gitScriptRun(
			            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
			            "encoderBoard.groovy" , // file to load
			            [10]// create a keepaway version
			            )
			            .movez(-encoderToEncoderDistance)
     CSG encoder =   encoderSimple .movez(-encoderToEncoderDistance)
	CSG screwHole = new Cylinder(screwDrillHole,screwDrillHole,screwLength,(int)8).toCSG() // a one line Cylinder
					.toZMax()
     CSG screwHoleKeepaway = new Cylinder(screwthreadKeepAway,screwthreadKeepAway,50,(int)8).toCSG() // a one line Cylinder
     					.toZMin()
	CSG screwHead= new Cylinder(boltHeadKeepaway/2,boltHeadKeepaway/2,screwLength*2,(int)8).toCSG() // a one line Cylinder
						.movez(screwHoleKeepaway.getMaxZ())

	CSG screwTotal = screwHead.union([screwHoleKeepaway,screwHole])
					//.movez()
     CSG screwSet =screwTotal
					.movex(-pinOffset)
					.rotz(mountPlatePinAngle)
					.union(screwTotal
						.movex(-pinOffset)
						.rotz(-mountPlatePinAngle))
	double centerHoleRad=(10.3+printerOffset.getMM())/2
	CSG centerHole =new Cylinder(centerHoleRad,centerHoleRad,20,(int)30)
							.toCSG()
							.movez(-10)
							.roty(90)
	double screwCenterLine = boltHeadKeepaway
	double encoderBearingHeight = encoderSimple.getMaxZ()
	double topPlateOffset = encoderToEncoderDistance*2-encoderBearingHeight*2
	double centerLinkToBearingTop = encoderToEncoderDistance-encoderBearingHeight
	double topOfGearToCenter = (centerLinkToBearingTop-gearBMeasurments.height)
	double totalSpringLength = 47.5
	double drivenLinkThickness =centerLinkToBearingTop+topOfGearToCenter
	double drivenLinkWidth = screwCenterLine*1.5+encoderCapRodRadius
	double drivenLinkX = totalSpringLength+encoderCapRodRadius
	double legLength = totalSpringLength
	double drivenLinkXFromCenter = legLength+encoderCapRodRadius
	double loadCellBoltCenter = -(40.0-5.0-(15.0/2))
	double thirdarmBoltBackSetDistance = 16.0
	CSG armScrews = screwTotal
					.movey(-screwCenterLine+screwHeadKeepaway)
					.union(screwTotal
						.movey(screwCenterLine-screwHeadKeepaway))
					.union(screwTotal
							.union(new Cylinder(boltHeadKeepaway/2,boltHeadKeepaway/2,screwLength*2,(int)8).toCSG() 
									.movez(27-drivenLinkThickness)
							)
							.movez(centerLinkToBearingTop-encoderBearingHeight)
							.movex(-thirdarmBoltBackSetDistance)
							.roty(90)
							)
					.roty(-90)
					//.movex(legLength+encoderCapRodRadius/2)
					.movez(centerLinkToBearingTop-screwHeadKeepaway*1.5)
	CSG LoadCellScrews = screwTotal
					.movey(-screwCenterLine+screwHeadKeepaway)
					.union(screwTotal
						.movey(screwCenterLine-screwHeadKeepaway))
					.movex(loadCellBoltCenter)
					.movez(-topOfGearToCenter)				
	CSG loadBearingPinBearing =new Cylinder(	brassBearingRadius,
										brassBearingRadius,
										drivenLinkThickness+encoderBearingHeight,
										(int)30).toCSG() 
						.toZMin()
						.movez(-pinLength/2)
	CSG loadBearingPin =new Cylinder(pinRadius,pinRadius,pinLength,(int)30).toCSG() 
						.movez(-pinLength/2)
						.union(	loadBearingPinBearing)	
	CSG loadCell = (CSG) ScriptingEngine
					 .gitScriptRun(
            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
            "loadCell.groovy" , // file to load
            null// no parameters (see next tutorial)
            )
            .rotx(-90)
            .movey(-drivenLinkWidth/2)
	/**
	 * Gets the all dh chains.
	 *
	 * @return the all dh chains
	 */
	public ArrayList<DHParameterKinematics> getLimbDHChains(MobileBase base) {
		ArrayList<DHParameterKinematics> copy = new ArrayList<DHParameterKinematics>();
		for(DHParameterKinematics l:base.getLegs()){
			copy.add(l);	
		}
		for(DHParameterKinematics l:base.getAppendages() ){
			copy.add(l);	
		}
		return copy;
	}
	@Override 
	public ArrayList<CSG> generateBody(MobileBase base ){
		ArrayList<CSG> attachmentParts = new ArrayList<CSG>()
		double maxz = 0.001
		
		for(DHParameterKinematics l:getLimbDHChains(base)){
			double thisZ = l.getRobotToFiducialTransform().getZ()
			if(thisZ>maxz)
				maxz=thisZ
		}
		DHParameterKinematics sourceLimb=base.getAppendages() .get(0)
	
		TransformNR step = sourceLimb.calcHome();
		
		Transform tipatHome = TransformFactory.nrToCSG(step)
		
		LinkConfiguration conf = sourceLimb.getLinkConfiguration(0);
		ArrayList<DHLink> dhLinks=sourceLimb.getChain().getLinks();
		DHLink dh = dhLinks.get(0);
		HashMap<String, Object> servoMeasurments = Vitamins.getConfiguration(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())
		LengthParameter tailLength		= new LengthParameter("Cable Cut Out Length",maxz,[500,0.01])
		tailLength.setMM(maxz)
		CSG servoReference=   Vitamins.get(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())
								.rotz(180+Math.toDegrees(dh.getTheta()))
		
		double servoNub = servoMeasurments.tipOfShaftToBottomOfFlange - servoMeasurments.bottomOfFlangeToTopOfBody
		double servoTop = servoReference.getMaxZ()-servoNub
		double topLevel = maxz -(springHeight/2)-linkMaterialThickness +encoderBearingHeight
		double servoPlane = topLevel - encoderBearingHeight
		double basexLength = gearDistance + servoMeasurments.servoThinDimentionThickness/2
		//double baseyLength = servoMeasurments.flangeLongDimention 
		double servoCentering = servoMeasurments.flangeLongDimention -servoMeasurments.shaftToShortSideFlandgeEdge
		double minimumWidth = (capPinSpacing-encoderCapRodRadius-cornerRadius)
		if(servoCentering<minimumWidth)
			servoCentering=minimumWidth
		double baseyLength = servoCentering*2
		double keepAwayDistance =10
		
		servoReference=servoReference
					.movez(servoPlane)
					.movex(-gearDistance)
		CSG encoderBaseKeepaway = (CSG) ScriptingEngine
					 .gitScriptRun(
			            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
			            "encoderBoard.groovy" , // file to load
			            [topLevel+5]// create a keepaway version
			            )
			            .movez(servoPlane)
		double encoderKeepawayDistance= encoderBaseKeepaway.getMaxX()
		double sidePlateclearenceHeight = 10*24.5
		/*
		for (int i=1;i<3;i++){
			servoReference=servoReference
						.union(servoReference
								.movez(servoMeasurments.flangeThickness*i))
		}
		*/
		CSG keepawayBottomX = new Cube(basexLength+(keepAwayDistance*3)+encoderKeepawayDistance,
							baseyLength-(keepAwayDistance*2),
							keepAwayDistance)
						.toCSG()
						.toZMin()
		CSG keepawayBottomY = new Cube(basexLength+encoderKeepawayDistance-(keepAwayDistance*2),
							baseyLength+(keepAwayDistance*3),
							keepAwayDistance)
							.toCSG()
							.toZMin()
		CSG baseShapeCutter = new Cube(basexLength+(keepAwayDistance*2)+encoderKeepawayDistance,
							baseyLength+(keepAwayDistance*2),
							topLevel)
						.toCSG()		
						.toXMax()
						.toYMin()
						.toZMin()
						.movex(keepAwayDistance+encoderKeepawayDistance)
						
		CSG baseShape = new RoundedCube(basexLength+(keepAwayDistance*2)+encoderKeepawayDistance,
							baseyLength+(keepAwayDistance*2),
							topLevel)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toZMin()
						.difference([keepawayBottomY,keepawayBottomX])
		/*
		CSG sidePlate = new Cube( basexLength+(keepAwayDistance*2)+encoderKeepawayDistance,
							1.0,
							topLevel)
							.toCSG()
							.movey((baseyLength+(keepAwayDistance*2))/2)
							.toZMin()
		sidePlate=sidePlate
				.union(sidePlate.movez(sidePlateclearenceHeight))
				.union(sidePlate.movez(-20*25.4))
				.hull()
		CSG strapSlot  = 	new Cube(5,
								(baseyLength+(keepAwayDistance*2))*2,
								25.4
								).toCSG()	
								.toXMax()
								.movex((basexLength+(keepAwayDistance*2)+encoderKeepawayDistance)/2 - 10)
		CSG strapSlots = strapSlot
		for(int i=1;i<13;i++){
			strapSlots=strapSlots.union(strapSlot.movez(-38*i))
		}
		sidePlate=sidePlate.difference(strapSlots)	
		*/							
		CSG screws = screwSet
					.movez(topLevel)
						
		CSG screwAcross = screwTotal.rotx(90)
						.movez(topLevel/2)

		screwAcross=screwAcross.union(
				screwAcross
					.movez(topLevel/2-(keepAwayDistance/2+screwHeadKeepaway)+2)
					.movex(baseShape.getMaxX()-(keepAwayDistance/2+screwHeadKeepaway)+3)
			).union(
				screwAcross
					
					.movex(baseShape.getMinX()+(keepAwayDistance/2+screwHeadKeepaway))
			).union(
				screwAcross
					.movez(topLevel/2-(keepAwayDistance/2+screwHeadKeepaway))
					.movex(screwHeadKeepaway)
			)	
		CSG bottomScrews = screwTotal.rotx(180)
		
		CSG bottomScrewSet =bottomScrews
					.movex(baseShape.getMaxX()-(keepAwayDistance/2+screwHeadKeepaway))
					.movey(baseShape.getMaxY()-(keepAwayDistance/2+screwHeadKeepaway))
					.union(
							bottomScrews
								.movex(baseShape.getMinX()+(keepAwayDistance/2+screwHeadKeepaway))
								.movey(baseShape.getMaxY()-(keepAwayDistance/2+screwHeadKeepaway))
						)
						.union(
							bottomScrews
								.movex(baseShape.getMinX()+(keepAwayDistance/2+screwHeadKeepaway))
								.movey(baseShape.getMinY()+(keepAwayDistance/2+screwHeadKeepaway))
						)
						.union(
							bottomScrews
								.movex(baseShape.getMaxX()-(keepAwayDistance/2+screwHeadKeepaway))
								.movey(baseShape.getMinY()+(keepAwayDistance/2+screwHeadKeepaway))
						)				
		baseShape = baseShape.difference([bottomScrewSet,screwAcross])	
		CSG nucleoBoard = nucleo.get(0)
		double baseBackSet = 	-baseShape.getMaxX()+keepAwayDistance+encoderKeepawayDistance
		double spacing = 75

		double nucleoMountPlacement = spacing+baseBackSet+nucleoBoard.getMaxX()
		CSG boxCut = new Cube(workcellSize/2+nucleoMountPlacement,workcellSize,thickness.getMM()*2).toCSG()
					.toXMax()
					.movex(workcellSize/2)

		
		CSG tipHole =new Cylinder(10,10,thickness.getMM()*3,(int)90).toCSG()
					.movez(-thickness.getMM()*1.5)
					.roty(90)
					.transformed(tipatHome)
		
		CSG footing =new Cylinder(workcellSize/2,workcellSize/2,thickness.getMM(),(int)90).toCSG()
		def cameraParts = getCameraMount()
		CSG basePlate = footing
						.toZMax()
						.difference(nucleoBoard
									.rotz(90)
							.movey(nucleoBoard.getMaxX()/2)
							.movex(-spacing-nucleoBoard.getMaxX()/2)
							.movex(baseBackSet)
						)
						.difference(	bottomScrewSet.movex(baseBackSet))
						.intersect(boxCut)
						.difference(tipHole)
						.difference(cameraParts)
		
		
						/*
		CSG sidePlateA=sidePlate
				.difference(screwAcross)
				.difference(screwAcross.movez(sidePlateclearenceHeight))
				.movex(baseBackSet)
		CSG sidePlateB = sidePlateA
						.movey(-(baseyLength+(keepAwayDistance*2)))
						*/
		baseShape = baseShape				
				.toYMin()
				.movey(-servoCentering-keepAwayDistance)
				.movex(baseBackSet)
				.difference([encoderBaseKeepaway,servoReference.toolOffset(printerOffset.getMM()),screws])

				
		CSG baseCap = getEncoderCap()
					.movez(topLevel)
		
			
		CSG baseShapeA = baseShape.difference(baseShapeCutter)
						.setColor(javafx.scene.paint.Color.CYAN);
		CSG baseShapeB = baseShape.intersect(baseShapeCutter)
						.setColor(javafx.scene.paint.Color.GREEN);
		baseCap.setColor(javafx.scene.paint.Color.LIGHTBLUE);			
		baseCap.setManufacturing({ toMfg ->
				return toMfg
						.roty(180)
						.toZMin()
			})
		baseShapeB.setManufacturing({ toMfg ->
				return toMfg
						.rotx(90)
						.toZMin()
			})
		baseShapeA.setManufacturing({ toMfg ->
				return toMfg
						.rotx(-90)
						.toZMin()
			})
		/*
		sidePlateA.setManufacturing({ toMfg ->
				return toMfg
						.rotx(-90)
						.toZMin()
						.toXMin()
						.movex(basePlate.getMaxX())
			})
		sidePlateB.setManufacturing({ toMfg ->
			return toMfg
					.rotx(-90)
					.toZMin()
					.toXMin()
					.movex(basePlate.getMaxX())
		})
		*/
		
		attachmentParts.addAll(cameraParts)
		//if(showRightPrintedParts)attachmentParts.add(sidePlateA)
		//if(showRightPrintedParts)attachmentParts.add(sidePlateB)
		if(showLeftPrintedParts)attachmentParts.add(baseShapeA)
		if(showRightPrintedParts)attachmentParts.add(baseShapeB)
		if(showRightPrintedParts)attachmentParts.add(basePlate)
		if(showLeftPrintedParts)attachmentParts.add(baseCap)
		return attachmentParts;
	}
	@Override 
	public ArrayList<CSG> generateCad(DHParameterKinematics sourceLimb, int linkIndex) {
		//return new ArrayList<CSG>()
		//Creating the horn
		ArrayList<DHLink> dhLinks=sourceLimb.getChain().getLinks();
		String legStr = sourceLimb.getXml()
		LinkConfiguration conf = sourceLimb.getLinkConfiguration(linkIndex);
		LinkConfiguration nextLink=null;
		if(linkIndex<dhLinks.size()-1){
			nextLink=sourceLimb.getLinkConfiguration(linkIndex+1);
		}
		
		String linkStr =conf.getXml()
		ArrayList<CSG> csg = null;
		HashMap<String,ArrayList<CSG>> legmap=null;
		if(map.get(legStr)==null){
			map.put(legStr, new HashMap<String,ArrayList<CSG>>())	
			// now load the cad and return it. 
		}
		legmap=map.get(legStr)
		if(legmap.get(linkStr) == null ){
			legmap.put(linkStr,new ArrayList<CSG>())
		}
		csg = legmap.get(linkStr)
		if(csg.size()>linkIndex){
			// this link is cached
			println "This link is cached"
			return csg;
		}
		
		DHLink dh = dhLinks.get(linkIndex);
		CSG springBlockPartRaw=springBlock(drivenLinkThickness)
		CSG springBlockPart =springBlockPartRaw
								.rotz(-Math.toDegrees(dh.getTheta()))
		CSG springBlockPartGear = springBlock(gearBMeasurments.height)
								.rotx(180)
								.rotz(-Math.toDegrees(dh.getTheta()))
		// creating the servo
		
	
	
		CSG springMoved = moveDHValues(loadCell
									.rotz(-Math.toDegrees(dh.getTheta()))
									.movez(springBlockPart.getMinZ())
									//.rotz(linkIndex==0?180:0)
									,dh)
		CSG tmpMyGear = gearB
					.rotz(5)
					.movez(-centerLinkToBearingTop)
		tmpMyGear = 	tmpMyGear	
					.difference(springBlockPartGear
								.intersect(tmpMyGear)
								.hull()
					)
					.union(springBlockPartGear)
		CSG loadCellBolts = moveDHValues(LoadCellScrews
							.rotz(-Math.toDegrees(dh.getTheta()))
								,dh)	
		CSG myGearB = moveDHValues(tmpMyGear
								.difference(loadBearingPin)
								,dh)
					.difference(loadCellBolts)
					.setColor(javafx.scene.paint.Color.LIGHTGREEN);
		CSG myPin = moveDHValues(loadBearingPin,dh)
		
		CSG myspringBlockPart = moveDHValues(springBlockPart
		
										.difference(loadBearingPin)
										,dh)	
							.difference(loadCellBolts)
							.setColor(javafx.scene.paint.Color.BROWN);
		CSG handMountPart=null;
		CSG myArmScrews = moveDHValues(armScrews
											.rotz(-Math.toDegrees(dh.getTheta()))
											,dh)
					   .movex(springBlockPartRaw.getMaxX())
		if(linkIndex<dhLinks.size()-1){
			HashMap<String, Object> shaftmap = Vitamins.getConfiguration(nextLink.getShaftType(),nextLink.getShaftSize())
			HashMap<String, Object> servoMeasurments = Vitamins.getConfiguration(nextLink.getElectroMechanicalType(),nextLink.getElectroMechanicalSize())
			//println conf.getShaftType() +" "+conf.getShaftSize()+" "+shaftmap
			double hornOffset = 	shaftmap.get("hornThickness")	
			double servoNub = servoMeasurments.tipOfShaftToBottomOfFlange - servoMeasurments.bottomOfFlangeToTopOfBody
		
			CSG servoReference=   Vitamins.get(nextLink.getElectroMechanicalType(),nextLink.getElectroMechanicalSize())
			.transformed(new Transform().rotZ(90))
			
			double servoTop = servoReference.getMaxZ()-servoNub
							
			CSG horn = Vitamins.get(nextLink.getShaftType(),nextLink.getShaftSize())	
						.rotx(180)
						.movez(hornOffset)
						.movey(-gearDistance)
			servoReference=servoReference
				.toZMax()
				.movez(servoNub-centerLinkToBearingTop)			
				.movey(-gearDistance)
				.rotz(90+Math.toDegrees(dh.getTheta()))
			CSG myGearA = gearA.clone()	
			for(int i=0;i<5;i++){
				myGearA=myGearA
					.difference(horn
								.movez(hornOffset*i)
								)
			}
			myGearA = myGearA
						.rotz(90+Math.toDegrees(dh.getTheta()))
						.movez(-centerLinkToBearingTop)	
						.setColor(javafx.scene.paint.Color.BLUE);
			if(linkIndex==0){
				CSG baseServo =servoReference.clone()
				CSG secondLinkServo =servoReference.clone()
				CSG baseForceSenseEncoder = encoder
										.rotz(180-Math.toDegrees(dh.getTheta()))
										.rotx(180)
				CSG baseEncoder = encoder.clone()
				
				previousEncoder = baseEncoder
				previousServo = baseServo
				CSG baseMyGearA = myGearA.clone()
								.setColor(javafx.scene.paint.Color.BLUE);
				baseMyGearA.setManufacturing({ toMfg ->
					return toMfg
							.toXMin()
							.toZMin()
				})
				add(csg,baseMyGearA,sourceLimb.getRootListener())
				if(showVitamins)add(csg,baseServo,sourceLimb.getRootListener())
				if(showVitamins)add(csg,baseEncoder,sourceLimb.getRootListener())
				if(showVitamins)add(csg,baseForceSenseEncoder,sourceLimb.getRootListener())
			}
			println "Link Hardware: using from index "+
					(linkIndex+1)+
					" "+nextLink.getElectroMechanicalSize() +
					" "+nextLink.getShaftSize()
					
			CSG forceSenseEncoder = encoder
								.rotz(180-Math.toDegrees(dh.getTheta()))
								.rotx(180)
			CSG baseEncoderCap = getEncoderCap()
							.movez(-centerLinkToBearingTop)
							
			CSG thirdPlusLinkServo =servoReference.clone()
			CSG linkEncoder = encoder.clone()
								.rotz(-Math.toDegrees(dh.getTheta()))
			ArrayList<CSG> esp = getLinkSideEncoderCap(nextLink)
			double linkCconnectorOffset = drivenLinkXFromCenter-(encoderCapRodRadius+bearingDiameter)/2
			def end = [-dh.getR()+linkCconnectorOffset,dh.getD()*0.98,0]
			def controlOne = [0,end.get(1)*1.1,0]
			def controlTwo = [end.get(0),0,end.get(2)*1.1]

			CSG connectorArmCross = new RoundedCube(cornerRadius*2,
											encoderCapRodRadius+bearingDiameter -cornerRadius-5,
											 encoderBearingHeight)
					.cornerRadius(cornerRadius)
					.toCSG()
					
			def ribs = Extrude.moveBezier(	connectorArmCross,
					controlOne, // Control point one
					controlTwo, // Control point two
					end ,// Endpoint
					
					5
					)
			CSG mountLug = new RoundedCube(encoderCapRodRadius+bearingDiameter,drivenLinkWidth,drivenLinkThickness)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toXMax()
						.toZMax()
						.movez(centerLinkToBearingTop)
						.movex(drivenLinkX)
						
			mountLug = moveDHValues(mountLug.rotz(-Math.toDegrees(dh.getTheta()))
											,dh)	
			mountLug=mountLug.union(
				mountLug
					.toZMax()
					.movez(centerLinkToBearingTop+encoderBearingHeight)
				)	
			CSG supportRib = ribs.get(ribs.size()-2)
							.movez(encoderBearingHeight -cornerRadius*2)
							//.union(ribs.get(ribs.size()-2))
							.union(ribs.get(ribs.size()-1))
							.toZMin()
							.movez(centerLinkToBearingTop-encoderBearingHeight+ cornerRadius*2)
							.union(mountLug)
							.hull()
			def linkParts = Extrude.bezier(	connectorArmCross,
					controlOne, // Control point one
					controlTwo, // Control point two
					end ,// Endpoint
					10
					)
			print "\r\nUnioning link..."
			CSG linkSection = CSG.unionAll(linkParts)
						//
						.toZMin()
						.movez(centerLinkToBearingTop )
						
			linkSection = 	linkSection
							.union(supportRib)
							.movex(5)// offset to avoid hitting pervious link
							.movey(-2)// offset to avoid hitting pervious link
			double xSize= (-linkSection.getMinX()+linkSection.getMaxX())
			double ySize= (-linkSection.getMinY()+linkSection.getMaxY())
			double zSize= (-linkSection.getMinZ()+linkSection.getMaxZ())
			CSG bottomCut = new Cube(xSize*2, ySize*2,zSize).toCSG()
							.toZMax()
							.toXMin()
							.movez(springBlockPart.getMinZ()+cornerRadius)
			bottomCut=moveDHValues(bottomCut
											.rotz(-Math.toDegrees(dh.getTheta()))
											,dh)	
			CSG otherEncoder = linkEncoder.rotx(180)			
			linkSection = 	linkSection				
							.difference(myspringBlockPart
									.intersect(linkSection)
									.hull()
									.toolOffset(printerOffset.getMM()))
							.difference(baseEncoderCap//.hull()
										.intersect(linkSection)
										.hull()
										)
							.difference(otherEncoder.hull()
										.intersect(linkSection)
										.hull())			
							.difference(myArmScrews)
							.difference(springMoved)
							.difference(bottomCut)
			print "Done\r\n"
			baseEncoderCap=baseEncoderCap.union(linkSection)
			baseEncoderCap.setColor(javafx.scene.paint.Color.LIGHTBLUE);
			esp.get(0).setColor(javafx.scene.paint.Color.ORANGE);
			if(esp.size()>1)esp.get(1).setColor(javafx.scene.paint.Color.WHITE);
			previousEncoder = linkEncoder
			previousServo = thirdPlusLinkServo
			myGearA.setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toZMin()
			})
			baseEncoderCap.setManufacturing({ toMfg ->
				return toMfg
						.roty(180)
						.toXMin()
						.toZMin()
			})
			esp.get(0).setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toZMin()
			})
			if(esp.size()>1)
			esp.get(1).setManufacturing({ toMfg ->
				return toMfg
						.roty(180)
						.toXMin()
						.toZMin()
			})
			if(showRightPrintedParts)add(csg,myGearA,dh.getListener())
			if(showVitamins)add(csg,thirdPlusLinkServo,dh.getListener())
			if(showVitamins)add(csg,linkEncoder,dh.getListener())
			if(showVitamins)add(csg,otherEncoder,dh.getListener())
			if(showRightPrintedParts)add(csg,esp.get(0),dh.getListener())
			if(esp.size()>1)if(showLeftPrintedParts)add(csg,esp.get(1),dh.getListener())
			if(showLeftPrintedParts)add(csg,baseEncoderCap,dh.getListener())
			
		}else{
			// load the end of limb
			// Target point
			handMountPart = handMount()
			CSG tipCalibrationPart= tipCalibration()
			
			double plateThickenss = (-handMountPart.getMinX()+handMountPart.getMaxX())
			double platewidth  = (-handMountPart.getMinY()+handMountPart.getMaxY())
			double plateOffset = Math.abs(handMountPart.getMaxX())

			double springBlockWidth =(-myspringBlockPart.getMinY()+myspringBlockPart.getMaxY())
			double linkLength = dh.getR() -plateOffset-plateThickenss -drivenLinkXFromCenter+8
			CSG connectorArmCross = new RoundedCube(plateThickenss,platewidth,drivenLinkThickness)
					.cornerRadius(cornerRadius)
					.toCSG()
					.toXMin()
			CSG screwHead= new Cylinder(boltHeadKeepaway/1.5,boltHeadKeepaway/1.5,drivenLinkThickness,(int)20).toCSG()
							.movez(-drivenLinkThickness/2)
							.movex(-thirdarmBoltBackSetDistance/1.5)
			CSG section = connectorArmCross
					.union(connectorArmCross
							.movex(linkLength )
					)
					.union(screwHead)
					.hull()
					.toXMax()
					.toZMin()
					.movex(-plateOffset-plateThickenss+cornerRadius*2)
			handMountPart=handMountPart
						.union(section)
			handMountPart=handMountPart
							.difference(myspringBlockPart
									.intersect(handMountPart)
									.hull()
									.toolOffset(printerOffset.getMM()*2))
							.difference(myArmScrews,springMoved.toolOffset(2)	)
							
			tipCalibrationPart.setColor(javafx.scene.paint.Color.PINK);
			handMountPart.setColor(javafx.scene.paint.Color.WHITE);
			
			tipCalibrationPart.setManufacturing({ toMfg ->
				return toMfg
					.rotx(90)
					.roty(90)
					.toZMin()
					.toXMin()
			})
			handMountPart.setManufacturing({ toMfg ->
				return toMfg
					.rotx(90)
					.toXMin()
					.toZMin()
			})
			if(showRightPrintedParts)add(csg,tipCalibrationPart,dh.getListener())
			if(showLeftPrintedParts)add(csg,handMountPart,dh.getListener())
		}
		
		myGearB.setManufacturing({ toMfg ->
			return reverseDHValues(toMfg,dh)
					.roty(180)
					.toZMin()
					.rotz(-Math.toDegrees(dh.getTheta()))
					.toXMin()
		})
		myspringBlockPart.setManufacturing({ toMfg ->
			return reverseDHValues(toMfg,dh)
					.toZMin()
					.rotz(-Math.toDegrees(dh.getTheta()))
					.toXMin()
		})
		if(showLeftPrintedParts)add(csg,myspringBlockPart,dh.getListener())
		if(showVitamins)add(csg,myPin,dh.getListener())
		if(showRightPrintedParts)add(csg,myGearB,dh.getListener())
		if(showVitamins)add(csg,springMoved,dh.getListener())
		return csg;
	}
	private CSG tipCalibration(){
		CSG plate = handMount()
		double plateThickenss = (-plate.getMinX()+plate.getMaxX())
		double platewidth  = (-plate.getMinY()+plate.getMaxY())
		plate=plate.movex(plateThickenss)
		double sphereSize = 20
		//centerHoleRad
		CSG centerHole =new Cylinder(1,1,Math.abs(plate.getMaxX())+20,(int)30)
							.toCSG()
							.roty(-90)
							.movex(- Math.abs(plate.getMaxX()))    
		CSG cableTieHole =   new Cylinder(1,sphereSize/2,sphereSize/2,(int)30)
							.toCSG()
							.roty(-90)
							//.movex(- Math.abs(plate.getMaxX())) 
		CSG pyramid = new Cylinder(	platewidth/2, // Radius at the bottom
                      		14, // Radius at the top
                      		Math.abs(plate.getMaxX()), // Height
                      		(int)6 //resolution
                      		).toCSG()//convert to CSG to display 
                      		.roty(-90)
                      		.movex(- Math.abs(plate.getMaxX()))      
          CSG tipSphere = new Sphere(sphereSize/2,(int)40,(int)40).toCSG()            			 
		plate=plate.union([pyramid,tipSphere])
				.difference(centerHole)
				.difference(cableTieHole)
		return plate
	}
	private CSG handMount(){
		
		CSG mountPlate = new RoundedCube(8,30,70)
					.cornerRadius(cornerRadius)
					.toCSG()

		HashMap<String, Object>  boltData = Vitamins.getConfiguration( "capScrew","M3")								
		CSG handBolt = Vitamins.get( "capScrew","M3");
		double boltCenterLong = 55.4+boltData.outerDiameter
		double boltShortDistance = 17.2+boltData.outerDiameter
		mountPlate=mountPlate
					.toXMin()
					.difference(centerHole)
					.difference(handBolt
								.roty(90)
								.movez(boltCenterLong /2)
								.movey(boltShortDistance/2)
					)
					.difference(handBolt
								.roty(90)
								.movez(-boltCenterLong /2)
								.movey(boltShortDistance/2)
					)
					.difference(handBolt
								.roty(90)
								.movez(-boltCenterLong /2)
								.movey(-boltShortDistance/2)
					)
					.difference(handBolt
								.roty(90)
								.movez(boltCenterLong /2)
								.movey(-boltShortDistance/2)
					)
		// offset the claw mount so the tip is at the kinematic center
		mountPlate=mountPlate.movex(-54.4)
		return mountPlate
	}

	private CSG springBlock(double thickness){
		if(springLinkBlockLocal.get(thickness)!=null)
			return springLinkBlockLocal.get(thickness).clone()
		CSG linkBlank = new RoundedCube(drivenLinkX,drivenLinkWidth,thickness)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toXMin()
						.toZMax()
						.movez(centerLinkToBearingTop)
						.movex(-(bearingData.innerDiameter/2)-2)
		double loadCellNub = loadCell.getMaxZ()
		if(loadCellNub>thickness)
			loadCellNub=thickness
		CSG linkBackBlank = new RoundedCube(25,drivenLinkWidth,loadCellNub)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toZMin()
						.movez(linkBlank.getMinZ())
						.movex(loadCellBoltCenter)
						
		CSG springCut = loadCell
						.movez(linkBackBlank.getMinZ())
		//for(int i=1;i<springData.numOfCoils;i++){
		//	springCut=springCut.union(springCut.movez(-springData.wireDiameter*i))
		//}
		double magnetPinDiameter = bearingData.innerDiameter/2
		CSG magnetPin = new Cylinder(magnetPinDiameter,magnetPinDiameter,encoderBearingHeight+2,(int)30).toCSG()
						.movez(linkBlank.getMaxZ())
		
		linkBlank =linkBlank
					.union(magnetPin)
					.difference(encoder.rotx(180))
					.difference(armScrews.movex(linkBlank.getMaxX()))
					.union(linkBackBlank)
					.difference([springCut])
					
					
					
		springLinkBlockLocal.put(thickness,linkBlank)
		return linkBlank
	}
	private CSG getEncoderCap(){
		if(encoderCapCache!=null)
			return encoderCapCache
		
		double bearingHolder = bearingDiameter/2 + encoderCapRodRadius/2
		double SidePlateThickness = encoderBearingHeight 
		CSG pin  =new Cylinder(encoderCapRodRadius,encoderCapRodRadius,SidePlateThickness,(int)30).toCSG()
					.movex(-pinOffset)
		double mountPlatePinAngle 	=Math.toDegrees(Math.atan2(capPinSpacing,pinOffset))
		
		CSG capPinSet=pin
					.rotz(mountPlatePinAngle)
					.union(pin.rotz(-mountPlatePinAngle))
		
		CSG center  =new Cylinder(bearingHolder,bearingHolder,SidePlateThickness,(int)30).toCSG()
		CSG pinColumn =pin .union(pin
									.movez(topPlateOffset))
								.hull() 
		CSG pivot = center.movex(-encoderCapRodRadius*4)
		CSG bottomBlock = capPinSet
						.union(pivot)
						.hull()
						.union(center.union(pivot).hull())
						//.toZMax()
						.movez(topPlateOffset)
						.difference(encoderKeepaway
								.rotx(180)
								.movez(encoderToEncoderDistance-encoderBearingHeight)
								
								)
						.union(pinColumn.rotz(mountPlatePinAngle))
						.union(pinColumn.rotz(-mountPlatePinAngle))
						.difference(screwSet
									)
		encoderCapCache = bottomBlock
		return encoderCapCache
	}	
	private ArrayList<CSG> getLinkSideEncoderCap(LinkConfiguration conf ){
		if(sidePlateLocal.get(conf.getXml())!=null)
			return sidePlateLocal.get(conf.getXml()).collect{
				it.clone()
			}
		double SidePlateThickness = encoderBearingHeight 
		HashMap<String, Object> shaftmap = Vitamins.getConfiguration(conf.getShaftType(),conf.getShaftSize())
		HashMap<String, Object> servoMeasurments = Vitamins.getConfiguration(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())
		
		double hornOffset = 	shaftmap.get("hornThickness")	
		double servoNub = servoMeasurments.tipOfShaftToBottomOfFlange - servoMeasurments.bottomOfFlangeToTopOfBody
		// creating the servo
		CSG servoReference=   Vitamins.get(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())
			.transformed(new Transform().rotZ(90))
			.toZMax()
			.movez(servoNub-centerLinkToBearingTop)			
			.movey(-gearDistance)
			.rotz(90)
			
		double servoTop = servoReference.getMaxZ()-servoNub
		double bearingHolder = bearingDiameter/2 + encoderCapRodRadius
		
		CSG pin  =new Cylinder(encoderCapRodRadius,encoderCapRodRadius,SidePlateThickness,(int)30).toCSG()
					.movex(-pinOffset)
		double mountPlatePinAngle 	=Math.toDegrees(Math.atan2(capPinSpacing,pinOffset))
		
		CSG capPinSet=pin
					.rotz(mountPlatePinAngle)
					.union(pin.rotz(-mountPlatePinAngle))
		
		CSG center  =new Cylinder(bearingHolder,bearingHolder,SidePlateThickness,(int)30).toCSG()
		
		CSG baseShape = new Cube(servoMeasurments.servoThinDimentionThickness+encoderCapRodRadius,
							servoMeasurments.flangeLongDimention+encoderCapRodRadius,
							SidePlateThickness)
						
						.toCSG()
						.toZMin()
						.toYMin()
						.movey(-servoMeasurments.shaftToShortSideFlandgeEdge-encoderCapRodRadius/2)
						.movex(-gearDistance)
		CSG bottomBlock = capPinSet.union([center,baseShape]).hull()
						.toZMax()
						.movez(encoderBearingHeight-encoderToEncoderDistance)
						.difference(encoderKeepaway)
						.difference(screwSet.movez(-encoderBearingHeight))
						.minkowskiDifference(servoReference,printerOffset.getMM())
						.minkowskiDifference(servoReference.movez(-2),printerOffset.getMM())
		double plateThickness = (-bottomBlock.getMinZ()+bottomBlock.getMaxZ())
		CSG boundingBox = new Cube(   (-bottomBlock.getMinX()+bottomBlock.getMaxX()),
								(-bottomBlock.getMinY()+bottomBlock.getMaxY()),
								plateThickness)
								.toCSG()
								.toXMax()
								.movex(bottomBlock.getMaxX())
								.toYMax()
								.movey(bottomBlock.getMaxY())
								.toZMax()
								.movez(bottomBlock.getMaxZ())
								.movez(- bearingData.width)
		CSG lowerbottomBlock=bottomBlock.difference(boundingBox)
		CSG upperbottomBlock = bottomBlock.intersect(boundingBox)						
		sidePlateLocal.put(conf.getXml(),[bottomBlock]) 
		return sidePlateLocal.get(conf.getXml())
	}
	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)
		
	}

	private ArrayList<CSG> getCameraMount(){
		//cameraLocationCSG
		
		double cameraMountSize = 38+5+thickness.getMM()
		Log.enableSystemPrint(true)
		println "Camera at "+cameraLocationNR
		double cameraBolt = (workcellSize-cameraMountSize)/2
		CSG camerMount = new Cube(	cameraMountSize+5,
								cameraMountSize+5,
								thickness.getMM()).toCSG()
								.toZMax()
								.transformed(cameraLocationCSG)
		CSG camerMountLug = new Cube(	cameraMountSize,
								thickness.getMM(),
								30).toCSG()
								.toYMax()
								.movey(cameraMountSize/2)
								.toZMin()
		CSG notch = new Cube(	thickness.getMM()).toCSG()
								.toZMax()
								.toYMax()		
								.movey(cameraMountSize/2)								
													
		CSG topLug=	camerMountLug.transformed(cameraLocationCSG)
		CSG baseLug = camerMountLug
						.rotz(180)
						.toXMax()
						.movex(workcellSize/2)
		CSG midLug = camerMountLug
						.rotz(180)
						.toXMin()
						.movex(camerMount.getMaxX()+20)
						.movez(cameraBolt)
		CSG notches =  notch
					.movex(10)
					.union(	notch
							.movex(-10)	)
		CSG bottomNotches = notches
						.rotx(180)
						.movex(cameraBolt)	
						.toZMax()
		CSG topNotches = notches.transformed(cameraLocationCSG)	
		CSG  nut= Vitamins.get( "lockNut",boltSizeParam.getStrValue());
		nut = nut.movey(thickness.getMM())
				.union(nut.movey(-thickness.getMM()))
				.hull()
				.movez(thickness.getMM())
		//boltSizeParam
		CSG boltCutout = new Cube(boltMeasurments.outerDiameter,
							thickness.getMM(),
							30-thickness.getMM()).toCSG()
							.toZMin()
							.union(new Cylinder(boltMeasurments.outerDiameter/2,
											boltMeasurments.outerDiameter/2,
											thickness.getMM(),(int)30).toCSG()
											.toZMax(),
											nut
							)
								
		boltCutout=boltCutout.movey(	-(cameraMountSize-thickness.getMM())/2				)
				.union(boltCutout.movey(	(cameraMountSize-thickness.getMM())/2				))
		CSG topBolts = boltCutout.transformed(cameraLocationCSG)
		CSG bottomBolts = boltCutout.movex(cameraBolt)
		CSG bracketA = topLug
					.union(midLug)
					.hull()
					.union(
						baseLug
						.union(midLug)
						.hull()
						,bottomNotches,topNotches)
					.difference(bottomBolts,topBolts)
		
		CSG bracketB = bracketA
						.toYMax()
						.movey(cameraMountSize/2)
		camerMount=camerMount.difference(topBolts,bracketA,bracketB)
		//return [bottomNotches]
		return [camerMount,bracketA,bracketB,bottomBolts]
	}

	private add(ArrayList<CSG> csg ,CSG object, Affine dh ){
		object.setManipulator(dh);
		csg.add(object);
		BowlerStudioController.addCsg(object);
	}
}

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=100;
Double zLock=0;
Closure calcHome = { DHParameterKinematics leg -> 
		TransformNR h=leg.calcHome() 

		TransformNR tr = leg.forwardOffset(new TransformNR())
		tr.setZ(zLock)
		
		return h;

}
boolean usePhysicsToMove = true;


return ScriptingEngine
          .gitScriptRun(
            "https://github.com/madhephaestus/carl-the-hexapod.git", // git location of the library
            "GenericWalkingEngine.groovy" , // file to load
            [stepOverHeight,stepOverTime,zLock,calcHome,usePhysicsToMove]
         )