Skip to content

Instantly share code, notes, and snippets.

@keionbis

keionbis/DynamicWalking.groovy

Forked from madhephaestus/DynamicWalking.groovy
Created August 28, 2019 16:15
Show Gist options
  • Save keionbis/e662508ec0981987602a9e368ffd2bc0 to your computer and use it in GitHub Desktop.
Save keionbis/e662508ec0981987602a9e368ffd2bc0 to your computer and use it in GitHub Desktop.
Download ZIP
XLKat_copy copy of XLKat
Raw
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])
Raw
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)
}
}
}
Raw
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;
Raw
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;
}
}
Raw
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>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment