Skip to content

Instantly share code, notes, and snippets.

@Octogonapus

Octogonapus/DefaultDhSolver.groovy

Last active February 17, 2018 21:43
Show Gist options
  • Save Octogonapus/edf8b3648e637d8041264e451c4e3321 to your computer and use it in GitHub Desktop.
Save Octogonapus/edf8b3648e637d8041264e451c4e3321 to your computer and use it in GitHub Desktop.
Download ZIP
NASA_Curiosity_copy copy of NASA_Curiosity
Raw
armCad.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import java.nio.file.Paths;
import eu.mihosoft.vrl.v3d.FileUtil;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import javafx.scene.transform.*;
println "Loading STL file"
// Load an STL file from a git repo
// Loading a local file also works here
ICadGenerator cadGen =new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
ArrayList<DHLink> dhLinks = d.getChain().getLinks()
ArrayList<CSG> parts = new ArrayList<>();
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();
if(linkIndex==0){
File mount = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/swivel-bracket.STL");
CSG mountCSG = Vitamins.get(mount)
.rotz(-90)
.roty(-90)
mountCSG.setManipulator(d.getRootListener());
//parts.add(mountCSG)
File swivel = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/swivel.STL");
CSG swivelCSG = Vitamins.get(swivel)
.rotz(90)
.roty(-90)
.roty(180)
.movex(-dh.getR())
.movey(dh.getD())
swivelCSG.setManipulator(manipulator);
parts.add(swivelCSG)
}
if(linkIndex==1){
File swivel = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/upper-arm.STL");
CSG swivelCSG = Vitamins.get(swivel)
.movex(-dh.getR())
.movez(-dh.getD())
swivelCSG.setManipulator(manipulator);
parts.add(swivelCSG)
}
if(linkIndex==2){
File swivel = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/lower-arm.STL");
CSG swivelCSG = Vitamins.get(swivel)
.rotx(180)
.movex(-dh.getR())
.movez(-dh.getD())
swivelCSG.setManipulator(manipulator);
parts.add(swivelCSG)
}
if(linkIndex==3){
File swivel = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/mahli-apxs.STL");
CSG swivelCSG = Vitamins.get(swivel)
.movex(-dh.getR())
.movez(-dh.getD())
swivelCSG.setManipulator(manipulator);
parts.add(swivelCSG)
}
for(int i=0;i<parts.size();i++){
parts.get(i).setColor(javafx.scene.paint.Color.WHITE)
}
return parts
}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {return new ArrayList<>();}
}
return cadGen
Raw
bodyCad.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import java.nio.file.Paths;
import eu.mihosoft.vrl.v3d.FileUtil;
import com.neuronrobotics.bowlerstudio.vitamins.*;
println "Loading STL file"
// Load an STL file from a git repo
// Loading a local file also works here
return new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {return new ArrayList<>()}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {
ArrayList<CSG> allCad=new ArrayList<>();
double size =40;
File mainBodyFile = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/body.STL");
File USP1R_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/upper-suspension-p1-right.STL");
File LSP1R_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/lower-suspension-p1-right.STL");
File USP2R_file= ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/upper-suspension-p2-right.STL");
File USP1L_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/upper-suspension-p1-left.STL");
File LSP1L_file= ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/lower-suspension-p1-left.STL");
File USP2L_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/upper-suspension-p2-left.STL");
//changed
File LSP2R_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/lower-suspension-p2-right.STL");
File LSP2L_file= ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/lower-suspension-p2-left.STL");
/*
double upperHeightAdjust = 2.175
double lowerAdjustAngle = 4.6
double UpperOffset = -78.5
double heightOfUpperHinge = 27+upperHeightAdjust
double offsetOfLowerBracket = 8.5
double LowerOffset = 65.25
double mainWheelAlignemnt=-5
double lowerHingeOffset = 1
*/
double widthOfBody =43
// Load the .CSG from the disk and cache it in memory
CSG body = Vitamins.get(mainBodyFile)
CSG USP1R = Vitamins.get(USP1R_file).movey(widthOfBody)
CSG LSP1R = Vitamins.get(LSP1R_file).movey(widthOfBody)
CSG USP2R = Vitamins.get(USP2R_file).movey(widthOfBody)
CSG LSP2R = Vitamins.get(LSP2R_file).movey(widthOfBody)
CSG USP1L = Vitamins.get(USP1L_file).movey(-widthOfBody)
CSG LSP1L = Vitamins.get(LSP1L_file).movey(-widthOfBody)
CSG USP2L = Vitamins.get(USP2L_file).movey(-widthOfBody)
CSG LSP2L = Vitamins.get(LSP2L_file).movey(-widthOfBody)
/*
FileUtil.write(Paths.get(USP1L_file.getAbsolutePath()),
USP1L.toStlString());
FileUtil.write(Paths.get(USP2L_file.getAbsolutePath()),
USP2L.toStlString());
FileUtil.write(Paths.get(LSP1L_file.getAbsolutePath()),
LSP1L.toStlString());
FileUtil.write(Paths.get(LSP2L_file.getAbsolutePath()),
LSP2L.toStlString());
//right
FileUtil.write(Paths.get(USP1R_file.getAbsolutePath()),
USP1R.toStlString());
FileUtil.write(Paths.get(USP2R_file.getAbsolutePath()),
USP2R.toStlString());
FileUtil.write(Paths.get(LSP1R_file.getAbsolutePath()),
LSP1R.toStlString());
FileUtil.write(Paths.get(LSP2R_file.getAbsolutePath()),
LSP2R.toStlString());
*/
body.setManipulator(b.getRootListener());
body.setColor(javafx.scene.paint.Color.WHITE)
def parts = [body ,USP1R,LSP1R,USP2R,LSP2R,USP1L,LSP1L,USP2L,LSP2L ] as ArrayList<CSG>
for(int i=1;i<parts.size();i++){
parts.get(i).setColor(javafx.scene.paint.Color.GRAY)
}
return parts;
}
};
Raw
DefaultDhSolver.groovy
import java.util.ArrayList;
import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.common.Log;
import Jama.Matrix;
return new DhInverseSolver() {
@Override
public double[] inverseKinematics(TransformNR target,
double[] jointSpaceVector,DHChain chain ) {
ArrayList<DHLink> links = chain.getLinks();
// THis is the jacobian for the given configuration
//Matrix jacobian = chain.getJacobian(jointSpaceVector);
Matrix taskSpacMatrix = target.getMatrixTransform();
int linkNum = jointSpaceVector.length;
double [] inv = new double[linkNum];
// this is an ad-hock kinematic model for d-h parameters and only works for specific configurations
double d = links.get(1).getD()- links.get(2).getD();
double r = links.get(0).getR();
double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(),2)+Math.pow(target.getY(),2));
double angleXYPlaneVect = Math.asin(target.getY()/lengthXYPlaneVect);
double angleRectangleAdjustedXY =Math.asin(d/lengthXYPlaneVect);
double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
if(Math.abs(Math.toDegrees(orentation))<0.01){
orentation=0;
}
double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
double zSet = target.getZ() - links.get(0).getD();
if(links.size()==5){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(4).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(4).getD();
else{
double tipySet = links.get(4).getR()*Math.sin(orentation);
double tipxSet = links.get(4).getR()*Math.cos(orentation);
//println "5 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
if(links.size()==4){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(3).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(3).getR();
else{
double tipySet = links.get(3).getR()*Math.sin(orentation);
double tipxSet = links.get(3).getR()*Math.cos(orentation);
//println "4 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
// Actual target for anylitical solution is above the target minus the z offset
TransformNR overGripper = new TransformNR(
xSet,
ySet,
zSet,
target.getRotation());
double l1 = links.get(1).getR();// First link length
double l2 = links.get(2).getR();
double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
/*
println ( "TO: "+target);
println ( "Trangular TO: "+overGripper);
println ( "lengthXYPlaneVect: "+lengthXYPlaneVect);
println( "angleXYPlaneVect: "+Math.toDegrees(angleXYPlaneVect));
println( "angleRectangleAdjustedXY: "+Math.toDegrees(angleRectangleAdjustedXY));
println( "lengthRectangleAdjustedXY: "+lengthRectangleAdjustedXY);
println( "r: "+r);
println( "d: "+d);
println( "x Correction: "+xSet);
println( "y Correction: "+ySet);
println( "Orentation: "+Math.toDegrees(orentation));
println( "z: "+zSet);
*/
if (vect > l1+l2 || vect<0 ||lengthRectangleAdjustedXY<0 ) {
throw new RuntimeException("Hypotenus too long: "+vect+" longer then "+l1+l2);
}
//from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
double a=l2;
double b=l1;
double c=vect;
double A =Math.acos((Math.pow(b,2)+ Math.pow(c,2) - Math.pow(a,2)) / (2.0*b*c));
double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2.0*a*c));
double C =Math.PI-A-B;//Rule of triangles
double elevation = Math.asin(zSet/vect);
/*
println( "vect: "+vect);
println( "A: "+Math.toDegrees(A));
println( "elevation: "+Math.toDegrees(elevation));
println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
println( "l2 from l1: "+Math.toDegrees(C));
*/
inv[0] = Math.toDegrees(orentation);
inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
if((int)links.get(1).getAlpha() ==180){
inv[2] = (Math.toDegrees(C))-180-//interior angle of the triangle, map to external angle
Math.toDegrees(links.get(2).getTheta());// offset for kinematics
}
if((int)links.get(1).getAlpha() ==0){
inv[2] = -(Math.toDegrees(C))+Math.toDegrees(links.get(2).getTheta());// offset for kinematics
}
if(links.size()>3)
inv[3] =-(inv[1] + inv[2]);// keep it parallell
// We know the wrist twist will always be 0 for this model
if(links.size()>4)
inv[4] = inv[0];//keep the tool orentation paralell from the base
for(int i=0;i<inv.length;i++){
if(Math.abs(inv[i]) < 0.01){
inv[i]=0;
}
// println( "Link#"+i+" is set to "+inv[i]);
}
int i=3;
if(links.size()>3)
i=5;
//copy over remaining links so they do not move
for(;i<inv.length && i<jointSpaceVector.length ;i++){
inv[i]=jointSpaceVector[i];
}
return inv;
}
};
Raw
DriveEngine.groovy
//Your code here
import Jama.Matrix;
import javafx.scene.transform.*;
return new com.neuronrobotics.sdk.addons.kinematics.IDriveEngine (){
public ArrayList<DHParameterKinematics> getAllDHChains(MobileBase source) {
ArrayList<DHParameterKinematics> copy = new ArrayList<DHParameterKinematics>();
for (DHParameterKinematics l : source.getSteerable()) {
copy.add(l);
}
for (DHParameterKinematics l : source.getDrivable()) {
copy.add(l);
}
return copy;
}
@Override
public void DriveArc(MobileBase source, TransformNR newPose, double seconds) {
newPose = newPose.inverse()
ArrayList<DHParameterKinematics> wheels = getAllDHChains( source)
ArrayList<DHParameterKinematics> steerable = source.getSteerable();
for(int i=0;i<wheels.size();i++){
// Get the current pose of the robots base
TransformNR global= source.getFiducialToGlobalTransform();
// set a new one if null
if(global==null){
global=new TransformNR()
source.setGlobalToFiducialTransform(global)
}
global=global.times(newPose);// new global pose
DHParameterKinematics thisWheel =wheels.get(i)
// get the pose of this wheel
TransformNR wheelStarting = thisWheel.getRobotToFiducialTransform();
Matrix btt = wheelStarting.getMatrixTransform();
Matrix ftb = global.getMatrixTransform();// our new target
Matrix mForward = ftb.times(btt)
TransformNR inc =new TransformNR( mForward);// this wheels new increment
TransformNR vect =new TransformNR(btt.inverse().times(mForward));// this wheels new increment
double xyplaneDistance = Math.sqrt(
Math.pow(vect.getX(),2)+
Math.pow(vect.getY(),2)
)
if(Math.abs(xyplaneDistance)<0.01){
xyplaneDistance=0;
}
double steer =90-Math.toDegrees( Math.atan2(vect.getX(),vect.getY()))
boolean reverseWheel = false
if(steer>90){
steer=steer-180
reverseWheel=true;
}
if(steer<-90){
steer=steer+180
reverseWheel=true;
}
ArrayList<DHLink> dhLinks = thisWheel.getChain().getLinks()
int wheelIndex =0;
if(steerable.contains(thisWheel)){
//println ""+i+" XY plane distance "+xyplaneDistance
//println "Steer angle "+steer
try{
thisWheel.setDesiredJointAxisValue(0,steer,0)
}catch(Exception e){
e.printStackTrace(System.out)
}
wheelIndex=1
}
DHLink dh = dhLinks.get(wheelIndex)
// Hardware to engineering units configuration
LinkConfiguration conf = thisWheel.getLinkConfiguration(wheelIndex);
// Engineering units to kinematics link (limits and hardware type abstraction)
AbstractLink abstractLink = thisWheel.getAbstractLink(wheelIndex);// 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();
double radiusOfWheel = dh.getR()
double theta=0
if(Math.abs(xyplaneDistance)>0.01){
theta=Math.toDegrees(xyplaneDistance/radiusOfWheel)*(reverseWheel?-1:1)
}
double currentWheel= thisWheel.getCurrentJointSpaceVector()[wheelIndex]
try{
thisWheel.setDesiredJointAxisValue(wheelIndex,theta+currentWheel,seconds);
}catch(Exception e){
e.printStackTrace(System.out)
}
}
}
@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
}
}
Raw
fixed_wheel.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import java.nio.file.Paths;
import eu.mihosoft.vrl.v3d.FileUtil;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import javafx.scene.transform.*;
println "Loading STL file"
// Load an STL file from a git repo
// Loading a local file also works here
return new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
ArrayList<DHLink> dhLinks = d.getChain().getLinks()
ArrayList<CSG> allCad=new ArrayList<CSG>()
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();
if (linkIndex==0){
File wheel_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/wheel.STL");
File tire_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/tire.STL");
/*
CSG wheel = Vitamins.get(wheel_file)
wheel=wheel
.movex(-wheel.getMaxX()/2)
.movey(-wheel.getMaxY()/2)
.movez(-wheel.getMaxZ()/2)
.rotx(90)
wheel.setManipulator(manipulator)
allCad.add(wheel)
*/
CSG tire = Vitamins.get(tire_file)
.movex(-dh.getR())
.movez(-dh.getD())
tire.setManipulator(manipulator)
allCad.add(tire)
}
for(int i=0;i<allCad.size();i++){
allCad.get(i).setColor(javafx.scene.paint.Color.GRAY)
}
return allCad;
}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {return new ArrayList<>();}
};
Raw
NASA_Curiosity_copy.xml
<root>
<mobilebase>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>bodyCad.groovy</file>
</cadEngine>
<driveEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DriveEngine.groovy</file>
</driveEngine>
<name>NASA_Curiosity_copy</name>
<appendage>
<name>RobotArm</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>armCad.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>4</index>
<scale>0.001</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>235</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.001</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>16.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>5</index>
<scale>0.001</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.001</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>60.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>6</index>
<scale>0.001</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>121</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.001</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>90.0</Theta>
<Radius>50.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<link>
<name>mahliApxs</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>7</index>
<scale>0.001</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.001</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>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>-75.00000000000001</x>
<y>-23.069999999999993</y>
<z>38.0952380952381</z>
<rotw>0.054558923327821106</rotw>
<rotx>0.01790304701018314</rotx>
<roty>0.007757152896105871</roty>
<rotz>0.9983199043252651</rotz>
</baseToZframe>
</appendage>
<steerable>
<name>Front_Left</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>steer_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>fl_steer</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>0</index>
<scale>2.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>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.001</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>fl_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>0</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>133.7089552238806</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.001</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>18.0</Radius>
<Alpha>-90.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>-78.5</x>
<y>-73.75</y>
<z>0.0</z>
<rotw>0.005986580456326229</rotw>
<rotx>-0.7070814300288117</rotx>
<roty>-0.7070782537892785</roty>
<rotz>0.006352572798075492</rotz>
</baseToZframe>
</steerable>
<steerable>
<name>Back_Left</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>steer_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>bl_steer</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>1</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>133.7089552238806</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.001</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>bl_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>2</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>144.84283804856315</staticOffset>
<isLatch>true</isLatch>
<indexLatch>124</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.001</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>18.0</Radius>
<Alpha>-90.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.5</x>
<y>-74.5</y>
<z>3.2862601528904634E-14</z>
<rotw>0.006169813372543178</rotw>
<rotx>-0.7070798601260253</rotx>
<roty>-0.7070798601260253</roty>
<rotz>0.006170592456906135</rotz>
</baseToZframe>
</steerable>
<steerable>
<name>Back_Right</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>steer_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>br_steer</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>2</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>133.7089552238806</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.001</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>br_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>3</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>109.22577411450214</staticOffset>
<isLatch>true</isLatch>
<indexLatch>133</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.001</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>18.0</Radius>
<Alpha>-90.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.5</x>
<y>74.5</y>
<z>0.0</z>
<rotw>0.006170456048332295</rotw>
<rotx>-0.707079857322067</rotx>
<roty>-0.707079857322067</roty>
<rotz>0.006170592432357253</rotz>
</baseToZframe>
</steerable>
<steerable>
<name>Front_Right</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>steer_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>fr_steer</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>3</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>133.7089552238806</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.001</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>fr_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>5</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>120.38805970149252</staticOffset>
<isLatch>true</isLatch>
<indexLatch>135</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.001</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>18.0</Radius>
<Alpha>-90.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>-78.5</x>
<y>73.75</y>
<z>0.0</z>
<rotw>0.005986580456326229</rotw>
<rotx>-0.7070814300288117</rotx>
<roty>-0.7070782537892785</roty>
<rotz>0.006352572798075492</rotz>
</baseToZframe>
</steerable>
<drivable>
<name>Center_Left</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>fixed_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>cl_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>1</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>162.25373134328353</staticOffset>
<isLatch>true</isLatch>
<indexLatch>171</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.001</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>18.0</Radius>
<Alpha>-90.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>-3.1554436208840472E-30</x>
<y>-74.0</y>
<z>1.6431300764452317E-14</z>
<rotw>0.006170592410918338</rotw>
<rotx>0.006170592427160763</rotx>
<roty>0.7070798567270872</roty>
<rotz>0.7070798567270871</rotz>
</baseToZframe>
</drivable>
<drivable>
<name>Center_Right</name>
<cadEngine>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>fixed_wheel.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/edf8b3648e637d8041264e451c4e3321.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>cr_wheel</name>
<deviceName>pid</deviceName>
<type>pid</type>
<index>4</index>
<scale>0.33</scale>
<upperLimit>1000000.0</upperLimit>
<lowerLimit>-1000000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>122.2910447761194</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.001</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>18.0</Radius>
<Alpha>-90.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>3.1554436208840472E-30</x>
<y>74.0</y>
<z>-1.6431300764452317E-14</z>
<rotw>0.006164910275843254</rotw>
<rotx>0.006176274512728991</rotx>
<roty>0.7070797574776199</roty>
<rotz>0.7070799559313109</rotz>
</baseToZframe>
</drivable>
<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.001</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>
Raw
steer_wheel.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import java.nio.file.Paths;
import eu.mihosoft.vrl.v3d.FileUtil;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import javafx.scene.transform.*;
println "Loading STL file"
// Load an STL file from a git repo
// Loading a local file also works here
return new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
ArrayList<DHLink> dhLinks = d.getChain().getLinks()
ArrayList<CSG> allCad=new ArrayList<CSG>()
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();
if (linkIndex==0){
File steer_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/steering-bracket.STL");
CSG steer = Vitamins.get(steer_file)
steer.setManipulator(manipulator)
allCad.add(steer)
}
if (linkIndex==1){
File wheel_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/wheel.STL");
File tire_file = ScriptingEngine.fileFromGit(
"https://github.com/NeuronRobotics/NASACurisoity.git",
"STL/tire.STL");
/*
CSG wheel = Vitamins.get(wheel_file)
wheel=wheel
.movex(-wheel.getMaxX()/2)
.movey(-wheel.getMaxY()/2)
.movez(-wheel.getMaxZ()/2)
.rotx(90)
wheel.setManipulator(manipulator)
allCad.add(wheel)
*/
CSG tire = Vitamins.get(tire_file)
.movex(-dh.getR())
.movez(-dh.getD())
tire.setManipulator(manipulator)
allCad.add(tire)
}
for(int i=0;i<allCad.size();i++){
allCad.get(i).setColor(javafx.scene.paint.Color.GRAY)
}
return allCad;
}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {return new ArrayList<>();}
};
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment