-
-
Save keionbis/e8fc119f731620c034f766768e7fe62a to your computer and use it in GitHub Desktop.
jaguar copy of DogRobot
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | |
} | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<root> | |
<mobilebase> | |
<cadEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>LinkedCadEngine.groovy</file> | |
</cadEngine> | |
<driveEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>Walking.groovy</file> | |
</driveEngine> | |
<name>jaguar</name> | |
<leg> | |
<name>DefaultLeg</name> | |
<cadEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>LinkedCadEngine.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>DefaultDhSolver.groovy</file> | |
</kinematics> | |
<link> | |
<name>basePan</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>3</index> | |
<scale>0.33</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>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>4.19</Delta> | |
<Theta>0.01</Theta> | |
<Radius>11.45</Radius> | |
<Alpha>-89.97</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>baseTilt</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>0</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>0.0</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>192.7014925373134</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>97</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>elbow</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>1</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>0.0</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>210.31120516818888</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>145</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</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>-50.8</x> | |
<y>27.94</y> | |
<z>66.04</z> | |
<rotw>0.7071684852014806</rotw> | |
<rotx>-0.0</rotx> | |
<roty>0.7070450717866884</roty> | |
<rotz>-0.0</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>DefaultLeg2</name> | |
<cadEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>LinkedCadEngine.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>DefaultDhSolver.groovy</file> | |
</kinematics> | |
<link> | |
<name>basePan</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>4</index> | |
<scale>0.33</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>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>4.19</Delta> | |
<Theta>0.01</Theta> | |
<Radius>11.43</Radius> | |
<Alpha>-89.97</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>baseTilt</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>5</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>0.0</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>211.7313432835821</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>97</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>elbow</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>6</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>15.223880597014928</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>128.0</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>145</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</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>50.8</x> | |
<y>-27.94</y> | |
<z>66.04</z> | |
<rotw>6.171209208740223E-5</rotw> | |
<rotx>-0.7071684825087826</rotx> | |
<roty>6.17013222352696E-5</roty> | |
<rotz>0.7070450690944604</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>DefaultLeg3</name> | |
<cadEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>LinkedCadEngine.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>DefaultDhSolver.groovy</file> | |
</kinematics> | |
<link> | |
<name>basePan</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>7</index> | |
<scale>0.33</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>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>4.19</Delta> | |
<Theta>0.01</Theta> | |
<Radius>11.43</Radius> | |
<Alpha>-89.97</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>baseTilt</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>8</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>0.0</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>183.18656716417908</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>97</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>elbow</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>12</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>15.223880597014928</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>128.0</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>145</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.01</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.01</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>50.8</x> | |
<y>27.94</y> | |
<z>66.04</z> | |
<rotw>6.171209208740223E-5</rotw> | |
<rotx>-0.7071684825087826</rotx> | |
<roty>6.17013222352696E-5</roty> | |
<rotz>0.7070450690944604</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>DefaultLeg4</name> | |
<cadEngine> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>LinkedCadEngine.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/17f2da51abe25e043043946d91610ddd.git</git> | |
<file>DefaultDhSolver.groovy</file> | |
</kinematics> | |
<link> | |
<name>basePan</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>13</index> | |
<scale>0.33</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>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>4.19</Delta> | |
<Theta>0.0</Theta> | |
<Radius>11.43</Radius> | |
<Alpha>-90.0</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>baseTilt</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>14</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>0.0</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>188.3955223880597</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>97</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>38.1</Radius> | |
<Alpha>0.0</Alpha> | |
</DHParameters> | |
</link> | |
<link> | |
<name>elbow</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>15</index> | |
<scale>0.33</scale> | |
<upperLimit>255.0</upperLimit> | |
<lowerLimit>15.223880597014928</lowerLimit> | |
<upperVelocity>1.0E8</upperVelocity> | |
<lowerVelocity>-1.0E8</lowerVelocity> | |
<staticOffset>128.0</staticOffset> | |
<isLatch>true</isLatch> | |
<indexLatch>145</indexLatch> | |
<isStopOnLatch>false</isStopOnLatch> | |
<homingTPS>10000000</homingTPS> | |
<vitamins> | |
<vitamin> | |
<name>electroMechanical</name> | |
<type>hobbyServo</type> | |
<id>DHV56mg_sub_Micro</id> | |
</vitamin> | |
<vitamin> | |
<name>shaft</name> | |
<type>hobbyServoHorn</type> | |
<id>DHV56mg_sub_Micro_1</id> | |
</vitamin> | |
</vitamins> | |
<passive>false</passive> | |
<mass>0.01</mass> | |
<centerOfMassFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>0.0</rotx> | |
<roty>0.0</roty> | |
<rotz>0.0</rotz></centerOfMassFromCentroid> | |
<imuFromCentroid> <x>0.0</x> | |
<y>0.0</y> | |
<z>0.0</z> | |
<rotw>1.0</rotw> | |
<rotx>-0.0</rotx> | |
<roty>-0.0</roty> | |
<rotz>-0.0</rotz></imuFromCentroid> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>38.1</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>-50.8</x> | |
<y>-27.94</y> | |
<z>66.04</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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator; | |
import com.neuronrobotics.bowlerstudio.creature.CreatureLab; | |
import org.apache.commons.io.IOUtils; | |
import com.neuronrobotics.bowlerstudio.vitamins.*; | |
//First we load teh default cad generator script | |
ICadGenerator defaultCadGen=(ICadGenerator) ScriptingEngine | |
.gitScriptRun( | |
"https://github.com/madhephaestus/carl-the-hexapod.git", // git location of the library | |
"ThreeDPrintCad.groovy" , // file to load | |
null | |
) | |
return new ICadGenerator(){ | |
@Override | |
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) { | |
ArrayList<CSG> allCad=defaultCadGen.generateCad(d,linkIndex); | |
//If you want you can add things here | |
//allCad.add(myCSG); | |
return allCad; | |
} | |
@Override | |
public ArrayList<CSG> generateBody(MobileBase b ) { | |
ArrayList<CSG> allCad=defaultCadGen.generateBody(b); | |
//If you want you can add things here | |
//allCad.add(myCSG); | |
return allCad; | |
} | |
}; |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import java.time.Duration; | |
import java.util.ArrayList; | |
import javafx.application.Platform; | |
import org.reactfx.util.FxTimer; | |
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics; | |
import com.neuronrobotics.sdk.addons.kinematics.MobileBase; | |
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR; | |
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR; | |
import com.neuronrobotics.sdk.util.ThreadUtil; | |
import com.neuronrobotics.sdk.addons.kinematics.IDriveEngine; | |
double stepOverHeight=0.1; | |
long stepOverTime=100; | |
Double zLock=-10; | |
Closure calcHome = { DHParameterKinematics leg -> | |
TransformNR h=leg.calcHome() | |
TransformNR tr = leg.forwardOffset(new TransformNR()) | |
tr.setZ(zLock) | |
return tr; | |
} | |
boolean usePhysicsToMove = true; | |
return ScriptingEngine | |
.gitScriptRun( | |
"https://github.com/madhephaestus/carl-the-hexapod.git", // git location of the library | |
"GenericWalkingEngine.groovy" , // file to load | |
[stepOverHeight,stepOverTime,zLock,calcHome,usePhysicsToMove] | |
) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment