XLKat_copy copy of XLKat

dhsolver3.groovy

package com.neuronrobotics.smallkat;

import com.neuronrobotics.bowlerstudio.BowlerStudio;
import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import java.util.ArrayList;

import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;
import eu.mihosoft.vrl.v3d.Transform;

public class scriptJavaIKModel implements DhInverseSolver {

	int limbIndex =0;
	public scriptJavaIKModel(int index){
		limbIndex=index;
	}

	@Override
	public double[] inverseKinematics(TransformNR target, double[] jointSpaceVector, DHChain chain) {
		//System.out.println("My IK");
        try {
       	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 z = target.getZ();
       	double y = target.getY();
       	double x = target.getX();
//		  z = Math.round(z*100.0)/100.0;
//		  y = Math.round(y*100.0)/100.0;
//            x = Math.round(x*100.0)/100.0;
//            
       	RotationNR q = target.getRotation();
       	
        //System.out.println("elevation: " + elev);
		//System.out.println("z: " + z);
       	//System.out.println("y: " + y);
       	//System.out.println("x: " + x);
       
       //double Oang = Math.PI/2 + q.getRotationElevation();
//            double Oang = Math.toRadians(45);
//            
//            double Oanginv = (Math.PI/2) - Oang;

       	double l1_d = links.get(0).getR();
       	double l2_d = links.get(1).getR();
       	double l3_d = links.get(2).getR();
       	double l4_d = links.get(3).getR();

       	//System.out.println("L1: " + l1_d);
       	//System.out.println("L2: " + l2_d);
       	//System.out.println("L3: " + l3_d);
       	//System.out.println("L4: " + l4_d);
       
       
       	double[] inv = new double[linkNum];
		double a1 = Math.atan2(y , x);
		double a1d = Math.toDegrees(a1);

		def newTip = new Transform()
			.movex(x)
			.movey(y)
			.movez(z)
			.rotz(a1d)
		//println newTip
		
		x=newTip.getX()
		y=newTip.getY()
		z=newTip.getZ()
		//System.out.println(" Base Angle : " + a1d);
		//System.out.println(" Base Orented z: " + z);
       	//System.out.println(" Base Orented y: " + y);
       	//System.out.println(" Base Orented x: " + x);
       
		double a2 = Math.atan2(z,x); // Z angle using x axis and z axis
		double a2d = Math.toDegrees(a2);
		//println a2d
		double elev = -a2d -45+24.26//Math.toDegrees(q.getRotationElevation() )
		//println "R Vector Angle "+a2d

       	double r1 = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); // X and Y plane Vector
       	double r2 = Math.sqrt(Math.pow(x, 2) + Math.pow(y,2)+Math.pow(z, 2)); // Leg Vector
       	double r3 = Math.sqrt(Math.pow(x, 2) + Math.pow(z, 2)); // x and z vector
		/*
       	def rvector = new Cube(r2,1,1).toCSG()
       				.toXMin()
       				.roty(a2d)
       				
       	def rvectorOrig = rvector.rotz(-a1d)
       				.setColor(javafx.scene.paint.Color.BLUE)
       	BowlerStudioController.addCsg(rvector)
       	BowlerStudioController.addCsg(rvectorOrig)
		*/
       	def wristCenter = new Transform()
       					.movex(-l4_d)
			.roty(-elev)
			.movey(y)
			.movez(z)
			.movex(x)
		/*
       	def foot = new Cube(l4_d,1,1).toCSG()
       			.toXMin()
       			.transformed(wristCenter)
       			.setColor(javafx.scene.paint.Color.AQUA)
       	BowlerStudioController.addCsg(foot)
		*/
		x=wristCenter.getX()
		y=wristCenter.getY()
		z=wristCenter.getZ()
		double wristAngle = Math.atan2(z,x);
		double wristAngleDeg =Math.toDegrees(wristAngle)
		double wristVect =  Math.sqrt(Math.pow(x, 2) + Math.pow(z, 2)); // x and z vector
		//System.out.println(" Wrist Angle: " + wristAngleDeg);
		//System.out.println(" Wrist Vect: " + wristVect);
		//System.out.println(" Wrist z: " + z);
       	//System.out.println(" Wrist y: " + y);
       	//System.out.println(" Wrist x: " + x);
       	/*
       	def wristVector = new Cube(wristVect,1,1).toCSG()
       			.toXMin()
       			.roty(wristAngleDeg)
       			.setColor(javafx.scene.paint.Color.WHITE)
       	BowlerStudioController.addCsg(wristVector)
		*/
       	double shoulderTiltAngle = Math.toDegrees(Math.acos(
       					(Math.pow(l2_d,2)+Math.pow(wristVect,2)-Math.pow(l3_d,2))/
       					(2*l2_d*wristVect)
       		))
       	double elbowTiltAngle = Math.toDegrees(Math.acos(
       					(Math.pow(l3_d,2)+Math.pow(l2_d,2)-Math.pow(wristVect,2))/
       					(2*l3_d*l2_d)
       		))
       	/*	
		def shoulderVector = new Cube(l2_d,1,1).toCSG()
       			.toXMin()
       			.roty(wristAngleDeg-shoulderTiltAngle)
       			.setColor(javafx.scene.paint.Color.GRAY)
       	BowlerStudioController.addCsg(shoulderVector)
       	*/
		inv[0]=a1d
		if(Math.toDegrees(links.get(2).getTheta())<0){
	       	inv[1]=wristAngleDeg+shoulderTiltAngle-Math.toDegrees(links.get(1).getTheta())
	       	inv[2]=elbowTiltAngle-180-Math.toDegrees(links.get(2).getTheta())
		}else{
			inv[1]=wristAngleDeg-shoulderTiltAngle-Math.toDegrees(links.get(1).getTheta())
	       	inv[2]=180-elbowTiltAngle-Math.toDegrees(links.get(2).getTheta())
		}
		inv[3]=-inv[1]-inv[2]-Math.toDegrees(links.get(3).getTheta())-elev-
	       			Math.toDegrees(links.get(1).getTheta())-
	       			Math.toDegrees(links.get(2).getTheta())
       	//System.out.println(inv[0]);
       	//System.out.println(inv[1]);
       	//System.out.println(inv[2]);
       	//System.out.println(inv[3]);


       if(Double.isNaN(inv[0]) || Double.isNaN(inv[1]) || Double.isNaN(inv[2]) || Double.isNaN(inv[3]))
       	throw new ArithmeticException("Can't move to that position");
       else
       	return inv;

       	
   } catch (Throwable t) {
       //BowlerStudio.printStackTrace(t);
       return jointSpaceVector;
   }
}

}

if(args==null)
	args=[0]
return new scriptJavaIKModel (args[0])

DynamicWalking.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;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;
import com.neuronrobotics.sdk.addons.kinematics.imu.*

enum WalkingState {
    Rising,ToHome,ToNewTarget,Falling
}

if(args==null){
	double stepOverHeight=15;
	long stepOverTime=20*5*3;// Servo loop times number of points times Nyquest doubeling
	Double zLock=-200;
	Closure calcHome = { DHParameterKinematics leg ->
			TransformNR h=leg.calcHome()
	 		TransformNR  legRoot= leg.getRobotToFiducialTransform()
			TransformNR tr = legRoot.copy()
			tr.setZ(zLock)
			//Bambi-on-ice the legs a bit
			if(legRoot.getY()>0){
				//tr.translateY(-5)
			}else{
				//tr.translateY(5)
			}

			return tr;

	}
	boolean usePhysicsToMove = true;
	long stepCycleTime =5000
	long walkingTimeout =5000
	int numStepCycleGroups = 2
	double standardHeadTailAngle =0;// -20
	double staticPanOffset = 0;// 10
	double coriolisGain = 1
	boolean headStable = false
	double maxBodyDisplacementPerStep = 70
	double minBodyDisplacementPerStep = 65
	args =  [stepOverHeight,
	stepOverTime,
	zLock,
	calcHome,
	usePhysicsToMove,
	stepCycleTime,
	numStepCycleGroups,
	standardHeadTailAngle,
	staticPanOffset,
	coriolisGain,
	headStable,
	maxBodyDisplacementPerStep,
	minBodyDisplacementPerStep,
	walkingTimeout]
}
Log.enableSystemPrint(true)
return new com.neuronrobotics.sdk.addons.kinematics.IDriveEngine (){
	boolean resetting=false;
	double stepOverHeight=(double)args.get(0);
	long stepOverTime=(long)args.get(1);
	private Double zLock=(Double)args.get(2);
	Closure calcHome =(Closure)args.get(3);
	boolean usePhysics=(args.size()>4?((boolean)args.get(4)):false);
	long stepCycleTime=args.get(5)
	long stepCycleTimeMax=args.get(5)
	long timeOfCycleStart= System.currentTimeMillis();
	int stepCycyleActiveIndex =0
	int numStepCycleGroups = args.get(6)
	double standardHeadTailAngle =args.get(7)
	double staticPanOffset = args.get(8)
	double coriolisGain=args.get(9)
	boolean headStable =args.get(10)
	double maxBodyDisplacementPerStep= args.get(11)
	double minBodyDisplacementPerStep =args.get(12)
	long walkingTimeout =args.get(13)


	ArrayList<DHParameterKinematics> legs;
	HashMap<Integer,ArrayList<DHParameterKinematics> > cycleGroups=new HashMap<>();
	HashMap<DHParameterKinematics,double[] > cycleStartPoint=new HashMap<>();
	TransformNR previousGLobalState;
	TransformNR target;
	TransformNR cachedNewPose;
	double cachedSeconds;
	RotationNR rot;
	int resettingindex=0;
	private long reset = System.currentTimeMillis();

	Thread stepResetter=null;
	boolean threadDone=false
	WalkingState walkingState= WalkingState.Rising
	MobileBase source
	TransformNR newPose =new TransformNR()
	long miliseconds
	boolean timout = false
	long loopTimingMS =5
	long timeOfLastLoop = System.currentTimeMillis()
	long timeOfLastIMUPrint = System.currentTimeMillis()
	int numlegs
	double gaitIntermediatePercentage
	TransformNR global
	int coriolisIndex = 0
	double coriolisDivisions = 36.0
	double coriolisDivisionsScale = 360.0/coriolisDivisions
	double coriolisTimeBase =10
	long coriolisTimeLast=0
	double startAngle = 0
	public void resetStepTimer(){
		reset = System.currentTimeMillis();
	}
	def getUpLegs(){
		if(null!=cycleGroups.get(stepCycyleActiveIndex))
			return cycleGroups.get(stepCycyleActiveIndex)
		return []
	}
	def getDownLegs(){
		def vals= legs.collect{
				if(!getUpLegs().contains(it))
					return it
			}
		vals.removeAll([null])
		return vals
	}
	void updateDynamics(IMUUpdate update){
		if(stepResetter==null||reset+walkingTimeout < System.currentTimeMillis())
			return

		long incrementTime = (System.currentTimeMillis()-timeOfLastIMUPrint)
		double velocity=0
		if(	Math.abs(update.getxAcceleration())>0 ||
			Math.abs(update.getxAcceleration())>0 ||
			Math.abs(update.getxAcceleration())>0
		)
			velocity=update.getRotyAcceleration()
		else
			velocity=0
		if (velocity>10)
			velocity=10
		if (velocity<-10)
			velocity=-10
		if(incrementTime>10){
			timeOfLastIMUPrint= System.currentTimeMillis()
			if(velocity>0){

				print "\r\nDynmics IMU state \n"
				for(def state :[update]){
					print " x = "+update.getxAcceleration()
					print "  y = "+update.getyAcceleration()
					print " z = "+update.getzAcceleration()
					print " rx = "+update.getRotxAcceleration()
					print " ry = "+update.getRotyAcceleration()
					print " rz = "+update.getRotzAcceleration()+"\r\n"
				}

			}
			long timeSince=	(System.currentTimeMillis()-timeOfCycleStart)
			double gaitTimeRemaining = (double) (System.currentTimeMillis()-timeOfCycleStart)
			double gaitPercentage = gaitTimeRemaining/(double)(stepCycleTime)
			double sinPanOffset = Math.sin(gaitPercentage*Math.PI)*staticPanOffset

			double standardHeadTailPan = (stepResetter==null)?0:(stepCycyleActiveIndex%2==0?sinPanOffset:-sinPanOffset)
			double bobingPercent = Math.cos(gaitPercentage*Math.PI-Math.PI/2)*standardHeadTailAngle/2+standardHeadTailAngle/2
			for(def d:source.getAllDHChains()){
				String limbName = d.getScriptingName()
				double sinCop = Math.sin(Math.toRadians(coriolisIndex*coriolisDivisionsScale))
				double cosCop = Math.cos(Math.toRadians(coriolisIndex*coriolisDivisionsScale))

				double computedTilt = bobingPercent+(velocity*sinCop*coriolisGain)
				double computedPan = standardHeadTailPan+(velocity*cosCop*coriolisGain)
				long coriolisincrementTime = (System.currentTimeMillis()-coriolisTimeLast)
				double coriolisTimeDivisionIncrement = (coriolisTimeBase/coriolisDivisions)
				//println coriolisIndex+" Time division = "+coriolisTimeDivisionIncrement+" elapsed = "+coriolisincrementTime
				if(coriolisTimeDivisionIncrement<coriolisincrementTime){
					coriolisTimeLast=System.currentTimeMillis()
					if(velocity>0){
						coriolisIndex++;
						coriolisIndex=(coriolisIndex>=coriolisDivisions?0:coriolisIndex)
					}else{
						coriolisIndex--;
						coriolisIndex=(coriolisIndex<0?coriolisDivisions-1:coriolisIndex)
					}
				}
				try{
					if(limbName.contentEquals("Tail")){
						d.setDesiredJointAxisValue(0,// link index
									computedTilt, //target angle
									0) // 2 seconds
						d.setDesiredJointAxisValue(1,// link index
									computedPan, //target angle
									0) // 2 seconds
					}
					if(limbName.contentEquals("Head")){
						if(!headStable){
							d.setDesiredJointAxisValue(0,// link index
										computedTilt, //target angle
										0) // 2 seconds
							d.setDesiredJointAxisValue(1,// link index
										computedPan, //target angle
										0) // 2 seconds
						}else{
							d.setDesiredJointAxisValue(0,// link index
										standardHeadTailAngle, //target angle
										0) // 2 seconds
							d.setDesiredJointAxisValue(1,// link index
										0, //target angle
										0) // 2 seconds
						}
					}
					//Thread.sleep(5)
				}catch(Exception e){
					//BowlerStudio.printStackTrace(e)
				}
			}

		}

	}
	void pose(def newAbsolutePose){
		source.setGlobalToFiducialTransform(newAbsolutePose)
		for(def leg:legs){
			def pose =compute(leg,1,new TransformNR())
			if(leg.checkTaskSpaceTransform(pose))
				leg.setDesiredTaskSpaceTransform(pose, 0);
		}
	}
	void sit(double sitAngle){
		return
	//if(!source.getScriptingName().contains("Kat"))
	//	return
		double incremnt = 0.05
		for(double i=0;i<1;i+=incremnt){
			double angle =  sitAngle*i+(startAngle*(1-i))
			//println "Sitting to "+angle +" from "+startAngle
			def newTF =new TransformNR(0,
						  0,
						  0,
						  new RotationNR(0,
								  0,
								angle
						  )
				      );
			pose(newTF)
			for(def d:source.getAllDHChains()){

				String limbName = d.getScriptingName()
				try{
					if(limbName.contentEquals("Tail")){
						d.setDesiredJointAxisValue(0,// link index
									standardHeadTailAngle+angle/3, //target angle
									0) // 2 seconds
						d.setDesiredJointAxisValue(1,// link index
									0, //target angle
									0) // 2 seconds

					}
					if(limbName.contentEquals("Head")){
						d.setDesiredJointAxisValue(0,// link index
									standardHeadTailAngle-angle/3, //target angle
									0) // 2 seconds
						d.setDesiredJointAxisValue(1,// link index
									0, //target angle
									0) // 2 seconds

					}
					Thread.sleep((long)(stepCycleTime*incremnt/source.getAllDHChains().size()))
					//d.setDesiredTaskSpaceTransform(d.getCurrentTaskSpaceTransform(),  0);

				}catch(Exception e){
					//BowlerStudio.printStackTrace(e)
				}
			}

		}

		startAngle=sitAngle
	}
	private void walkLoop(){
		long incrementTime = (System.currentTimeMillis()-reset)
				if(incrementTime>miliseconds){
					timout = true
				}else
					timout = false

		long time = System.currentTimeMillis()-timeOfLastLoop
		if(time>loopTimingMS){
			//print "\r\nWalk cycle loop time "+(System.currentTimeMillis()-timeOfLastLoop) +" "
			timeOfLastLoop=System.currentTimeMillis()
			walkingCycle()
			//print " Walk cycle took "+(System.currentTimeMillis()-timeOfLastLoop)
		}
		if(reset+walkingTimeout< System.currentTimeMillis()){
			threadDone=true;
			stepResetter=null;
			//if(!source.getScriptingName().contains("Kat")){
				println "FIRING reset from reset thread"
				resetting=true;
				long tmp= reset;
				for(def d:source.getAllDHChains()){
					String limbName = d.getScriptingName()
					try{
						if(limbName.contentEquals("Tail")){
							d.setDesiredJointAxisValue(0,// link index
										standardHeadTailAngle, //target angle
										0) // 2 seconds
							d.setDesiredJointAxisValue(1,// link index
										0, //target angle
										0) // 2 seconds
						}
						if(limbName.contentEquals("Head")){
							d.setDesiredJointAxisValue(0,// link index
										standardHeadTailAngle, //target angle
										0) // 2 seconds
							d.setDesiredJointAxisValue(1,// link index
										0, //target angle
										0) // 2 seconds
						}
						coriolisIndex=0;
					}catch(Exception e){
						BowlerStudio.printStackTrace(e)
					}
				}
				for(int i=0;i<numlegs;i++){
					StepHome(legs.get(i))
				}
				
				resettingindex=numlegs;
				resetting=false;
				return
			//}
			sit(-10);

		}
	}
	public void walkingCycle(){

		//println "Cycle = "+miliseconds+" "+incrementTime

		def upLegs = getUpLegs()
		def downLegs =getDownLegs()
		//println upLegs
		//println downLegs
		for (def leg :upLegs){
			if(leg!=null)
				upStateMachine(leg)

		}
		double gaitTimeRemaining = (double) (System.currentTimeMillis()-timeOfCycleStart)
		double gaitPercentage = gaitTimeRemaining/(double)(stepCycleTime)
		if(!timout){
			double timeRemaining =(double) (System.currentTimeMillis()-reset)
			//double percentage =timeRemaining/ (double)(miliseconds)
			//println "Walk Percent = "+percentage+" of " +miliseconds
			for (def leg :downLegs){
				if(leg!=null)
					downMove( leg, gaitPercentage)
			}
		}


	}
	private void downMove(def leg,double percentage){
		//println "Down Moving to "+percentage
		def pose =compute(leg,percentage,newPose)
		if(leg.checkTaskSpaceTransform(pose))
			leg.setDesiredTaskSpaceTransform(pose, 0);
	}
	private void upStateMachine(def leg){
		def debugFlag =false

		if(leg.getScriptingName().equals("BackRight")){
			debugFlag=true;
		}
		//println "Up Moving to "+percentage
		double gaitTimeRemaining = (double) (System.currentTimeMillis()-timeOfCycleStart)
		double gaitPercentage = gaitTimeRemaining/(double)(stepCycleTime)
		def tf =dynamicHome( leg)
		def NewTmpPose = timout?new TransformNR():newPose.inverse()
		def myPose=timout?new TransformNR():newPose

		switch(walkingState){
		case WalkingState.Rising:
			gaitIntermediatePercentage=gaitPercentage*4.0
			if(gaitIntermediatePercentage>1)
				gaitIntermediatePercentage=1
			//gaitIntermediatePercentage=1
			tf = compute(leg,gaitIntermediatePercentage,myPose)
			tf.setZ(zLock+(stepOverHeight*gaitIntermediatePercentage));
			if(leg.getScriptingName().equals("BackRight")){
				//println tf
			}
			if(gaitPercentage>0.25) {
				walkingState= WalkingState.ToHome
				//println "\n\n\nto Home "
				getUpLegs().each{
					tf = compute(it,gaitIntermediatePercentage,myPose)
					tf.setZ(zLock+(stepOverHeight*gaitIntermediatePercentage));
					if(it.getScriptingName().equals("BackRight")){
						//println tf
					}
					if(it.checkTaskSpaceTransform(tf))
				 		cycleStartPoint.put(it,calcForward(it,tf))
				 	else
				 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
				}
				//computeUpdatePose()
			}else
				break;
		case WalkingState.ToHome:
			gaitIntermediatePercentage=(gaitPercentage-0.25)*4.0
			if(gaitIntermediatePercentage>1)
				gaitIntermediatePercentage=1
			def current = compute(leg,0,myPose)

			def dyHome = dynamicHome( leg)
			//if(gaitIntermediatePercentage<0.9){
				double xinc=(dyHome.getX()-current.getX())*(1-gaitIntermediatePercentage);
				double yinc=(dyHome.getY()-current.getY())*(1-gaitIntermediatePercentage);

				dyHome.translateX(-xinc);
				dyHome.translateY(-yinc);
			//}

			tf=dyHome
			tf.setZ(zLock+(stepOverHeight));
			if(gaitPercentage>0.5) {
				//println "To new target " +gaitIntermediatePercentage
				walkingState= WalkingState.ToNewTarget
				getUpLegs().each{
					tf=dynamicHome( it)
					tf.setZ(zLock+(stepOverHeight));
					if(it.getScriptingName().equals("BackRight")){
						//println tf
					}
					try{
						if(it.checkTaskSpaceTransform(tf))
					 		cycleStartPoint.put(it,calcForward(it,tf))
					 	else
					 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
			 		}catch(Exception ex){
						System.err.println(it.getName()+" cant achive "+tf );
						cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
					}
				}
				//computeUpdatePose()
			}else
				break;
		case WalkingState.ToNewTarget:
			gaitIntermediatePercentage=(gaitPercentage-0.5)*4.0
			if(gaitIntermediatePercentage>1)
				gaitIntermediatePercentage=1
			double localPercent = gaitIntermediatePercentage*((double)numStepCycleGroups)
			//localPercent=((double)numStepCycleGroups-1)
			tf = compute(leg,localPercent,NewTmpPose)
			tf.setZ(zLock+(stepOverHeight));
			if(gaitPercentage>0.75) {
				walkingState= WalkingState.Falling
				//println "Falling " +gaitIntermediatePercentage
				getUpLegs().each{
					tf = compute(it,localPercent,NewTmpPose)
					tf.setZ(zLock+(stepOverHeight));
					try{
						if(it.getScriptingName().equals("BackRight")){
						//println tf
					}
						if(it.checkTaskSpaceTransform(tf))
					 		cycleStartPoint.put(it,calcForward(it,tf))
					 	else
					 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
					}catch(Exception ex){
						System.err.println(it.getName()+" cant achive "+tf );
						cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
					}
				}
				//computeUpdatePose()

			}
			break;
		case WalkingState.Falling:
			gaitIntermediatePercentage=(gaitPercentage-0.75)*4.0
			if(gaitIntermediatePercentage>1)
				gaitIntermediatePercentage=1
			tf = compute(leg,gaitIntermediatePercentage,myPose)
			tf.setZ(zLock+stepOverHeight-(stepOverHeight*gaitIntermediatePercentage));
			//tf.setZ(zLock+stepOverHeight);
			if(gaitPercentage>=1) {
				//tf = dynamicHome( leg)
				walkingState=WalkingState.Rising
				//println "Rising Walk cycle loop time "+(System.currentTimeMillis()-timeOfCycleStart) +" "
				getUpLegs().each{
					tf = compute(it,gaitIntermediatePercentage,myPose)
					tf.setZ(zLock+stepOverHeight-(stepOverHeight*gaitIntermediatePercentage));
					if(it.getScriptingName().equals("BackRight")){
						//println tf
					}
					if(it.checkTaskSpaceTransform(tf))
				 		cycleStartPoint.put(it,calcForward(it,tf))
				 	else
				 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
				}
				getDownLegs().each{
					//def pose =compute(it,1,newPose)
					//if(it.checkTaskSpaceTransform(pose))
				 	//	cycleStartPoint.put(it,calcForward(it,pose))
				 	//else
				 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
				}
				timeOfCycleStart=System.currentTimeMillis()
				stepCycyleActiveIndex++
				if(stepCycyleActiveIndex==numStepCycleGroups){
					stepCycyleActiveIndex=0;
				}
				long start = System.currentTimeMillis()

				computeUpdatePose()
				//println "Compute new pose took : "+(System.currentTimeMillis()-start)
			}
			break;
		}
		//println "Gait Percent = "+gaitIntermediatePercentage

		if(leg.checkTaskSpaceTransform(tf))
			leg.setDesiredTaskSpaceTransform(tf, 0);
		else{
			if(leg.getScriptingName().contains("BackRight")){
			//Log.enableErrorPrint()
			println leg.getScriptingName()+" failed in state "+ walkingState+" "+tf
			}

		}
	}
	private void computeUpdatePose(){
		if (cachedNewPose==null)
			return

		TransformNR n=cachedNewPose;
		double sec=cachedSecond
		cachedNewPose=null

		//n=new TransformNR()
		//stepCycleTime=Math.round(sec*1000)
		numlegs = source.getLegs().size();
		legs = source.getLegs();
		global= source.getFiducialToGlobalTransform();
		if(global==null){
			global=new TransformNR()

		}
		global=new TransformNR(global.getX(),
		  global.getY(),
		  global.getZ(),
		  new RotationNR(Math.toDegrees(n.getRotation().getRotationTilt()),
				  Math.toDegrees(global.getRotation().getRotationAzimuth()),
				 Math.toDegrees( n.getRotation().getRotationElevation())));
		source.setGlobalToFiducialTransform(global)
		n=new TransformNR(n.getX(),
		  n.getY(),
		  n.getZ(),
		  new RotationNR(0,
				  Math.toDegrees(n.getRotation().getRotationAzimuth()),
				  0));
		double percentOfPose=1
		double BodyDisplacement = Math.sqrt(Math.pow(n.getX(),2)+Math.pow(n.getY(),2))/1000
		double speedCalc = BodyDisplacement/sec
		double rotCalc = Math.toDegrees(n.getRotation().getRotationAzimuth())/sec
		stepCycleTime=Math.round(sec*percentOfPose*1000.0)
		if(stepCycleTime<stepOverTime){
			percentOfPose = ((double)stepOverTime)/((double)stepCycleTime)
		}
		try{
			newPose=scaleTransform(n,percentOfPose)
			stepCycleTime=Math.round(sec*percentOfPose*1000.0)
			//println "\n\nTarget at down target displacement = "+BodyDisplacement+" Absolute Velocity "+speedCalc+"m/s and  Z degrees per second= "+rotCalc+" cycle time = "+stepCycleTime

			double cycleMinimumDisplacement = minBodyDisplacementPerStep/(numStepCycleGroups-1)
			while(!newPosePossible(	newPose) &&
				percentOfPose>0.05 &&
				stepCycleTime<stepOverTime){
				percentOfPose-=0.1
				newPose=scaleTransform(n,percentOfPose)
				stepCycleTime=Math.round(sec*percentOfPose*1000.0)
				//println "Speeding up gait to meet speed "+stepCycleTime
			}
			while(newPosePossible(	newPose) &&
				stepCycleTime<stepOverTime){
				percentOfPose+=0.1
				newPose=scaleTransform(n,percentOfPose)
				stepCycleTime=Math.round(sec*percentOfPose*1000.0)
				//println "Speeding up gait to meet speed "+stepCycleTime
			}
			while(getMaximumDisplacement(newPose)< cycleMinimumDisplacement&&
				stepCycleTime<stepCycleTimeMax&&
				newPosePossible(	newPose)
				){
				percentOfPose+=0.75
				stepCycleTime=Math.round(sec*percentOfPose*1000.0)
				newPose=scaleTransform(n,percentOfPose)
				//println "Slowing down up gait to meet speed "+stepCycleTime
			}
			while(!newPosePossible(	newPose) &&
				percentOfPose>0.01 ){
				percentOfPose-=0.1
				newPose=scaleTransform(n,percentOfPose)
				stepCycleTime=stepOverTime
				//println "Speeding up gait to meet speed "+stepCycleTime
			}
		}catch(Exception ex){
			newPose=new TransformNR()
		}

		if(!newPosePossible(	newPose)){
			Log.enableErrorPrint()
			println "\n\n\n\n!!!\nPose not possible\n "+newPose+"\n!!!!\n\n"
			newPose=new TransformNR()
		}

		miliseconds = Math.round(sec*percentOfPose*990)
		BodyDisplacement = Math.sqrt(Math.pow(newPose.getX(),2)+Math.pow(newPose.getY(),2))
		speedCalc = BodyDisplacement/((double)stepCycleTime)
		rotCalc = Math.toDegrees(newPose.getRotation().getRotationAzimuth())/((double)stepCycleTime)*1000.0
		//println  String.format("Actual displacement = %.2f Moving at down target Absolute Velocity %.2f m/s and  Z degrees per second= %.2f cycle time = %d", BodyDisplacement,speedCalc,rotCalc,stepCycleTime);

	}

	private def getLegCurrentPose(def leg){
		double[] joints = cycleStartPoint.get(leg)
		TransformNR armOffset = leg.forwardKinematics(joints)
		return leg.forwardOffset(armOffset);//leg.getCurrentTaskSpaceTransform();
	}
	private TransformNR compute(def leg,double percentage,def bodyMotion){
		def vals = getDisplacementIncrement(leg,bodyMotion)
		TransformNR footStarting=vals[2]
		//apply the increment to the feet
		//println "Feet increments x = "+xinc+" y = "+yinc
		footStarting.translateX(vals[0]*percentage);
		footStarting.translateY(vals[1]*percentage);
		footStarting.setZ(zLock);
		return footStarting
	}
	private boolean newPosePossible(def newPose){
		for(def leg:legs){
			def ok=true
			def linkLocation = calcForward( leg , dynamicHome( leg))
			def linkLocationOld = cycleStartPoint.get(leg)
			double maxDownPercent = 	(numStepCycleGroups-1)
			cycleStartPoint.put(leg, linkLocation)
			def pose =compute(leg,maxDownPercent,newPose)
			if(! leg.checkTaskSpaceTransform(pose))
				ok= false//one of the legs cant reach this pose
			pose.setZ(zLock+(stepOverHeight));
			if(! leg.checkTaskSpaceTransform(pose))
				ok= false//one of the legs cant reach this step over pose
			pose =compute(leg,maxDownPercent,newPose.inverse())
			if(! leg.checkTaskSpaceTransform(pose))
				ok= false//one of the legs cant reach this pose
			pose.setZ(zLock+(stepOverHeight));
			if(! leg.checkTaskSpaceTransform(pose))
				ok= false//one of the legs cant reach this step over pose
			cycleStartPoint.put(leg, linkLocationOld)
			if(!ok)
				return false
		}
		return true;
	}
	/**
	 * Calc Inverse kinematics of a limb .
	 *
	 * @param jointSpaceVect the joint space vect
	 * @return the transform nr
	 */
	public double[] calcForward(DHParameterKinematics leg ,TransformNR transformTarget){
		try{
			def joints= leg.inverseKinematics(leg.inverseOffset(transformTarget));
			return joints
		}catch(Exception ex){
			System.err.println(leg.getName()+" cant achive "+transformTarget );
			return leg.getCurrentJointSpaceVector();
		}
	}
	boolean check(DHParameterKinematics leg,TransformNR newPose){
		TransformNR stepup = newPose.copy();
		stepup.setZ(stepOverHeight + zLock );
		if(!leg.checkTaskSpaceTransform(newPose)){
			return false
		}
		if(!leg.checkTaskSpaceTransform(stepup)){
			return false
		}
		return true
	}
	def dynamicHome(def leg){
		//TODO apply dynamics to home location
		return calcHome(leg).copy()
	}
	private void StepHome(def leg){
		try{
			def home = dynamicHome(leg)
			TransformNR up = home.copy()
			up.setZ(stepOverHeight + zLock )
			TransformNR down = home.copy()
			down.setZ( zLock )
			try {
				// lift leg above home
				//println "lift leg  "+up
				leg.setDesiredTaskSpaceTransform(up, 0);
			} catch (Exception e) {
				//println "Failed to reach "+up
				BowlerStudio.printStackTrace(e);
			}
			ThreadUtil.wait((int)stepOverTime);
			try {
				//step to new target
				//println "step leg down "+down

				leg.setDesiredTaskSpaceTransform(down, 0);
				//set new target for the coordinated motion step at the end
			} catch (Exception e) {
				//println "Failed to reach "+down
				BowlerStudio.printStackTrace(e);
			}
			ThreadUtil.wait((int)stepOverTime);

			cycleStartPoint.put(leg,leg.getCurrentJointSpaceVector())

		}catch(Exception e){
			BowlerStudio.printStackTrace(e)
		}
	}
	private void buildCycleGroups(){
		try{
			for(int i=0;i<numStepCycleGroups;i++){
				if(cycleGroups.get(i)==null){
					def cycleSet = []
					if(numStepCycleGroups==source.getLegs().size()){
						cycleSet.add(source.getLegs().get(i))
					}else if (numStepCycleGroups == 2) {
						int amount = (int)(source.getLegs().size()/numStepCycleGroups)
						if((amount%2)==0){
							for(def leg:source.getLegs()){
								TransformNR  legRoot= leg.getRobotToFiducialTransform()
								if(legRoot.getX()>0&&legRoot.getY()>0 && i==0){
									cycleSet.add(leg)
								}else
								if(legRoot.getX()<0&&legRoot.getY()<0 && i==0){
									cycleSet.add(leg)
								}else
								if(legRoot.getX()>0&&legRoot.getY()<0 && i==1){
									cycleSet.add(leg)
								}else
								if(legRoot.getX()<0&&legRoot.getY()>0 && i==1){
									cycleSet.add(leg)
								}
							}
						}else{
							println "Alternating gait "
							for(int j=0;j<source.getLegs().size();j++){
								if(i==0){
									if((j%2)==0){
										cycleSet.add(source.getLegs().get(j))
									}
								}
								if(i==1){
									if((j%2)!=0){
										cycleSet.add(source.getLegs().get(j))
									}
								}
							}
						}

					}
					//println "Adding "+cycleSet.size()+" to index "+i
					cycleGroups.put(i, cycleSet)
				}
			}
		}catch(Exception e){
			BowlerStudio.printStackTrace(e)
		}
	}
	TransformNR scaleTransform(TransformNR incoming, double scale){
		return new TransformNR(incoming.getX()*scale,
			  incoming.getY()*scale,
			  incoming.getZ()*scale,
			  new RotationNR(Math.toDegrees(incoming.getRotation().getRotationTilt())*scale,
					  Math.toDegrees(incoming.getRotation().getRotationAzimuth())*scale,
					  Math.toDegrees(incoming.getRotation().getRotationElevation())*scale));
	}
	double getMaximumDisplacement(TransformNR bodyMotion){
		double max=0;
		for(def leg:legs){
			def disp =getDisplacement( leg, bodyMotion)
			if(Math.abs(disp)>max){
				max=Math.abs(disp)
			}
		}
		return max
	}
	double getDisplacement(def leg,TransformNR bodyMotion){
		def vals = getDisplacementIncrement(leg,bodyMotion)

		return Math.sqrt(Math.pow(vals[0],2)+Math.pow(vals[1],2))
	}
	def getDisplacementIncrement(def leg,TransformNR bodyMotion){
		double[] joints = cycleStartPoint.get(leg)
		TransformNR armOffset = leg.forwardKinematics(joints)
		TransformNR footStarting = leg.forwardOffset(armOffset);//leg.getCurrentTaskSpaceTransform();
		def myglobal=global.times(bodyMotion);// new global pose
		Matrix btt =  leg.getRobotToFiducialTransform().getMatrixTransform();
		Matrix ftb = myglobal.getMatrixTransform();// our new target
		Matrix current = armOffset.getMatrixTransform();
		Matrix mForward = ftb.times(btt).times(current);
		TransformNR inc =new TransformNR( mForward);
		inc.setZ(zLock);
		double xinc=(footStarting.getX()-inc.getX());
		double yinc=(footStarting.getY()-inc.getY());
		return [xinc,yinc,footStarting]
	}
	@Override
	public void DriveArc(MobileBase source, TransformNR newPose, double seconds) {
		DriveArcLocal( source,  newPose,  seconds,true);
	}
	@Override
	public void DriveVelocityStraight(MobileBase source, double cmPerSecond) {
		// TODO Auto-generated method stub

	}
	@Override
	public void DriveVelocityArc(MobileBase source, double degreesPerSecond,
			double cmRadius) {
		// TODO Auto-generated method stub

	}
	public void DriveArcLocal(MobileBase s, TransformNR n, double sec, boolean retry) {

		try{

			//println "Walk update "+n
			if(s==null){
				println "Null mobile base"
				return
			}
			if(source!=s){
				source=s;
				source.getImu().clearhardwareListeners()
				source.getImu().addhardwareListeners({update ->
					try{
						updateDynamics(update)
					}catch(Exception e){
						e.printStackTrace()
					}
				})
				source.getImu().clearvirtualListeners()
				source.getImu().addvirtualListeners({update ->
					try{
						updateDynamics(update)
					}catch(Exception e){
						e.printStackTrace()
					}
				})
				buildCycleGroups();
			}
			cachedNewPose=n;
			cachedSecond=sec;
			resetStepTimer();

			if(stepResetter==null){

				stepResetter = new Thread(){
					public void run(){
						computeUpdatePose()
						legs.each{
					 		cycleStartPoint.put(it,it.getCurrentJointSpaceVector())
						}
						sit(0);
						timeOfCycleStart= System.currentTimeMillis();
						try{
							threadDone=false;
							walkingState= WalkingState.Rising
							stepCycyleActiveIndex=0;
							println "Starting step reset thread"
							timeOfCycleStart=System.currentTimeMillis()
							while(source.isAvailable() && stepResetter!=null){
								Thread.sleep(1)// avoid thread lock
								try{
									walkLoop();
								}catch(Exception e){
									BowlerStudio.printStackTrace(e)
								}
							}
							println "Finished step reset thread"
						}catch(Exception e){
							BowlerStudio.printStackTrace(e)
						}
					}


				};
				stepResetter.start();
			}

		}catch(Exception e){
			BowlerStudio.printStackTrace(e)
		}
	}

}

moveTest.groovy

import org.apache.commons.io.IOUtils;
import  com.neuronrobotics.bowlerstudio.physics.*;
import com.neuronrobotics.bowlerstudio.threed.*;

def base;
//Check if the device already exists in the device Manager
if(args==null){
	base=DeviceManager.getSpecificDevice( "XLKat_copy",{
			//If the device does not exist, prompt for the connection
			
			MobileBase m = MobileBaseLoader.fromGit(
				"https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git",
				"XLKat_copy.xml"
				)
			if(m==null)
				throw new RuntimeException("Arm failed to assemble itself")
			println "Connecting new device robot arm "+m
			return m
		})
}else
	base=args.get(0)


println "Now we will move just one leg"
DHParameterKinematics leg0 = base.getAllDHChains().get(0)
double zLift=-65
println "Start from where the arm already is and move it from there with absolute location"
TransformNR current = new TransformNR(210.17,0,15.02,new RotationNR(90,0,-45)) ;
//current.translateY(zLift);
current.translateX(-40);
current.translateZ(-zLift);
leg0.setDesiredTaskSpaceTransform(current,  2);

Thread.sleep(4000)

current = new TransformNR(210.17,0,15.02,new RotationNR(90,0,-45)) ;

leg0.setDesiredTaskSpaceTransform(current,  2);
return null;

simplecad.groovy

//Your code here
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import java.nio.file.Paths;
import eu.mihosoft.vrl.v3d.FileUtil;
import eu.mihosoft.vrl.v3d.Transform;
import javafx.scene.transform.Affine;
import com.neuronrobotics.bowlerstudio.physics.TransformFactory;
println "Loading STL file"
return new ICadGenerator(){

	private CSG moveDHValues(CSG incoming,DHLink dh ){
		TransformNR step = new TransformNR(dh.DhStep(0)).inverse()
		Transform move = TransformFactory.nrToCSG(step)
		return incoming.transformed(move)

	}

	@Override
	public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
		
		ArrayList<CSG> allCad=new ArrayList<>();
		ArrayList<DHLink> dhLinks = d.getChain().getLinks()
		DHLink dh = dhLinks.get(linkIndex)
		// Hardware to engineering units configuration
		LinkConfiguration conf = d.getLinkConfiguration(linkIndex);
		// Engineering units to kinematics link (limits and hardware type abstraction)
		AbstractLink abstractLink = d.getAbstractLink(linkIndex);// Transform used by the UI to render the location of the object
		// Transform used by the UI to render the location of the object
		Affine manipulator = dh.getListener();
		Affine lastLinkFrame
		def massKg = conf.getMassKg()
		def centerOfMass = TransformFactory.nrToCSG(conf.getCenterOfMassFromCentroid() )
		def CMvis = new Sphere(500*massKg).toCSG()
					.transformed(centerOfMass)
		
		if(linkIndex==0)
			lastLinkFrame=d.getRootListener()
		else
			lastLinkFrame=dhLinks.get(linkIndex-1).getListener();
		
		def rVal = new Cube(dh.getR()>0?dh.getR():5,1,1).toCSG()
					.toXMax()
					.toZMax()
		rVal.setColor(javafx.scene.paint.Color.RED)
		CSG profile = new Cube(1,// x dimention
					20,// y dimention
				
				1//  Z dimention
				)
				.toCSG()// converts it to a CSG tor display
				.toYMin()
				.toZMin()
				//.toXMin()
				
		CSG theta;
		double thetaval = Math.toDegrees(dh.getTheta())
		if(Math.abs(thetaval)>10){
			theta= CSG.unionAll(
			Extrude.revolve(profile,
						0, // rotation center radius, if 0 it is a circle, larger is a donut. Note it can be negative too
						Math.abs(thetaval),// degrees through wich it should sweep
						(int)10)//number of sweep increments
			)
		}else{
			theta = profile
		}
		
		if(thetaval>0){
			theta= theta.rotz(-thetaval)
		}
		theta= theta.rotz(90)
		.movez(-0.5)
		theta.setColor(thetaval>0?javafx.scene.paint.Color.BLUE:javafx.scene.paint.Color.AQUA)

		CSG alpha;
		double alphaVal = Math.toDegrees(dh.getAlpha())
		if(Math.abs(alphaVal)>10){
			alpha= CSG.unionAll(
			Extrude.revolve(profile,
						0, // rotation center radius, if 0 it is a circle, larger is a donut. Note it can be negative too
						Math.abs(alphaVal),// degrees through wich it should sweep
						(int)10)//number of sweep increments
			)
			//.rotz(alphaVal<0?-alphaVal:0)
		}else{
			alpha = profile
		}
		alpha= alpha.roty(90)
		if(alphaVal>0){
			alpha= alpha.rotx(alphaVal)	
		}
		alpha= alpha
			.rotx(-90)
			.movex(-dh.getR())
		alpha.setColor(alphaVal>0?javafx.scene.paint.Color.YELLOW:javafx.scene.paint.Color.GOLDENROD)

		def dpart = new Cube(1,1,dh.getD()>0?dh.getD():1).toCSG()
					.toZMin()
		double upperLimit = -abstractLink.getMaxEngineeringUnits()
		double lowerLimit = -abstractLink.getMinEngineeringUnits()
		double totalRange = -(upperLimit-lowerLimit)
		def min = 10
		if(totalRange>360-min)
			totalRange=360-min
		if(totalRange<min)
			totalRange=min
		def name = d.getScriptingName()
		//println name
		
		def printit = name.equals("FrontRight")&&linkIndex==0
		if(printit)println "\n\n\nLink range = "+totalRange+" "+upperLimit+" " +lowerLimit
		def rangeComp = 360-totalRange
		def orentationAdjust = -thetaval+90
		def Range
		if(rangeComp>min)
			Range = CSG.unionAll(
			Extrude.revolve(profile,
					0, // rotation center radius, if 0 it is a circle, larger is a donut. Note it can be negative too
					rangeComp,// degrees through wich it should sweep
					(int)min)//number of sweep increments
			)
		else
			Range =profile
		
		Range=Range
			.rotz(lowerLimit+orentationAdjust)
			.movez(-2)

		
		Range.setColor(javafx.scene.paint.Color.LIGHTGREEN)
		def upperLim = profile
					.rotz(upperLimit+orentationAdjust)
					.setColor(javafx.scene.paint.Color.HOTPINK)
		def lowerLim = profile
					.rotz(lowerLimit+orentationAdjust)
					.setColor(javafx.scene.paint.Color.WHITE)
		def zeroLim = profile
					.rotz(orentationAdjust)
					.setColor(javafx.scene.paint.Color.INDIGO)
		def lastFrameParts = [theta,dpart,upperLim,lowerLim,zeroLim,Range]
		def parts = [rVal,alpha,CMvis] as ArrayList<CSG>
		for(int i=0;i<parts.size();i++){
			parts.get(i).setManipulator(manipulator);
			//parts.get(i).setColor(javafx.scene.paint.Color.RED)
		}
		for(int i=0;i<lastFrameParts.size();i++){
			lastFrameParts.get(i).setManipulator(lastLinkFrame);
			//parts.get(i).setColor(javafx.scene.paint.Color.RED)
		}
		parts.addAll(lastFrameParts)
		return parts;

	}



@Override
	public ArrayList<CSG> generateBody(MobileBase b ) {
		ArrayList<CSG> allCad=new ArrayList<>();
		
		def massKg = b.getMassKg()
		def centerOfMass = TransformFactory.nrToCSG(b.getCenterOfMassFromCentroid() )
		def CMvis = new Sphere(500*massKg).toCSG()
					.transformed(centerOfMass)
					
		def centerOfIMU = TransformFactory.nrToCSG(b.getIMUFromCentroid() )
		def IMU = new Cube(5).toCSG()
					.transformed(centerOfIMU)
					.setColor(javafx.scene.paint.Color.WHITE)			
	// Load the .CSG from the disk and cache it in memory
		CSG body  = new Cube(4).toCSG();

		body.setManipulator(b.getRootListener());
		body.setColor(javafx.scene.paint.Color.GREY)
		def parts = [body,IMU, CMvis] as ArrayList<CSG>
		for(int i=0;i<parts.size();i++){
			parts.get(i)
		}
		return parts;
	}
}

XLKat_copy.xml

<root>
<mobilebase>
	<cadEngine>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>simplecad.groovy</file>
	</cadEngine>
	<driveEngine>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>DynamicWalking.groovy</file>
	</driveEngine>

<name>XLKat_copy</name>
<leg>

