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();
		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];
		if(linkNum<3){
			for(int i=i;i<linkNum;i++){
				inv[i]=jointSpaceVector[i]+0.0001
			}
		}
		// 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()==5){
			double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
								links.get(2).getTheta()+
								links.get(4).getTheta())
			//println "Tip angulation orentation = "+tipAngulationSum
			if(tipAngulationSum==90)
				zSet+=links.get(4).getD();
			else{
				double tipySet = links.get(4).getR()*Math.sin(orentation);
				double tipxSet = links.get(4).getR()*Math.cos(orentation);
				//println "5 links back Setting tip x="+tipxSet+" y="+tipySet
				xSet-=tipxSet
				ySet-=tipySet
			}
		}
		if(links.size()==4){
			double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
								links.get(2).getTheta()+
								links.get(3).getTheta())
			//println "Tip angulation orentation = "+tipAngulationSum
			if(tipAngulationSum==90)
				zSet+=links.get(3).getR();
			else{
				double tipySet = links.get(3).getR()*Math.sin(orentation);
				double tipxSet = links.get(3).getR()*Math.cos(orentation);
				//println "4 links back Setting tip x="+tipxSet+" y="+tipySet
				xSet-=tipxSet
				ySet-=tipySet
			}
		}
		// 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/65ac76aeb898d2c00867b7b8397367e9.git</git>
		<file>seaLinkGen.groovy</file>
	</cadEngine>
	<driveEngine>
		<git>https://gist.github.com/65ac76aeb898d2c00867b7b8397367e9.git</git>
		<file>WalkingDriveEngine.groovy</file>
	</driveEngine>

<name>HephaestusWorkCell_copy</name>
<appendage>

<name>HephaestustArm</name>
	<cadEngine>
		<git>https://gist.github.com/65ac76aeb898d2c00867b7b8397367e9.git</git>
		<file>seaLinkGen.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://gist.github.com/65ac76aeb898d2c00867b7b8397367e9.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>hv5932mg</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>hv6214mg_6</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>135.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>hv5932mg</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>hv6214mg_6</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>175.0</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>hv5932mg</id>
		</vitamin>
		<vitamin>
			<name>shaft</name>
			<type>hobbyServoHorn</type>
			<id>hv6214mg_6</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>169.28</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>65.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 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()