<name>FrontRight</name>
	<cadEngine>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>simplecad.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>dhsolver3.groovy</file>
	</kinematics>
<link>
	<name>shoulderRoll</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>0</index>
	<scale>0.833</scale>
	<upperLimit>360.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>90.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>105</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>00</x>
	<y>0</y>
	<z>0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

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

</link>
<link>
	<name>shoulderPitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>1</index>
	<scale>0.833</scale>
	<upperLimit>360.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>90.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>105</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>-22.5</Theta>
		<Radius>92.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>kneePitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>2</index>
	<scale>0.833</scale>
	<upperLimit>360.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>117.00623746936958</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>97</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>22.5</Theta>
		<Radius>75.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>anklePitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>3</index>
	<scale>-0.829</scale>
	<upperLimit>360.0</upperLimit>
	<lowerLimit>-360.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>180.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>145</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>45.0</Theta>
		<Radius>71.03</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>73.80952380952382</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>0.7071684852014806</rotw>
	<rotx>-0.0</rotx>
	<roty>0.7070450717866884</roty>
	<rotz>-0.0</rotz>
</baseToZframe>

</leg>
<leg>

<name>BackRight</name>
	<cadEngine>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>simplecad.groovy</file>
	</cadEngine>
	<kinematics>
		<git>https://gist.github.com/76a595d821689e938078ca6c1af9a5f4.git</git>
		<file>dhsolver3.groovy</file>
	</kinematics>
<link>
	<name>shoulderRoll</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>8</index>
	<scale>0.833</scale>
	<upperLimit>100.0</upperLimit>
	<lowerLimit>-100.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>105</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

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

</link>
<link>
	<name>shoulderPitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>9</index>
	<scale>0.833</scale>
	<upperLimit>135.0</upperLimit>
	<lowerLimit>-135.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>0.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>105</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>22.5</Theta>
		<Radius>92.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>kneePitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>10</index>
	<scale>0.833</scale>
	<upperLimit>360.0</upperLimit>
	<lowerLimit>0.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>240.5883270216084</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>97</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>0.0</rotx>
	<roty>0.0</roty>
	<rotz>0.0</rotz></centerOfMassFromCentroid>
	<imuFromCentroid>	<x>0.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>1.0</rotw>
	<rotx>-0.0</rotx>
	<roty>-0.0</roty>
	<rotz>-0.0</rotz></imuFromCentroid>

	<DHParameters>
		<Delta>0.0</Delta>
		<Theta>-22.5</Theta>
		<Radius>75.0</Radius>
		<Alpha>0.0</Alpha>
	</DHParameters>

</link>
<link>
	<name>anklePitch</name>
	<deviceName>hidDevice</deviceName>
	<type>dummy</type>
	<index>11</index>
	<scale>1.0</scale>
	<upperLimit>170.0</upperLimit>
	<lowerLimit>-165.0</lowerLimit>
	<upperVelocity>1.0E8</upperVelocity>
	<lowerVelocity>-1.0E8</lowerVelocity>
	<staticOffset>10.0</staticOffset>
	<isLatch>true</isLatch>
	<indexLatch>145</indexLatch>
	<isStopOnLatch>false</isStopOnLatch>
	<homingTPS>10000000</homingTPS>

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

	</vitamins>
	<passive>false</passive>
	<mass>0.01</mass>
	<centerOfMassFromCentroid>	<x>-30.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</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>45.0</Theta>
		<Radius>95.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>-100.0</x>
	<y>0.0</y>
	<z>0.0</z>
	<rotw>0.7071684852014806</rotw>
	<rotx>-0.0</rotx>
	<roty>0.7070450717866884</roty>
	<rotz>-0.0</rotz>
</baseToZframe>

</leg>

<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>