ICadGenerator c= new ICadGenerator(){
	double centerOfRobotToBackEdgeOfBoard = 222.357	
	int linkResolution = 6
	double boardX = 530*2
	double boardY = 330*2
	boolean showVitamins =false
	boolean showRightPrintedParts = true
	boolean showLeftPrintedParts = true
	int[] version = com.neuronrobotics.javacad.JavaCadBuildInfo.getBuildInfo();
	HashMap<String , HashMap<String,ArrayList<CSG>>> map =  new HashMap<>();
	HashMap<String,ArrayList<CSG>> bodyMap =  new HashMap<>();
	LengthParameter thickness 				= new LengthParameter("Material Thickness",6.1,[10,1])
	LengthParameter ballRadius = new LengthParameter("ballRadius",(1.1*25.4)/2,[20,0.001])
	LengthParameter ballCenter = new LengthParameter("ballCenter",30.72,[20,0.001])
	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 = 700
	double cameraLocation =(workcellSize-20)/2
	TransformNR cameraLocationNR = new TransformNR(cameraLocation+20,0,cameraLocation+20,new RotationNR(0,-180,-35))
	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 motorBackSetDistance =5
	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/#98381a516/=19n8j9z
	// PN: 98381A516		
	double pinRadius = ((3/16)*25.4+printerOffset.getMM())/2
	double pinExtraDepth = 1.0/2.0*25.4
	double pinLength = (1.5*25.4)+pinExtraDepth + (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-(pinExtraDepth/2)
	// #8x 1-5/8 wood screw
	double screwDrillHole=((boltMeasurments.outerDiameter-1.2)+printerOffset.getMM())/2
	double screwthreadKeepAway= (boltMeasurments.outerDiameter+(printerOffset.getMM()*2))/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 washerThickness = 2.0
	double washerOd = 17
	double washerId = 12.75
	double encoderToEncoderDistance = (springHeight/2)+linkMaterialThickness + (washerThickness*2)
	
	CSG washerInner =new Cylinder(washerId/2,washerId/2,washerThickness,(int)30).toCSG() // a one line Cylinder
	CSG washerOuter      =new Cylinder(washerOd/2,washerOd/2,washerThickness,(int)30).toCSG() // a one line Cylinder
	CSG washer = 		washerOuter.difference(washerInner)
	CSG washerWithKeepaway = washerOuter
							.difference(washerInner
										.makeKeepaway(printerOffset.getMM()*1.5
										))
	
	DHParameterKinematics neck=null;
	CSG  tmpNut= Vitamins.get( "lockNut",boltSizeParam.getStrValue())
					.rotz(30)
	
	
	CSG gearA = Vitamins.get( "vexGear",gearAParam.getStrValue())
				.movex(-gearDistance)
	CSG gearB = Vitamins.get( "vexGear",gearBParam.getStrValue());
	CSG bolt = Vitamins.get( "capScrew",boltSizeParam.getStrValue());
	CSG gearStandoff = new Cylinder(gearA.getMaxY(),gearA.getMaxY(),motorBackSetDistance+washerThickness,20).toCSG()
						.toZMax()
						.movex(-gearDistance)
	CSG gearKeepaway = gearStandoff.toolOffset(1).getBoundingBox()
	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>>();
	
	
     double rectParam = 0.1*25.4
	CSG part = new Cube(rectParam,rectParam,2*rectParam).toCSG()
	def loadCellCutoutLocal = CSG.unionAll( Extrude.bezier(	part,
					[(double)20.0,(double)0.0,(double)0.0], // Control point one
					[(double)20.0,(double)0.0,(double)25.0], // Control point two
					[(double)0.0,(double)0.0,(double)(20.0+(3.0*rectParam)+4.0)], // Endpoint
					(int)10)
					).movex(-21.5)
					 .movez(-2.0-rectParam)
				
	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
			            )
			            .union(loadCellCutoutLocal)
			            .movez(-encoderToEncoderDistance)
     CSG encoder1 =   encoderSimple.union(loadCellCutoutLocal).movez(-encoderToEncoderDistance)
	CSG screwHole = new Cylinder(screwDrillHole,screwDrillHole,screwLength,(int)8).toCSG() // a one line Cylinder
					.toZMax()
     CSG screwHoleKeepaway = new Cylinder(screwthreadKeepAway,screwthreadKeepAway,100+(washerThickness*4),(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 screwChampher = new Cylinder(screwDrillHole,screwthreadKeepAway,printerOffset.getMM()*4,(int)8).toCSG() // a one line Cylinder
					.toZMax()
	CSG screwTotal = CSG.unionAll([screwHead,screwChampher,screwHoleKeepaway,screwHole])
					//.movez()
     CSG screwSet =screwTotal
					.movex(-pinOffset)
					.rotz(mountPlatePinAngle)
					.union(screwTotal
						.movex(-pinOffset)
						.rotz(-mountPlatePinAngle))
					//.movez(-encoderSimple.getMaxZ())
	double centerHoleRad=(20)/2
	CSG centerHole =new Cylinder(centerHoleRad,centerHoleRad,20,(int)30)
							.toCSG()
							.movez(-10)
							.roty(90)
	double loadCellwidth = 12.7
	double screwOffsetFromSides = 5
	double screwCenterLine = boltHeadKeepaway+loadCellwidth-screwOffsetFromSides
	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-(washerThickness*2)
	double drivenLinkWidth = screwCenterLine*1.5+encoderCapRodRadius+screwOffsetFromSides
	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	LockNutKeepaway = tmpNut.movey(thickness.getMM())
				.union(tmpNut.movey(-thickness.getMM()))
				.hull()
	CSG	LockNutCentered = LockNutKeepaway.movey(thickness.getMM())	
	double nutDriverRadius = (0.75*25.4)/2
	double nutInsetDistance =tmpNut.getMaxZ();
	CSG nutDriver = new Cylinder(nutDriverRadius,nutInsetDistance*2).toCSG()
	CSG nutAndDriverKeepaway = nutDriver.union(tmpNut.movez(-1))
	
	CSG wrenchKeepaway=new Cube(20,40,6).toCSG()
					.toYMax()
					.movey(3)
					.toZMax()
					.movez(printerOffset.getMM())
					.union(tmpNut
							.makeKeepaway(printerOffset.getMM())
							.rotx(180)	
							.movez(1)
							)
	CSG screwWithNut = screwTotal.union(LockNutCentered.makeKeepaway(printerOffset.getMM())
									.rotx(180)
									.union(wrenchKeepaway)
									.rotz(90)
									)
									.movez(-58)
	double thirdPinStandoff =37								
	CSG armScrews = screwWithNut.rotz(-45)
					.movey(-screwCenterLine+screwHeadKeepaway)
					.union(screwWithNut.rotz(45)
						.movey(screwCenterLine-screwHeadKeepaway))
					.union(screwTotal
							.union(new Cylinder(boltHeadKeepaway/2,boltHeadKeepaway/2,screwLength*2,(int)8).toCSG() 
									.movez(thirdPinStandoff-drivenLinkThickness)
							)
							.movez(centerLinkToBearingTop-encoderBearingHeight)
							.movex(-thirdarmBoltBackSetDistance)
							.roty(90)
							)
					.union(screwTotal
							.movez((-centerLinkToBearingTop-encoderBearingHeight)*2)
							.union(new Cylinder(boltHeadKeepaway/2,boltHeadKeepaway/2,screwLength*2,(int)8).toCSG() 
									.movez(thirdPinStandoff-drivenLinkThickness+centerLinkToBearingTop-encoderBearingHeight)
							)
							.movex(-thirdarmBoltBackSetDistance+22)
							.roty(90)
							)
					.roty(-90)
					//.movex(legLength+encoderCapRodRadius/2)
					.movez(centerLinkToBearingTop-screwHeadKeepaway*1.5)
	// Loading the gear bolts for the load cell
	double distanceToTopOfGear = topOfGearToCenter
	CSG gearScrew =screwTotal
					.union(nutAndDriverKeepaway.roty(180))
					.movez(-gearBMeasurments.height+washerThickness+nutInsetDistance)
	CSG LoadCellScrews = gearScrew
					.movey(-screwCenterLine+screwHeadKeepaway)
					.union(gearScrew
						.movey(screwCenterLine-screwHeadKeepaway))
					.movex(loadCellBoltCenter)
					.movez(-distanceToTopOfGear+2)		
									
	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)
            .movez(1)
            
     CSG standoffBLock=null
     ArrayList<CSG> manipulationParts = []
     CSG manipulationBall=null
	/**
	 * 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())
		double totalFlangLen = (servoMeasurments.flangeLongDimention-servoMeasurments.servoThickDimentionThickness)/2
		double shaftToShortSideFlandgeEdge = servoMeasurments.shaftToShortSideDistance+totalFlangLen
		
		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()))
		//						.movez(-motorBackSetDistance)
		
		double servoNub = servoMeasurments.tipOfShaftToBottomOfFlange - servoMeasurments.bottomOfFlangeToTopOfBody
		//double servoTop = servoReference.getMaxZ()-servoNub
		double topLevel = maxz -(springHeight/2)-linkMaterialThickness +encoderBearingHeight-(washerThickness*2)
		double servoPlane = topLevel - servoMeasurments.bottomOfFlangeToTopOfBody
		double servoEncoderPlane = topLevel - encoderBearingHeight
		double basexLength = gearDistance + servoMeasurments.servoThinDimentionThickness/2
		//double baseyLength = servoMeasurments.flangeLongDimention 
		double servoCentering = servoMeasurments.flangeLongDimention -shaftToShortSideFlandgeEdge
		double minimumWidth = (capPinSpacing-encoderCapRodRadius-cornerRadius)
		if(servoCentering<minimumWidth)
			servoCentering=minimumWidth
		double baseyLength = servoCentering*2
		double keepAwayDistance =10
		CSG gearHole = gearKeepaway
					.movez(topLevel+washerThickness)
		//servoReference=servoReference
		//			.movez(servoPlane)
		//			.movex(-gearDistance)

		CSG baseServo = Vitamins.get(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())
				.rotz(180)
				.movez(maxz-centerLinkToBearingTop-encoderBearingHeight)		
				.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(servoEncoderPlane)
		double encoderKeepawayDistance= 20
		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])
		/*
		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
						.movez(-topLevel)
						.rotx(90)
						.movez(topLevel/2)

		screwAcross=CSG.unionAll([
			//screwAcross,// middle bolt
			screwAcross.movez(topLevel/2-(keepAwayDistance/2+screwHeadKeepaway))// Frontmost bolt
					 .movex(baseShape.getMaxX()-(keepAwayDistance/2+screwHeadKeepaway)-7-printerOffset.getMM()),
			//screwAcross.movex(baseShape.getMinX()+(keepAwayDistance/2+screwHeadKeepaway)), Back most bolt
			screwAcross.movez(topLevel/2-(keepAwayDistance/2+screwHeadKeepaway))
						.movex(screwHeadKeepaway)			
		])
		CSG bottomScrews = screwTotal.rotx(180)
						.union(tmpNut
							.toZMax()
							.movez(-10)
							.union(tmpNut)
							.hull()
							.makeKeepaway(printerOffset.getMM())
							)
		// Originally calculated, fixed to make sure maunfactured parts mesh
		//Bottom Bolt hole pattern x= 97.78700180053711 y = 77.5 inset = 9.75
		//double inset = (keepAwayDistance/2+screwHeadKeepaway)
		//double screwX = baseShape.toXMin().getMaxX()-(inset*2)
		//double screwY = baseShape.toYMin().getMaxY()-(inset*2)
		double inset =  9.75
		double screwX = 97.78700180053711
		double screwY = 77.5	
		
		double distanceFromCenterToBolt = 	baseShape.getMaxX() - inset		
		println "Bottom Bolt hole pattern x total= "+screwX+
		" y total= "+screwY+
		" inset from edge= "+inset+
		" Center to forward Robot Bolts=" +(distanceFromCenterToBolt/2)+
		" center distance to calibration hole ="+tipatHome.getX()
		
		CSG bottomScrewSet =bottomScrews
					.movex(-screwX)
					.movey(screwY/2)
					.union(
						bottomScrews
							.movex(-screwX)
							.movey(-screwY/2)
					)
					.union(
						bottomScrews
							.movey(screwY/2)
					)
					.union(
						bottomScrews
							.movey(-screwY/2)
					)
					.movex(distanceFromCenterToBolt)
					.movez(topLevel)				
		baseShape = baseShape.difference([bottomScrewSet,screwAcross])	
		double boardKeepaway =50
		CSG nucleoBoard = nucleo.get(0).movey(-boardKeepaway)
		double baseBackSet = 	-baseShape.getMaxX()+keepAwayDistance+encoderKeepawayDistance
		double spacing = 75

		double nucleoMountPlacement = spacing+baseBackSet+nucleoBoard.getMaxX()
		CSG boxCut = new Cube((workcellSize/2)+nucleoMountPlacement+boardKeepaway,workcellSize,thickness.getMM()*2).toCSG()
					.toXMax()
					.movex(workcellSize/2)
		manipulationParts.clear()
		def parts =(ArrayList<CSG>)ScriptingEngine
					 .gitScriptRun(
            "https://github.com/WPIRoboticsEngineering/RBELabCustomParts.git", // git location of the library
            "3001TrackingObjects.groovy" , // file to load
            null// no parameters (see next tutorial)
            ) 
          manipulationBall=parts.get(0)
		          		.movez(-ballCenter.getMM())
		            		.roty(90) 
		manipulationParts.addAll(parts.collect{
            	it.movez(-ballCenter.getMM())
            	.roty(90)            	
			.transformed(tipatHome)
            })
		
		//CSG tipHole =new Cylinder(10,10,thickness.getMM()*3,(int)90).toCSG()
		//			.movez(-thickness.getMM()*1.5)
					
		double footingWidth = 330
		double insetForMakerbeam =15
		CSG footing =new Cube(workcellSize,footingWidth,thickness.getMM()).toCSG()
					.toXMin()
					.movex(-centerOfRobotToBackEdgeOfBoard)
		CSG hole =  new Cylinder(1.5, // Radius at the bottom
                      		1.5, // Radius at the top
                      		thickness.getMM()*4, // Height
                      		(int)30 //resolution
                      		).toCSG()//convert to CSG to display 
                      		.movez(thickness.getMM()*-2)
         	def holeset =  hole.movey(  footingWidth/2-insetForMakerbeam)
         				.union(hole.movey(  -footingWidth/2+insetForMakerbeam))  
         	def allSideHoles=holeset
         	for(int i= -workcellSize+insetForMakerbeam;i< workcellSize-insetForMakerbeam;i+=   workcellSize/6){
         		 //allSideHoles= allSideHoles.union(	holeset.movex(i))	
         	}
		double etchWith =0.5
		etchX = (workcellSize/2)-2
		//CSG etchX =new Cube((workcellSize/2)-2,etchWith,thickness.getMM()).toCSG()
		//			.toXMin()
		//CSG etchY =new Cube(etchWith,footingWidth-2,thickness.getMM()).toCSG()
		def etchParts = []
		double gridDimention =25
		
		CSG etch =new Cube(10,10,thickness.getMM()).toCSG()
					.toXMin()
					.toYMin()

		etch=etch
		int x =0
		for(double i=-150;i<151;i+=gridDimention*2){
			for(double j=0;j<etchX;j+=gridDimention){
				etchParts.add(etch
						.movex(j)
						.movey(i))
			}
		}
		
		//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(manipulationParts)
						//.difference(cameraParts)
						.difference(etchParts)
						//.union(allSideHoles)
		
		basePlateUpper=basePlate.intersect(footing)
		basePlateLower=basePlate.difference(footing)	/*
		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,baseServo,screws,gearHole])

				
		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()
			})
		
		basePlateUpper.setManufacturing({ toMfg ->
				p= toMfg
						.toXMin()
						.toYMin()
						.toZMin()
				p.addExportFormat("svg")
				//Transform t=new Transform()
				return p
			})
		basePlateLower.setManufacturing({ toMfg ->
				p= toMfg
						.toXMin()
						.toYMin()
						.toZMin()
				p.addExportFormat("svg")
				//Transform t=new Transform()
				return p
			})
		
		/*
		sidePlateA.setManufacturing({ toMfg ->
				return toMfg
						.rotx(-90)
						.toZMin()
						.toXMin()
						.movex(basePlate.getMaxX())
			})
		sidePlateB.setManufacturing({ toMfg ->
			return toMfg
					.rotx(-90)
					.toZMin()
					.toXMin()
					.movex(basePlate.getMaxX())
		})
		*/
		

		
		//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)
//		for (int i=0;i<cameraParts.size();i++){
//			CSG p = cameraParts.get(i)
//			p.addExportFormat("svg")
			
//		}
		//cameraParts.forEach{
		//	it.addExportFormat("svg")
		//	add(attachmentParts,it,null,"cameraStand_SVG")
		//}
		basePlateUpper.addExportFormat("svg")
		basePlateLower.addExportFormat("svg")
		add(attachmentParts,baseShapeA,null,"baseLeft")
		add(attachmentParts,baseShapeB,null,"baseRight")
		add(attachmentParts,basePlateUpper,null,"basePlateEtching_SVG")
		add(attachmentParts,basePlateLower,null,"basePlateCut_SVG")
		add(attachmentParts,baseCap,null,"baseCap")
		add(attachmentParts,manipulationParts.get(0),null,"colorObject")
		add(attachmentParts,manipulationParts.get(1),null,"coaster")
		add(attachmentParts,manipulationParts.get(2),null,"calibrationObject")
		return attachmentParts;
	}
	@Override 
	public ArrayList<CSG> generateCad(DHParameterKinematics sourceLimb, int linkIndex) {
		def parts =(ArrayList<CSG>)ScriptingEngine
					 .gitScriptRun(
            "https://github.com/WPIRoboticsEngineering/RBELabCustomParts.git", // git location of the library
            "3001TrackingObjects.groovy" , // file to load
            null// no parameters (see next tutorial)
            ) 
          manipulationBall=parts.get(0)
		          		.movez(-ballCenter.getMM())
		            		.roty(90) 
		//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 = springBlockPin(gearBMeasurments.height)
								.rotx(180)
								.rotz(-Math.toDegrees(dh.getTheta()))
		// creating the servo
		
	
		
		
		CSG springMoved = moveDHValues(loadCell
            							//.movez(washerThickness)
									.rotz(-Math.toDegrees(dh.getTheta()))
									.movez(springBlockPart.getMinZ())
									//.rotz(linkIndex==0?180:0)
									,dh)
		CSG washerMoved = washerWithKeepaway
					.movez(centerLinkToBearingTop-washerThickness)
		CSG tmpMyGear = gearB
					.rotz(5)
					.union(washer.toZMax())
					.movez(-centerLinkToBearingTop+washerThickness)
		tmpMyGear = 	tmpMyGear	
					.difference(springBlockPartGear
								.intersect(tmpMyGear)
								.hull()
					)
					.union(springBlockPartGear)
		CSG loadCellBolts = moveDHValues(LoadCellScrews
							.rotz(-Math.toDegrees(dh.getTheta()))
								,dh)	
		CSG myArmScrews = moveDHValues(armScrews
						.rotz(-Math.toDegrees(dh.getTheta()))
						,dh)
   						.movex(springBlockPartRaw.getMaxX())
		CSG myGearB = moveDHValues(tmpMyGear
								.difference(loadBearingPin)
								,dh)
					.difference(loadCellBolts)
					.difference(myArmScrews)
					.setColor(javafx.scene.paint.Color.LIGHTGREEN);
		CSG myPin = moveDHValues(loadBearingPin,dh)
		
		CSG myspringBlockPart = moveDHValues(springBlockPart
		
										.difference(loadBearingPin)
										,dh)	
							.difference(loadCellBolts)
							.difference(myGearB)
							.setColor(javafx.scene.paint.Color.BROWN);
							
		CSG handMountPart=null;
		
		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
			CSG servoReference=   Vitamins.get(nextLink.getElectroMechanicalType(),nextLink.getElectroMechanicalSize())
			.rotz(90)
			
			double servoTop = servoReference.getMaxZ()
			servoReference=servoReference
				.movez(-centerLinkToBearingTop-encoderBearingHeight)		
				.movex(-gearDistance)
				//.rotz(90+Math.toDegrees(dh.getTheta()))
			double gearPlacementVSMotor = -(motorBackSetDistance+washerThickness)

			myGearA = getGearWithSpline( nextLink );
			if(linkIndex==0){
				CSG baseServo = Vitamins.get(nextLink.getElectroMechanicalType(),nextLink.getElectroMechanicalSize())
							.rotz(180)
							.movez(-centerLinkToBearingTop-encoderBearingHeight)		
							.movex(-gearDistance)
				CSG secondLinkServo =servoReference.clone()
				CSG baseForceSenseEncoder = encoder1
										
				CSG baseEncoder = encoder1
				
				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(),"servoGear")
				if(showVitamins)add(csg,baseServo,sourceLimb.getRootListener(),"servo")
				if(showVitamins)add(csg,baseEncoder,sourceLimb.getRootListener(),"encoder")
				if(showVitamins)add(csg,baseForceSenseEncoder,sourceLimb.getRootListener(),"encoder")
			}
			println "Link Hardware: using from index "+
					(linkIndex+1)+
					" "+nextLink.getElectroMechanicalSize() +
					" "+nextLink.getShaftSize()
					
			CSG forceSenseEncoder = encoder1
								
								.rotz(180-Math.toDegrees(dh.getTheta()))
								.rotx(180)
			CSG baseEncoderCap = getEncoderCap()
							.movez(-centerLinkToBearingTop)
							
			CSG thirdPlusLinkServo =servoReference.clone()
			CSG linkEncoder = encoder1
								
								.rotz(-Math.toDegrees(dh.getTheta()))
			ArrayList<CSG> esp = getServoCap(nextLink)
			double linkCconnectorOffset = drivenLinkXFromCenter-(encoderCapRodRadius+bearingDiameter)/2
			def end = [(double)-dh.getR()+linkCconnectorOffset,(double)dh.getD()*0.98,(double)0]
			def controlOne = [(double)-2 ,(double)end.get(1)*0.8,(double)0]
			def controlTwo = [(double)end.get(0),(double)0,(double)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
					
					(int)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 bounding = ribs.get(ribs.size()-2).getBoundingBox()
			/*
CSG supportRib = ribs.get(ribs.size()-2)
							//.intersect(bounding.movex(-5))
							//.intersect(bounding.movex(5))
							.movez(encoderBearingHeight -cornerRadius*2)
							//.union(ribs.get(ribs.size()-2))
							.union(ribs.get(ribs.size()-1)
							*/
			CSG upperRib = ribs.get(ribs.size()-2)
						.movez(encoderBearingHeight -cornerRadius*2)
						//.movex(-2)
						.movey(12)
			CSG supportRib = ribs.get(ribs.size()-1)
							.union(upperRib)
							.toZMin()
							.movez(centerLinkToBearingTop-encoderBearingHeight+ cornerRadius*2)
							.union(mountLug)
							.hull()
							.movex(5)// offset to avoid hitting pervious link
							//.movey(-1)// offset to avoid hitting pervious link
							//.movez(-2)// adjust support to met previous gear
			def linkParts = Extrude.bezier(	connectorArmCross,
					controlOne, // Control point one
					controlTwo, // Control point two
					end ,// Endpoint
					linkResolution
					)
			print "\r\nUnioning link..."
			long start = System.currentTimeMillis()
			CSG bracketBezier = CSG.unionAll(linkParts)
						.toZMin()
						.movez(centerLinkToBearingTop )
							.movex(5)// offset to avoid hitting pervious link
							.movey(-2)// offset to avoid hitting pervious link

			CSG sidePlateWithServo =esp.get(0)							
			
			CSG linkSection = bracketBezier
						
						
			linkSection = 	linkSection
							.union(supportRib)
			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)			
			//if(linkIndex==0){
				CSG rightSIdeSupport =bracketBezier
									.toZMin()
									.movez(sidePlateWithServo.getMinZ())	
									.union([supportRib
											.mirrorz()
											.movex(5)
											]
											)
									.difference(sidePlateWithServo.hull())
									.minkowskiDifference(
										springBlockPart.hull(),// the part we want to fit into a cutout
										6.0// the offset distance to fit
										)
									.minkowskiDifference(
										myGearB.hull(),// the part we want to fit into a cutout
										6.0// the offset distance to fit
										)
				sidePlateWithServo =sidePlateWithServo	
					.union(rightSIdeSupport)		
					.difference(linkEncoder)
					//.difference(baseEncoderCap)	
					.difference([springBlockPart,
							tmpMyGear,myGearB])	
					.difference(myArmScrews)
					.difference(springMoved)
					//.difference(bottomCut)
					.difference(linkSection)
			//}
			linkSection = 	linkSection				
				.difference(myspringBlockPart
						.intersect(linkSection)
						.hull()
						.toolOffset(printerOffset.getMM()))
				.difference(baseEncoderCap//.hull()
							.intersect(linkSection)
							.hull()
							)
				.difference(otherEncoder.rotz(180)
							.intersect(linkSection)
							)	
				.difference(	springBlockPart)	
				.difference(myArmScrews)
				.difference(springMoved)
				.difference(bottomCut)
				//.difference(loadCellBolts)
			double took = System.currentTimeMillis()-start
			print "Done, took "+(took/1000.0) +" seconds\r\n"
			baseEncoderCap=baseEncoderCap.union(linkSection)
			baseEncoderCap.setColor(javafx.scene.paint.Color.LIGHTBLUE);
			sidePlateWithServo.setColor(javafx.scene.paint.Color.ORANGE);
			if(esp.size()>1)esp.get(1).setColor(javafx.scene.paint.Color.WHITE);
			previousEncoder = linkEncoder
			previousServo = thirdPlusLinkServo

			double chipToShortside = 8
			double chipToLongSide  = 9.0
			double mountHoleRadius = 2.0/2  
			double chipToShortsideReal = 5.5
			double chipToLongSideReal  = 9.0 
			
			double standoffHeight = 6
			if( standoffBLock==null){
				CSG bolt =new Cylinder(3,3,standoffHeight,(int)20).toCSG() // a one line Cylinder
								           
				CSG boltSet = bolt
						.movex(chipToLongSideReal)
						.movey(chipToShortsideReal)
						
				.union(bolt
						.movex(-chipToLongSideReal)
						.movey(chipToShortsideReal)
						)
				.union(bolt
						.movex(chipToLongSideReal)
						.movey(-chipToShortsideReal)
						)
				.union(bolt
						.movex(-chipToLongSideReal)
						.movey(-chipToShortsideReal)
						) 
				
				print "Creating standoff block..."		    
			     standoffBLock = new Cube((chipToLongSide+mountHoleRadius*1.5)*2.2,(chipToShortside+mountHoleRadius*1.5)*2.2,	2).toCSG()	
			     					.toZMin()
			     					.union(boltSet )
			     					.union(boltSet.rotz(90))
			     					.movez(encoderToEncoderDistance)
			     standoffBLock = standoffBLock
			     					.difference(otherEncoder)
			     					.difference(springBlockPart.hull())
			     					.setColor(javafx.scene.paint.Color.GREY);
			     print "done\n"	
			}
			CSG myStandoff= standoffBLock.clone()
			washerMoved.setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toYMin()
						.toZMin()
			})
		     myStandoff.setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toYMin()
						.toZMin()
			})
			myGearA.setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toZMin()
			})
			baseEncoderCap.setManufacturing({ toMfg ->
				return toMfg
						.roty(180)
						.toXMin()
						.toZMin()
			})
			sidePlateWithServo.setManufacturing({ toMfg ->
				return toMfg
						.toXMin()
						.toZMin()
			})
			if(esp.size()>1)
			esp.get(1).setManufacturing({ toMfg ->
				return toMfg
						.roty(180)
						.toXMin()
						.toZMin()
			})

			add(csg,washerMoved,dh.getListener(),"washer")
			add(csg,myStandoff,dh.getListener(),"encoderStandoff")
			if(showRightPrintedParts)add(csg,myGearA,dh.getListener(),"servoGear")
			if(showVitamins)add(csg,thirdPlusLinkServo,dh.getListener(),"servo")
			if(showVitamins)add(csg,linkEncoder,dh.getListener(),"encoder")
			//if(showVitamins)add(csg,horn,dh.getListener(),"horn")
			if(showVitamins)add(csg,otherEncoder,dh.getListener(),"otherEncoder")
			if(showRightPrintedParts)add(csg,sidePlateWithServo,dh.getListener(),"sidePlate"+linkIndex)
			if(esp.size()>1)if(showLeftPrintedParts)add(csg,esp.get(1),dh.getListener(),"encoderPlate"+linkIndex)
			if(showLeftPrintedParts)add(csg,baseEncoderCap,dh.getListener(),"baseEncoderCap"+linkIndex)
			
		}else{
			// load the end of limb
			// Target point
			handMountPart = handMount()
			CSG tipCalibrationPart= tipCalibration()
			File gripBaseFile = ScriptingEngine.fileFromGit(
				"https://github.com/madhephaestus/SeriesElasticActuator.git",
				"gripper/all.stl");
				/*
			File gripLeftFile = ScriptingEngine.fileFromGit(
				"https://github.com/madhephaestus/SeriesElasticActuator.git",
				"gripper/left.stl");
			// Load the .CSG from the disk and cache it in memory
			File gripRightFile = ScriptingEngine.fileFromGit(
				"https://github.com/madhephaestus/SeriesElasticActuator.git",
				"gripper/right.stl");
				*/
			// Load the .CSG from the disk and cache it in memory
			double boltPlateThickness = 6
			CSG cup =new Cylinder(ballRadius.getMM()+2,15).toCSG() // a one line Cylinder
			CSG box = cup.getBoundingBox()
					.movez(7)
					.movex(2.5)
			CSG bolt =new Cylinder(4.3/2,70).toCSG() // a one line Cylinder		
			bolt = bolt.union (bolt.movex(-12))
					.movex(-ballRadius.getMM()-8.75)
			
			mounStrip = new Cube(	40,// X dimention
						13.2,// Y dimention
						boltPlateThickness//  Z dimention
						).toCSG()
						.toXMax()
						.toZMax()
						.movez(cup.getMaxZ())
			cup = cup.intersect(box)
					.union(mounStrip )
					.difference(manipulationBall.makeKeepaway(1))
					.difference(bolt)
			
			//CSG gripBase  = Vitamins.get(gripBaseFile)	
			//				.movex(-56.75-ballRadius.getMM()-8+ballCenter.getMM())
			//				.movez(-2.25)		
			//				.movey(-3.4)	
			//CSG gripLeft  = Vitamins.get(gripLeftFile)
			//CSG gripRight  = Vitamins.get(gripRightFile)
			
			
						//.union([gripLeft,gripRight])
						
			
			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,drivenLinkWidth,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)
					.movez(-topOfGearToCenter)
					.movez(washerThickness)
			handMountPart=handMountPart
						.union(section)
			try{
				handMountPart=handMountPart
								.difference(myspringBlockPart
										.intersect(handMountPart)
										.hull()
										.toolOffset(printerOffset.getMM()*2))
								.difference(myArmScrews.movex(-45),springMoved.toolOffset(2)	)
			}catch(Exception ex){
				//BowlerStudio.printStackTrace(ex)
			}				
			tipCalibrationPart.setColor(javafx.scene.paint.Color.PINK);
			handMountPart.setColor(javafx.scene.paint.Color.WHITE);
			
			handMountPart.setManufacturing({ toMfg ->
				return toMfg
					.rotx(90)
					.toXMin()
					.toZMin()
			})
			cupr=cup.mirrorz()
			cup.setManufacturing({ toMfg ->
				return toMfg
					.rotx(180)
					.toZMin()
			})
			cupr.setManufacturing({ toMfg ->
				return toMfg
					.toZMin()
			})
			//if(showRightPrintedParts)add(csg,tipCalibrationPart,dh.getListener(),"calibrationTip")
			if(showLeftPrintedParts)add(csg,handMountPart,dh.getListener(),"lastLink")
			//add(csg,gripBase,dh.getListener(),"gripBase")
			//add(csg,manipulationBall,dh.getListener(),"gripBall")
			add(csg,cup,dh.getListener(),"cup")
			add(csg,cupr,dh.getListener(),"cupr")
			//add(csg,gripLeft,dh.getListener(),"gripLeft")
			//add(csg,gripRight,dh.getListener(),"gripRight")
		}
		
		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(),"loadCellBlock")
		if(showVitamins)add(csg,myPin,dh.getListener(),"pin")
		if(showRightPrintedParts)add(csg,myGearB,dh.getListener(),"drivenGear")
		if(showVitamins)add(csg,springMoved,dh.getListener(),"loadCell")
		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,drivenLinkWidth,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-ballRadius.getMM()-8)
		return mountPlate
	}
	private CSG springBlockPin(double thickness){
		double magnetPinDiameter = bearingData.innerDiameter/2
		return new Cylinder(magnetPinDiameter,magnetPinDiameter,encoderBearingHeight+6+thickness,(int)30).toCSG()
				.toZMax()
				.movez(encoderToEncoderDistance+6)
				.difference(encoder1.rotx(180))
	}
	private CSG springBlock(double thickness){
		if(springLinkBlockLocal.get(thickness)!=null)
			return springLinkBlockLocal.get(thickness).clone()
		CSG linkBlank = new RoundedCube(drivenLinkX-1,drivenLinkWidth,thickness)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toXMin()
						.toZMax()
						.movez(centerLinkToBearingTop)
						.movex(-(bearingData.innerDiameter/2)-1)
		double loadCellNub = loadCell.getMaxZ()
		if(loadCellNub>thickness)
			loadCellNub=thickness
		CSG connectionLink = new RoundedCube(25,drivenLinkWidth-20,loadCellNub)
							.cornerRadius(cornerRadius)
							.toCSG()	
							.toZMin()
		CSG linkBackBlank = new RoundedCube(25,drivenLinkWidth,loadCellNub)
						.cornerRadius(cornerRadius)
						.toCSG()
						.toZMin()
						.union(connectionLink.movex(-3))
						.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))
		//}
		
		CSG magnetPin = springBlockPin(thickness)
		
		linkBlank =linkBlank
					.union(magnetPin)
					.difference(armScrews
						.movex(linkBlank.getMaxX())
						.movez(washerThickness)
						)
					.union(linkBackBlank)
					.difference([springCut])
					.difference(encoder1.rotx(180))
					.movez(-washerThickness)
		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+printerOffset.getMM()/2))
								.hull() 
								.movez(-printerOffset.getMM()/2)
		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 def getGearWithSpline(LinkConfiguration nextLink ){
		def type =nextLink.getShaftType()
		println "Gear with shaft "+type
		CSG horn = Vitamins.get(nextLink.getShaftType(),nextLink.getShaftSize())	
					.rotx(180)
					.toZMin()
		double hornOffset = 	horn.getTotalZ()
						
		horn=horn.movex(-gearDistance)

		double gearPlacementVSMotor = -(motorBackSetDistance+washerThickness)
		CSG myGearA = gearA.clone()
					.union(gearStandoff)
					.movez(washerThickness)	
		horn=horn.movez(gearPlacementVSMotor+washerThickness)
		def numHorns =(myGearA.getTotalZ()-horn.getTotalZ())*3
		
		for(double i=0;i<numHorns;i+=horn.getTotalZ()/3){
			horn=horn.union(horn
							.movez(i))
			
			//println "Moving horn  = "+i			
		}
		myGearA=myGearA
				.difference(horn)
		// special recess for measured difference
		//myGearA=myGearA
		//		.difference(horn.movez(-2.0))
		myGearA = myGearA
					//.rotz(-90)
					.movez(-centerLinkToBearingTop)	
					.setColor(javafx.scene.paint.Color.BLUE);
		return myGearA					
		//return [myGearA,horn.movez(-centerLinkToBearingTop)	]
	}
	private def getServoCap(LinkConfiguration conf ){
		if(sidePlateLocal.get(conf.getXml())!=null)
			return sidePlateLocal.get(conf.getXml()).collect{
				it.clone()
			}
		CSG servoReference=   Vitamins.get(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize())

		double SidePlateThickness = encoderBearingHeight 
		double totalFlangLen = servoReference.getTotalX()
		double shaftToShortSideFlandgeEdge = servoReference.getMaxX()
		double hornOffset = 	0	
		double servoNub = 0
		// creating the servo
		 servoReference=  servoReference
			.movez(-centerLinkToBearingTop-SidePlateThickness)			
			.movey(-gearDistance)
			.rotz(90)
			//.movez(-motorBackSetDistance/2)
		CSG gearHole = gearKeepaway
					.movez(servoNub-centerLinkToBearingTop)	
		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(servoReference.getTotalX()+encoderCapRodRadius,
							servoReference.getTotalY()+encoderCapRodRadius,
							SidePlateThickness)
						
						.toCSG()
						.toZMin()
						//.toYMin()
						//.movey(-encoderCapRodRadius/2)
						.movex(-gearDistance-encoderCapRodRadius)
		CSG bottomBlock = capPinSet.union([center,baseShape]).hull()
						.toZMax()
						.movez(encoderBearingHeight-encoderToEncoderDistance)
						.difference(encoderKeepaway)
						.difference(screwSet.movez(-encoderToEncoderDistance-encoderBearingHeight))
						.difference([servoReference.movez(-2),gearHole.movez(2)])
						.difference(servoReference)
		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 = 85+5+thickness.getMM()
		Log.enableSystemPrint(true)
		println "Camera at "+cameraLocationNR
		double cameraBolt = (workcellSize-cameraMountSize)/2
		/*
		CSG camera = (CSG) ScriptingEngine
					 .gitScriptRun(
			            "https://github.com/madhephaestus/SeriesElasticActuator.git", // git location of the library
			            "camera.groovy" , // file to load
			            null// create a keepaway version
			            )
		*/
		CSG cameraBase = new Cube(6.5,52,thickness.getMM()).toCSG()
			.toZMax()   
			.movex(9+35/2)
		CSG camera = new Cylinder(35/2,35/2,	  thickness.getMM()    ,(int)30).toCSG()   
		.toZMax()
		.union(cameraBase)
		
		camera = camera
			            .transformed(cameraLocationCSG)
		CSG camerMount = new Cube(	cameraMountSize+5,
								cameraMountSize+5,
								thickness.getMM()).toCSG()
								.toZMax()
								.transformed(cameraLocationCSG)
								.difference(camera)
		CSG camerMountLug = new Cube(	45+5+thickness.getMM(),
								thickness.getMM(),
								45).toCSG()
								.toYMax()
								.movey(cameraMountSize/2)
								.toZMin()
								.movex(10)
		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)
						.movex(-20)
		CSG midLug = camerMountLug
						.rotz(180)
						.toXMin()
						.movex(camerMount.getMaxX()+20)
						.movez(cameraBolt+thickness.getMM())
		CSG notches =  notch
					.movex(15)
					.union(	notch
							.movex(-15)	)
		CSG bottomNotches = notches
						.rotx(180)
						.movex(cameraBolt)	
						.toZMax()
		CSG topNotches = notches.transformed(cameraLocationCSG)	

		CSG nut = LockNutKeepaway
				.movez(thickness.getMM())
		//boltSizeParam
		CSG boltCutout = new Cube(boltMeasurments.outerDiameter,
							thickness.getMM(),
							45-thickness.getMM()).toCSG()
							.toZMin()
							.union(new Cylinder(boltMeasurments.outerDiameter/2,
											boltMeasurments.outerDiameter/2,
											thickness.getMM(),(int)30).toCSG()
											.toZMax()
											,
											nut
							)
							.movez(-thickness.getMM()/2)
								
		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)
					.difference(bottomBolts,topBolts)
					.difference(camerMount.hull())
					.union(topNotches)
		CSG bracketB = bracketA
						.toYMax()
						.movey(cameraMountSize/2)
		camerMount=camerMount.difference(topBolts,bracketA,bracketB)
		double cameraOffset = cameraMountSize+22
		camerMount.setManufacturing({ toMfg ->
				TransformNR step = cameraLocationNR.inverse()
				Transform move = TransformFactory.nrToCSG(step)
				p= toMfg
						.transformed(move)
						.toXMin()
						.toYMin()
						.toZMin()
						.movex(-cameraOffset*1)
				p.addExportFormat("svg")
				return p
		})
		
		bracketA.setManufacturing({ toMfg ->
				p= toMfg
						.rotx(90)
						.toXMin()
						.toYMin()
						.toZMin()
						.movex(-cameraOffset*2)
				p.addExportFormat("svg")
				return p
		})
		bracketB.setManufacturing({ toMfg ->
				p= toMfg
						.rotx(90)
						.toXMin()
						.toYMin()
						.toZMin()
						.movex(-cameraOffset*3-10)
						.movey(-35)
				p.addExportFormat("svg")
				return p
		})
		bottomBolts.setManufacturing({ toMfg ->
				return null
		})
		
		//return [bottomNotches]
		return [camerMount,bracketA,bracketB,bottomBolts].collect{
			it.addExportFormat("svg")
			return it
		}
		
	}

	private add(ArrayList<CSG> csg ,CSG object, Affine dh , String name){
		if(dh!=null)
			object.setManipulator(dh);
		csg.add(object);
		//BowlerStudioController.addCsg(object);
		if(version[0]>0 || version[1]>=11){
			println "Name API found"
			object.setName(name)
		}
	}
}
return c//[c.springBlock(c.drivenLinkThickness), c.springBlockPin(c.gearBMeasurments.height).movey(60)]

base=DeviceManager.getSpecificDevice( "HephaestusWorkCell",{
			//If the device does not exist, prompt for the connection
			
			MobileBase m = MobileBaseLoader.fromGit(
				"https://github.com/madhephaestus/SeriesElasticActuator.git",
				"seaArm.xml"
				)
			if(m==null)
				throw new RuntimeException("Arm failed to assemble itself")
			println "Connecting new device robot arm "+m
			return m
		})
def arm = base.getAppendages().get(0)
int linkIndex=2
LinkConfiguration conf = arm.getLinkConfiguration(linkIndex);
return c.generateBody(base)
return c.getGearWithSpline(conf)
return c.generateCad(arm,  linkIndex)
return c.getServoCap(conf)

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
         )