Skip to content

Instantly share code, notes, and snippets.

@lxpk

lxpk/CarlTheRobot.xml

Last active November 24, 2015 06:36
Show Gist options
  • Save lxpk/e77f2d0fc4696a7634be to your computer and use it in GitHub Desktop.
Save lxpk/e77f2d0fc4696a7634be to your computer and use it in GitHub Desktop.
Download ZIP
Carl The Hexapod
Raw
CarlTheRobot.xml
<root>
<mobilebase>
<driveType>walking</driveType>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<driveEngine>
<gist>bcb4760a449190206170</gist>
<file>WalkingDriveEngine.groovy</file>
</driveEngine>
<name>MuttTheRobot</name>
<leg>
<name>Carl_One</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</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>133.7089552238806</staticOffset>
<isLatch>true</isLatch>
<indexLatch>105</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</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>93.74626865671641</staticOffset>
<isLatch>true</isLatch>
<indexLatch>97</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>2</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>15.223880597014928</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>87.01481398975272</staticOffset>
<isLatch>true</isLatch>
<indexLatch>145</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>-42.0</x>
<y>-28.0</y>
<z>0.0</z>
<rotw>0.3826834323650895</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>-0.9238795325112876</rotz>
</baseToZframe>
</leg>
<leg>
<name>Carl_Two</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<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>133.2089552238806</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>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</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>116.0820895522388</staticOffset>
<isLatch>true</isLatch>
<indexLatch>148</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>5</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>30.44776119402985</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>96.30251726442415</staticOffset>
<isLatch>true</isLatch>
<indexLatch>120</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>0.0</x>
<y>-40.0</y>
<z>0.0</z>
<rotw>0.7071067811865476</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>-0.7071067811865475</rotz>
</baseToZframe>
</leg>
<leg>
<name>Carl_Three</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>6</index>
<scale>0.33</scale>
<upperLimit>194.1044776119403</upperLimit>
<lowerLimit>0.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>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</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>91</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>8</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>74.21641791044776</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>115.85781911338827</staticOffset>
<isLatch>true</isLatch>
<indexLatch>139</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>42.0</x>
<y>-26.0</y>
<z>0.0</z>
<rotw>0.9762960071199334</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>-0.21643961393810288</rotz>
</baseToZframe>
</leg>
<leg>
<name>Carl_Four</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>9</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>55.18656716417911</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>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>10</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>82.32835820895522</staticOffset>
<isLatch>true</isLatch>
<indexLatch>137</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>11</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>70.41044776119404</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>128.09940966807753</staticOffset>
<isLatch>true</isLatch>
<indexLatch>131</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>42.0</x>
<y>26.0</y>
<z>0.0</z>
<rotw>0.9799247046208297</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.19936793441719713</rotz>
</baseToZframe>
</leg>
<leg>
<name>Carl_Five</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>12</index>
<scale>0.33</scale>
<upperLimit>201.71641791044777</upperLimit>
<lowerLimit>0.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>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</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>126.09701492537313</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>14</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>81.82835820895522</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>148.3079750501225</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>0.0</x>
<y>40.0</y>
<z>0.0</z>
<rotw>0.7071067811865476</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.7071067811865475</rotz>
</baseToZframe>
</leg>
<leg>
<name>Carl_Six</name>
<cadEngine>
<gist>bcb4760a449190206170</gist>
<file>ThreeDPrintCad.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>15</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.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>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>16</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>108.97014925373135</staticOffset>
<isLatch>true</isLatch>
<indexLatch>120</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>17</index>
<scale>0.33</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>58.992537313432834</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>124.79048785921142</staticOffset>
<isLatch>true</isLatch>
<indexLatch>141</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>-90.0</Theta>
<Radius>80.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>-42.0</x>
<y>28.0</y>
<z>0.0</z>
<rotw>0.3826834323650895</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.9238795325112876</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>
</mobilebase>
</root>
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()>4){
zSet+=links.get(4).getD();
}
// 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()));
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(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 camera 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
GenericWalkingEngine.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;
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);
TransformNR [] home=null;
TransformNR previousGLobalState;
TransformNR target;
RotationNR rot;
int resettingindex=0;
private long reset = System.currentTimeMillis();
MobileBase source;
Thread stepResetter=null;
public void resetStepTimer(){
reset = System.currentTimeMillis();
}
@Override
public void DriveArc(MobileBase source, TransformNR newPose, double seconds) {
if(stepResetter==null){
stepResetter = new Thread(){
public void run(){
println "Starting step reset thread"
int numlegs = source.getLegs().size();
ArrayList<DHParameterKinematics> legs = source.getLegs();
while(source.isAvailable()){
ThreadUtil.wait(10);
if(reset+5000 < System.currentTimeMillis()){
println "FIRING reset from reset thread"
resetting=true;
long tmp= reset;
if(home==null){
home = new TransformNR[numlegs];
for(int i=0;i<numlegs;i++){
//home[i] = legs.get(i).forwardOffset(new TransformNR());
home[i] =calcHome(legs.get(i))
}
}
for(int i=0;i<numlegs;i++){
TransformNR up = home[i].copy()
up.setZ(stepOverHeight + zLock )
TransformNR down = home[i].copy()
down.setZ( zLock )
try {
// lift leg above home
println "lift leg "+i
legs.get(i).setDesiredTaskSpaceTransform(up, 0);
} catch (Exception e) {
println "Failed to reach "+up
e.printStackTrace();
}
ThreadUtil.wait((int)stepOverTime);
try {
//step to new target
println "step leg "+i
legs.get(i).setDesiredTaskSpaceTransform(down, 0);
//set new target for the coordinated motion step at the end
} catch (Exception e) {
println "Failed to reach "+down
e.printStackTrace();
}
}
resettingindex=numlegs;
resetting=false;
println "Legs all reset legs"
while(source.isAvailable() && tmp==reset){
ThreadUtil.wait(10);
}
}
}
}
};
stepResetter.start();
}
resetStepTimer();
try{
int numlegs = source.getLegs().size();
TransformNR [] feetLocations = new TransformNR[numlegs];
TransformNR [] newFeetLocations = new TransformNR[numlegs];
ArrayList<DHParameterKinematics> legs = source.getLegs();
if(home==null){
Log.enableSystemPrint(true)
home = new TransformNR[numlegs];
for(int i=0;i<numlegs;i++){
//home[i] = legs.get(i).forwardOffset(new TransformNR());
home[i] =calcHome(legs.get(i))
println "Home for link "+i+" is "+home[i]
}
}
// Load in the locations of the tips of each of the feet.
for(int i=0;i<numlegs;i++){
feetLocations[i]=legs.get(i).getCurrentPoseTarget();
if(zLock==null){
//sets a standard plane at the z location of the first leg.
zLock=feetLocations[i].getZ();
println "ZLock level set to "+zLock
}
home[i] =calcHome(legs.get(i))
feetLocations[i].setZ(home[i].getZ());
}
//zLock =zLock+newPose.getZ();
previousGLobalState = source.getFiducialToGlobalTransform().copy();
//newPose.setY(0);
target= newPose.copy();
//Apply transform to each dimention of current pose
double el = newPose.getRotation().getRotationElevation() ;
double ti = newPose.getRotation().getRotationTilt() ;
TransformNR global= source.getFiducialToGlobalTransform().times(newPose);
// New target calculated appliaed to global offset
source.setGlobalToFiducialTransform(global);
//Set it back to where it was to use the interpolator for global move at the end
for(int i=0;i<numlegs;i++){
double footx,footy;
newFeetLocations[i]=legs.get(i).getCurrentPoseTarget();
// start by storing where the feet are
if(!legs.get(i).checkTaskSpaceTransform(feetLocations[i])){
//perform the step over
home[i] =calcHome(legs.get(i))
//println "Leg "+i+" setep over to x="+feetLocations[i].getX()+" y="+feetLocations[i].getY()
if(resetting)
System.out.println("\r\nWaiting for "+resettingindex+" reset to finish...")
while(resetting && resettingindex==i && source.isAvailable()){
ThreadUtil.wait(10);
}
println i+" foot reset needed "+feetLocations[i].getX()+" y:"+feetLocations[i].getY()
feetLocations[i].setZ(zLock);
feetLocations[i].setX(home[i].getX());
feetLocations[i].setY(home[i].getY());
int j=0;
println i+" Step over location"+feetLocations[i].getX()+" y:"+feetLocations[i].getY()
double xunit;
double yunit ;
TransformNR lastGood= feetLocations[i].copy();
TransformNR stepup = feetLocations[i].copy();
TransformNR stepUnit = feetLocations[i].copy();
stepup.setZ(stepOverHeight + zLock );
while(legs.get(i).checkTaskSpaceTransform(feetLocations[i]) &&
legs.get(i).checkTaskSpaceTransform(stepup) &&
legs.get(i).checkTaskSpaceTransform(stepUnit) &&
j<10000){
feetLocations[i].setZ(zLock );
stepUnit=lastGood;
lastGood=feetLocations[i].copy();
//get the orientation of the base and invert it
TransformNR inverseRot =new TransformNR(0,0,0,source.getFiducialToGlobalTransform().getRotation()).inverse()
//transform the feet by the inverse orientation
TransformNR rotPose=inverseRot.times(feetLocations[i]);
//invert the target pose
TransformNR rotPoseinv = newPose.inverse();
//apply the inverted target, then un-do the orientation inversion to get final location
TransformNR incr =inverseRot.inverse().times(rotPoseinv.times(rotPose));
//now calculate a a unit vector increment
double xinc=(feetLocations[i].getX()-incr.getX())/1;
double yinc=(feetLocations[i].getY()-incr.getY())/1;
//apply the increment to the feet
feetLocations[i].translateX(xinc);
feetLocations[i].translateY(yinc);
j++;
stepup = feetLocations[i].copy();
stepup.setZ(stepOverHeight + zLock );
}
println i+" furthest availible x:"+lastGood.getX()+" y:"+lastGood.getY()
//step back one unit vector to get to acheivable location
feetLocations[i]=stepUnit;
stepup = stepUnit.copy();
stepup.setZ(stepOverHeight + zLock );
println i+" new step y:"+feetLocations[i].getX()+" y:"+feetLocations[i].getY()
DHParameterKinematics leg = legs.get(i);
resetting=true;
// new Thread(){
// public void run(){
resettingindex=i;
try {
// lift leg above home
println "lift leg "+resettingindex
leg.setDesiredTaskSpaceTransform(stepup, 0);
} catch (Exception e) {
println "Failed to reach "+stepup
e.printStackTrace();
}
ThreadUtil.wait((int)stepOverTime);
try {
//step to new target
println "step leg "+resettingindex
leg.setDesiredTaskSpaceTransform(lastGood, 0);
//set new target for the coordinated motion step at the end
} catch (Exception e) {
println "Failed to reach "+lastGood
e.printStackTrace();
}
ThreadUtil.wait((int)stepOverTime/2);
System.out.println(" Reset "+resettingindex+" Done!\r\n")
resetting=false;
resettingindex=numlegs;
// }
// }.start();
}
resetStepTimer();
}
for(int i=0;i<numlegs;i++){
if(!legs.get(i).checkTaskSpaceTransform(feetLocations[i])){
throw new RuntimeException(i + " leg foot locatrion is not acheivable "+newPose);
}
}
//all legs have a valid target set, perform coordinated motion
for(int i=0;i<numlegs;i++){
legs.get(i).setDesiredTaskSpaceTransform(feetLocations[i], seconds);
}
// while(resetting && source.isAvailable()){
// //System.out.println("Waiting...")
// ThreadUtil.wait(100);
// }
}catch (Exception ex){
ex.printStackTrace();
println "This step is not possible, undoing "
// New target calculated appliaed to global offset
source.setGlobalToFiducialTransform(source.getFiducialToGlobalTransform().times(newPose.inverse()));
//Set it back to where it was to use the interpolator for global move at the end
return;
}
}
@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
LaserCutCad.groovy
import eu.mihosoft.vrl.v3d.Extrude;
import java.io.File;
import java.io.IOException;
import java.net.URISyntaxException;
import java.nio.file.Paths;
import java.util.ArrayList;
import javafx.scene.paint.Color;
import javax.vecmath.Matrix4d;
import Jama.Matrix;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.nrconsole.plugin.BowlerCam.RGBSlider.ColorBox;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
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.common.Log;
import com.neuronrobotics.bowlerstudio.vitamins.IVitamin;
import com.neuronrobotics.bowlerstudio.vitamins.MicroServo;
import com.neuronrobotics.bowlerstudio.vitamins.Vitamins;
import eu.mihosoft.vrl.v3d.CSG;
import eu.mihosoft.vrl.v3d.Cube;
import eu.mihosoft.vrl.v3d.FileUtil;
import eu.mihosoft.vrl.v3d.STL;
import eu.mihosoft.vrl.v3d.Sphere;
import eu.mihosoft.vrl.v3d.Transform;
import eu.mihosoft.vrl.v3d.Cylinder;
import eu.mihosoft.vrl.v3d.Vector3d;
import javafx.scene.paint.Color;
/**
* This cad script generates a 1:10 scale rendering of a robot made from flat cut pieces
* @author hephaestus
*
*/
return new ICadGenerator(){
double scaleOfRobot= 1.0/10.0;
double boardThickness = 18.0*scaleOfRobot;
//CSG servoReference= new MicroServo().toCSG();
CSG servoReference= Vitamins.get("smallservo.stl")
.transformed(new Transform().rotZ(-90))
// .transformed(new Transform().translateZ(12.0))
// .transformed(new Transform().translateX(5.4));
//CSG horn= STL.file(NativeResource.inJarLoad(IVitamin.class,"smallmotorhorn.stl").toPath())
CSG horn = new Cube(6,4,18).toCSG();
private double attachmentRodWidth=10;
private double attachmentBaseWidth=15;
private double printerTollerence =0.5;
double cylandarRadius = 14;
private CSG reverseDHValues(CSG incoming,DHLink dh ){
return incoming
.transformed(new Transform().rotX(-Math.toDegrees(dh.getAlpha())))
//.transformed(new Transform().rotZ(Math.toDegrees(dh.getTheta())))
}
private CSG moveDHValues(CSG incoming,DHLink dh ){
return incoming.transformed(new Transform().translateZ(-dh.getD()))
.transformed(new Transform().rotZ(-Math.toDegrees(dh.getTheta())))
.transformed(new Transform().rotZ((90+Math.toDegrees(dh.getTheta()))))
.transformed(new Transform().translateX(-dh.getR()))
.transformed(new Transform().rotX(Math.toDegrees(dh.getAlpha())));
}
ArrayList<CSG> generateBodyParts(MobileBase base ,boolean printing){
ArrayList<CSG> allCad=new ArrayList<>();
ArrayList<Vector3d> points=new ArrayList<>();
ArrayList<CSG> cutouts=new ArrayList<>();
for(DHParameterKinematics l:base.getAllDHChains()){
TransformNR position = l.getRobotToFiducialTransform();
RotationNR rot = position.getRotation()
Matrix vals =position.getMatrixTransform();
double [] elemenents = [
vals.get(0, 0),
vals.get(0, 1),
vals.get(0, 2),
vals.get(0, 3),
vals.get(1, 0),
vals.get(1, 1),
vals.get(1, 2),
vals.get(1, 3),
vals.get(2, 0),
vals.get(2, 1),
vals.get(2, 2),
vals.get(2, 3),
vals.get(3, 0),
vals.get(3, 1),
vals.get(3, 2),
vals.get(3, 3),
] as double[];
Matrix4d rotation= new Matrix4d(elemenents);
points.add(new Vector3d(position.getX(), position.getY()));
}
double distance = 5
CSG upperBody = Extrude.points( new Vector3d(0, 0, attachmentBaseWidth/2),
points)
CSG lowerBody = Extrude.points( new Vector3d(0, 0,attachmentBaseWidth/2),
points
)
.movez(-attachmentBaseWidth/2)
for(CSG c:cutouts){
upperBody= upperBody.difference(c);
lowerBody= lowerBody.difference(c);
//allCad.add(c)
}
if(!printing){
upperBody.setColor(Color.CYAN);
lowerBody.setColor(Color.ALICEBLUE);
upperBody.setManipulator(base.getRootListener());
lowerBody.setManipulator(base.getRootListener());
}else{
upperBody=upperBody
.transformed(new Transform().rotX(180))
.toZMin()
lowerBody= lowerBody
.toZMin()
}
allCad.addAll(upperBody,
lowerBody
)
return allCad;
}
ArrayList<CSG> generateBody(MobileBase base ){
ArrayList<CSG> allCad=new ArrayList<>();
//Start by generating the legs using the DH link based generator
for(DHParameterKinematics l:base.getAllDHChains()){
for(CSG csg:generateCad(l.getChain().getLinks())){
allCad.add(csg);
}
}
try{
//now we genrate the base pieces
for(CSG csg:generateBodyParts( base ,false)){
allCad.add(csg);
}
}catch (Exception ex){
ex.printStackTrace();
}
return allCad;
}
ArrayList<File> generateStls(MobileBase base , File baseDirForFiles ){
ArrayList<File> allCadStl = new ArrayList<>();
int leg=0;
//Start by generating the legs using the DH link based generator
for(DHParameterKinematics l:base.getAllDHChains()){
int link=0;
for(CSG csg:generateCad(l.getChain().getLinks(),true)){
File dir = new File(baseDirForFiles.getAbsolutePath()+"/"+base.getScriptingName()+"/"+l.getScriptingName())
if(!dir.exists())
dir.mkdirs();
File stl = new File(dir.getAbsolutePath()+"/Leg_"+leg+"_part_"+link+".stl");
FileUtil.write(
Paths.get(stl.getAbsolutePath()),
csg.toStlString()
);
allCadStl.add(stl);
link++;
}
leg++;
}
int link=0;
//now we genrate the base pieces
for(CSG csg:generateBodyParts( base,true )){
File dir = new File(baseDirForFiles.getAbsolutePath()+"/"+base.getScriptingName()+"/")
if(!dir.exists())
dir.mkdirs();
File stl = new File(dir.getAbsolutePath()+"/Body_part_"+link+".stl");
FileUtil.write(
Paths.get(stl.getAbsolutePath()),
csg.toStlString()
);
allCadStl.add(stl);
link++;
}
return allCadStl;
}
public ArrayList<CSG> generateCad(ArrayList<DHLink> dhLinks ){
return generateCad(dhLinks ,false);
}
public ArrayList<CSG> generateCad(ArrayList<DHLink> dhLinks,boolean printBed ){
ArrayList<CSG> csg = new ArrayList<CSG>();
return csg;
}
};
Raw
ThreeDPrintCad.groovy
import eu.mihosoft.vrl.v3d.Extrude;
import java.io.File;
import java.io.IOException;
import java.net.URISyntaxException;
import java.nio.file.Paths;
import java.util.ArrayList;
import javafx.scene.paint.Color;
import javax.vecmath.Matrix4d;
import Jama.Matrix;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.imageprovider.NativeResource;
import com.neuronrobotics.nrconsole.plugin.BowlerCam.RGBSlider.ColorBox;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
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.common.Log;
import com.neuronrobotics.bowlerstudio.vitamins.IVitamin;
import com.neuronrobotics.bowlerstudio.vitamins.MicroServo;
import com.neuronrobotics.bowlerstudio.vitamins.Vitamins;
import eu.mihosoft.vrl.v3d.CSG;
import eu.mihosoft.vrl.v3d.Cube;
import eu.mihosoft.vrl.v3d.FileUtil;
import eu.mihosoft.vrl.v3d.RoundedCube
import eu.mihosoft.vrl.v3d.STL;
import eu.mihosoft.vrl.v3d.Sphere;
import eu.mihosoft.vrl.v3d.Transform;
import eu.mihosoft.vrl.v3d.Cylinder;
import eu.mihosoft.vrl.v3d.Vector3d;
import javafx.scene.paint.Color;
return new ICadGenerator(){
//CSG servoReference= new MicroServo().toCSG();
CSG servoReference= (CSG)(ScriptingEngine.inlineGistScriptRun("3f9fef17b23acfadf3f7", "servo.groovy" , null))
.transformed(new Transform().rotZ(-90))
// .transformed(new Transform().translateZ(12.0))
// .transformed(new Transform().translateX(5.4));
//CSG horn= STL.file(NativeResource.inJarLoad(IVitamin.class,"smallmotorhorn.stl").toPath())
CSG horn = new Cube(6,5,19).toCSG();
private double attachmentRodWidth=10;
private double attachmentBaseWidth=15;
private double printerTollerence =0.5;
double cylandarRadius = 14;
private CSG toZMin(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateZ(-target.getBounds().getMin().z));
}
private CSG toZMax(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateZ(-target.getBounds().getMax().z));
}
private CSG toXMin(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateX(-target.getBounds().getMin().x));
}
private CSG toXMax(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateX(-target.getBounds().getMax().x));
}
private CSG toYMin(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateY(-target.getBounds().getMin().y));
}
private CSG toYMax(CSG incoming,CSG target){
return incoming.transformed(new Transform().translateY(-target.getBounds().getMax().y));
}
private CSG toZMin(CSG incoming){
return toZMin(incoming,incoming);
}
private CSG toZMax(CSG incoming){
return toZMax(incoming,incoming);
}
private CSG toXMin(CSG incoming){
return toXMin(incoming,incoming);
}
private CSG toXMax(CSG incoming){
return toXMax(incoming,incoming);
}
private CSG toYMin(CSG incoming){
return toYMin(incoming,incoming);
}
private CSG toYMax(CSG incoming){
return toYMax(incoming,incoming);
}
private CSG makeKeepaway(CSG incoming){
double x = Math.abs(incoming.getBounds().getMax().x )+ Math.abs(incoming.getBounds().getMin().x)
double y = Math.abs(incoming.getBounds().getMax().y) + Math.abs(incoming.getBounds().getMin().y)
double z = Math.abs(incoming.getBounds().getMax().z )+ Math.abs(incoming.getBounds().getMin().z)
double xtol=(x+printerTollerence)/x
double ytol= (y+printerTollerence)/y
double ztol=(z+printerTollerence)/z
double xPer=-(Math.abs(incoming.getBounds().getMax().x)-Math.abs(incoming.getBounds().getMin().x))/x
double yPer=-(Math.abs(incoming.getBounds().getMax().y)-Math.abs(incoming.getBounds().getMin().y))/y
double zPer=-(Math.abs(incoming.getBounds().getMax().z)-Math.abs(incoming.getBounds().getMin().z))/z
//println " Keep away x = "+y+" new = "+ytol
return incoming
.transformed(new Transform().scale(xtol,
ytol,
ztol ))
.transformed(new Transform().translateX(printerTollerence * xPer))
.transformed(new Transform().translateY(printerTollerence*yPer))
.transformed(new Transform().translateZ(printerTollerence*zPer))
}
private CSG getAppendageMount(){
double cylindarKeepawayHeight = 80;
CSG attachmentbase =new Cylinder(// The first part is the hole to put the screw in
40,
cylindarKeepawayHeight,
(int)20).toCSG()
.toXMin()
.transformed(new Transform().translateX(-cylandarRadius*1.2))
.transformed(new Transform().translateZ(-cylindarKeepawayHeight/2))
return attachmentbase;
}
private CSG getMountScrewKeepaway(){
CSG screw = new Cylinder(// The first part is the hole to put the screw in
7.5/2,
200,
(int)20).toCSG()
screw =screw.union(new Cylinder(// This the the tapper section in the fasening part
4.1/2,
7.5/2,
3,
(int)20).toCSG().toZMax()
).toZMin()
screw =screw.union(new Cylinder(// This the the hole in the fasening part
4.1/2,
3,
(int)20).toCSG().toZMax()
).toZMin()
screw =screw.union(new Cylinder(// This the the hole in the threaded part
2.6/2,
30,
(int)20).toCSG().toZMax()
)
return screw;
}
private CSG getAttachment(){
CSG attachmentbase = new RoundedCube(attachmentBaseWidth,attachmentBaseWidth,4)
.cornerRadius(attachmentBaseWidth/10)
.noCenter()
.toCSG()
.movex(-attachmentBaseWidth/2)
.movey(-(attachmentBaseWidth/2))
CSG cutOffBottomOfAttachment =new Cube( (attachmentBaseWidth-attachmentRodWidth)/2,
attachmentBaseWidth,
10)
.toCSG()
.movex(-attachmentBaseWidth/2+(attachmentBaseWidth-attachmentRodWidth)/4)
.rotz(-90)
attachmentbase=attachmentbase.difference(cutOffBottomOfAttachment)
CSG post = toZMin(new Cube( attachmentRodWidth,
attachmentRodWidth,
Math.abs(servoReference.getBounds().getMax().x)+5+attachmentRodWidth/2)
.toCSG());
attachmentbase = toZMax(attachmentbase.union(post))
.transformed(new Transform().translateZ( attachmentRodWidth/2));
CSG hornAttach =toZMin(toYMin( toYMax( toZMax(horn).transformed(new Transform().translateZ( 4))) ,
post),
post
);
attachmentbase =attachmentbase.difference(hornAttach);
double pinMax = 3;
double pinMin =3;
CSG bearingPin =toYMax( new Cylinder(pinMax,pinMin, (int)5 ,(int)50).toCSG()
.transformed(new Transform().rotX(90)),
post);
attachmentbase =attachmentbase.difference(bearingPin);
return attachmentbase.transformed(new Transform().rot(-90, -90, 0));
}
private CSG getFoot(){
CSG attach = getAttachment();
CSG foot = new Sphere(attachmentRodWidth).toCSG();
return toXMax(attach.union(foot));
}
Transform convertTransform(TransformNR incoming){
}
private CSG reverseDHValues(CSG incoming,DHLink dh ){
return incoming
.transformed(new Transform().rotX(-Math.toDegrees(dh.getAlpha())))
//.transformed(new Transform().rotZ(Math.toDegrees(dh.getTheta())))
}
private CSG moveDHValues(CSG incoming,DHLink dh ){
return incoming.transformed(new Transform().translateZ(-dh.getD()))
.transformed(new Transform().rotZ(-Math.toDegrees(dh.getTheta())))
.transformed(new Transform().rotZ((90+Math.toDegrees(dh.getTheta()))))
.transformed(new Transform().translateX(-dh.getR()))
.transformed(new Transform().rotX(Math.toDegrees(dh.getAlpha())));
}
int minz= 1000000;
int maxz=-1000000;
int minx= 1000000;
int maxx=-1000000;
int miny= 1000000;
int maxy=-1000000;
ArrayList<CSG> generateBodyParts(MobileBase base ,boolean printing){
ArrayList<CSG> allCad=new ArrayList<>();
ArrayList<Vector3d> points=new ArrayList<>();
ArrayList<CSG> cutouts=new ArrayList<>();
ArrayList<CSG> attach=new ArrayList<>();
for(DHParameterKinematics l:base.getAllDHChains()){
TransformNR position = l.getRobotToFiducialTransform();
RotationNR rot = position.getRotation()
Matrix vals =position.getMatrixTransform();
double [] elemenents = [
vals.get(0, 0),
vals.get(0, 1),
vals.get(0, 2),
vals.get(0, 3),
vals.get(1, 0),
vals.get(1, 1),
vals.get(1, 2),
vals.get(1, 3),
vals.get(2, 0),
vals.get(2, 1),
vals.get(2, 2),
vals.get(2, 3),
vals.get(3, 0),
vals.get(3, 1),
vals.get(3, 2),
vals.get(3, 3),
] as double[];
Matrix4d rotation= new Matrix4d(elemenents);
double xpos = position.getX();
cutouts.add(getAppendageMount()
.transformed(new Transform(rotation))
);
CSG attachment = getAttachment()
.transformed(new Transform(rotation))
int thisMinz = attachment.getBounds().getMin().z;
int thisMaxz = attachment.getBounds().getMax().z;
if(thisMinz<minz)
minz=thisMinz
if(thisMaxz>maxz)
maxz=thisMaxz
int thisMiny = attachment.getBounds().getMin().y
int thisMaxy = attachment.getBounds().getMax().y
if(thisMiny<miny)
miny=thisMiny
if(thisMaxy>maxy)
maxy=thisMaxy
int thisMinx = attachment.getBounds().getMin().x;
int thisMaxx = attachment.getBounds().getMax().x;
if(thisMinx<minx)
minx=thisMinx
if(thisMaxx>maxx)
maxx=thisMaxx
attach.add(attachment);
points.add(new Vector3d(position.getX(), position.getY()));
}
int heightOfBody=(maxz-minz+2);
int widthOfBody=(maxx-minx+2);
int depthOfBody=(maxy-miny+2);
println "Height= "+ heightOfBody+ " widthOfBody= "+ widthOfBody+" depthOfBody= "+ depthOfBody
CSG upperBody = new Cube( widthOfBody,// X dimention
depthOfBody,// Y dimention
heightOfBody// Z dimention
)
.noCenter()
.toCSG()
.movez(minz-1)
.movey(miny-1)
.movex(minx-1)
for(CSG c:cutouts){
upperBody= upperBody.difference(c);
//allCad.add(c)
}
for(CSG c:attach){
upperBody= upperBody.union(c);
//allCad.add(c)
}
if(!printing){
upperBody.setColor(Color.CYAN);
upperBody.setManipulator(base.getRootListener());
}else{
upperBody=upperBody
.transformed(new Transform().rotX(180))
.toZMin()
}
allCad.add(upperBody)
return allCad;
}
ArrayList<CSG> generateBody(MobileBase base ){
ArrayList<CSG> allCad=new ArrayList<>();
//Start by generating the legs using the DH link based generator
for(DHParameterKinematics l:base.getAllDHChains()){
for(CSG csg:generateCad(l.getChain().getLinks())){
allCad.add(csg);
}
}
try{
//now we genrate the base pieces
for(CSG csg:generateBodyParts( base ,false)){
allCad.add(csg);
}
}catch (Exception ex){
}
return allCad;
}
ArrayList<File> generateStls(MobileBase base , File baseDirForFiles ){
ArrayList<File> allCadStl = new ArrayList<>();
int leg=0;
//Start by generating the legs using the DH link based generator
for(DHParameterKinematics l:base.getAllDHChains()){
int link=0;
for(CSG csg:generateCad(l.getChain().getLinks(),true)){
File dir = new File(baseDirForFiles.getAbsolutePath()+"/"+base.getScriptingName()+"/"+l.getScriptingName())
if(!dir.exists())
dir.mkdirs();
File stl = new File(dir.getAbsolutePath()+"/Leg_"+leg+"_part_"+link+".stl");
FileUtil.write(
Paths.get(stl.getAbsolutePath()),
csg.toStlString()
);
allCadStl.add(stl);
link++;
}
leg++;
}
int link=0;
//now we genrate the base pieces
for(CSG csg:generateBodyParts( base,true )){
File dir = new File(baseDirForFiles.getAbsolutePath()+"/"+base.getScriptingName()+"/")
if(!dir.exists())
dir.mkdirs();
File stl = new File(dir.getAbsolutePath()+"/Body_part_"+link+".stl");
FileUtil.write(
Paths.get(stl.getAbsolutePath()),
csg.toStlString()
);
allCadStl.add(stl);
link++;
}
return allCadStl;
}
public ArrayList<CSG> generateCad(ArrayList<DHLink> dhLinks ){
return generateCad(dhLinks ,false);
}
public ArrayList<CSG> generateCad(ArrayList<DHLink> dhLinks,boolean printBed ){
//printBed=true;
ArrayList<CSG> csg = new ArrayList<CSG>();
DHLink dh = dhLinks.get(0);
CSG rootAttachment=getAttachment();
//CSG rootAttachment=getAppendageMount();
if(printBed){
rootAttachment=rootAttachment
//.transformed(new Transform().rotY(90))
.toZMin()
}else{
rootAttachment.setManipulator(dh.getRootListener());
}
csg.add(rootAttachment);//This is the root that attaches to the base
rootAttachment.setColor(Color.GOLD);
CSG foot=getFoot();
CSG servoKeepaway = toXMin(toZMax( new Cube(Math.abs(servoReference.getBounds().getMin().x) +
Math.abs(servoReference.getBounds().getMax().x),
Math.abs(servoReference.getBounds().getMin().y) +
Math.abs(servoReference.getBounds().getMax().y),
Math.abs(servoReference.getBounds().getMax().z)).toCSG(),
)
)
servoKeepaway = servoKeepaway
.movex(-Math.abs(servoReference.getBounds().getMin().x))
.movez(-5)
if(dhLinks!=null){
for(int i=0;i<dhLinks.size();i++){
dh = dhLinks.get(i);
CSG nextAttachment=getAttachment();
CSG servo=servoReference.transformed(new Transform().translateZ(-12.5))// allign to the horn
.union(servoKeepaway)
.transformed(new Transform().rotX(180))// allign to the horn
.transformed(new Transform().rotZ(-90))// allign to the horn
;
servo= makeKeepaway(servo)
double rOffsetForNextLink;
if(i==dhLinks.size()-1){
rOffsetForNextLink = dh.getR()-
( 2.1+
Math.abs(foot.getBounds().getMin().x)
)
}else{
rOffsetForNextLink = dh.getR()-
( 2.1+
Math.abs(nextAttachment.getBounds().getMin().x)
)
}
//println "Link # "+i+" offset = "+(dh.getR()-rOffsetForNextLink)
if(rOffsetForNextLink<attachmentBaseWidth){
rOffsetForNextLink=attachmentBaseWidth
}
double linkThickness = dh.getD();
if(linkThickness<attachmentBaseWidth/2)
linkThickness=attachmentBaseWidth/2
linkThickness +=3;
servo = servo
.movez(-3.30)
servo= moveDHValues(servo,dh);
double yScrewOffset = 2.5
CSG upperLink = toZMin(new Cylinder(cylandarRadius,linkThickness,(int)20).toCSG())
CSG upperScrews = getMountScrewKeepaway()
.transformed(new Transform().translateY(17+(attachmentBaseWidth+3)/2))
.transformed(new Transform().translateZ(upperLink.getBounds().getMax().z - linkThickness ))
if(dh.getR()>60){
upperScrews =upperScrews.union( getMountScrewKeepaway()
.transformed(new Transform().translateY(rOffsetForNextLink-5))
)
}
// adding the radius rod
CSG rod = toYMin(
toZMin(
new Cube(
attachmentBaseWidth+3,
rOffsetForNextLink,
upperLink.getBounds().getMax().z
).toCSG()
)
)
CSG clip = toYMin(
toZMax(
new Cube(
attachmentBaseWidth+3,
9,
attachmentBaseWidth+3
).toCSG()
)
)
.transformed(new Transform().translateY(rOffsetForNextLink))// allign to the NEXT ATTACHMENT
.transformed(new Transform().translateZ(linkThickness))// allign to the NEXT ATTACHMENT
upperLink=upperLink.union(rod,clip,upperScrews);
upperLink= upperLink.difference(upperScrews);
upperLink=upperLink.transformed(new Transform().translateZ(Math.abs(servoReference.getBounds().getMax().z-3)))
upperLink= moveDHValues(upperLink,dh).difference(servo);
if(i== dhLinks.size()-1)
upperLink= upperLink.difference(makeKeepaway(foot));
else
upperLink= upperLink.difference(makeKeepaway(nextAttachment));
double LowerLinkThickness = attachmentRodWidth/2-2
CSG lowerLink = toZMax(new Cylinder(
cylandarRadius,
LowerLinkThickness,
(int)20).toCSG()
)
CSG pinBlank = new Cylinder( 4.5,
LowerLinkThickness,
(int)20)
.toCSG()
lowerLink=lowerLink.union(pinBlank)
lowerLink=lowerLink.transformed(new Transform().translateZ(-attachmentRodWidth/2))
CSG lowerClip =
new Cube(
attachmentBaseWidth+3,
rOffsetForNextLink,
LowerLinkThickness +linkThickness+6
).toCSG().toZMin().toYMin()
.transformed(new Transform().translateY(9))// allign to the NEXT ATTACHMENT
.transformed(new Transform().translateZ(-attachmentRodWidth/2 -LowerLinkThickness ))
lowerLink=lowerLink.union(
lowerClip
);
//Remove the divit or the bearing
lowerLink= lowerLink.difference(
nextAttachment
.makeKeepaway((double)-0.2)
.movez(-0.15),
upperScrews
.movez(6)
)// allign to the NEXT ATTACHMENT);
lowerLink= moveDHValues(lowerLink,dh);
//remove the next links connector and the upper link for mating surface
lowerLink= lowerLink.difference(upperLink,servo);
if(i== dhLinks.size()-1)
lowerLink= lowerLink.difference(makeKeepaway(foot));
else
lowerLink= lowerLink.difference(makeKeepaway(nextAttachment));
if(printBed){
upperLink=reverseDHValues(upperLink,dh)
.transformed(new Transform().rotY(180))
.toZMin()
lowerLink=reverseDHValues(lowerLink,dh)
.transformed(new Transform().rotY(0))
.toZMin()
nextAttachment=nextAttachment
//.transformed(new Transform().rotY(90))
.toZMin()
}else{
nextAttachment.setManipulator(dh.getListener());
nextAttachment.setColor(Color.CHOCOLATE);
servo.setManipulator(dh.getListener());
upperLink.setColor(Color.GREEN);
upperLink.setManipulator(dh.getListener());
lowerLink.setColor(Color.WHITE);
lowerLink.setManipulator(dh.getListener());
//csg.add(servo);// view the servo
//csg.add(upperScrews);//view the screws
}
if(i<dhLinks.size()-1)
csg.add(nextAttachment);//This is the root that attaches to the base
csg.add(upperLink);//This is the root that attaches to the base
csg.add(lowerLink);//White link forming the lower link
}
if(printBed){
foot=foot
.transformed(new Transform().rotY(90))
.toZMin()
}else{
foot.setManipulator(dhLinks.get(dhLinks.size()-1).getListener());
}
foot.setColor(Color.GOLD);
csg.add(foot);//This is the root that attaches to the base
}
return csg;
}
};
Raw
WalkingDriveEngine.groovy
import java.time.Duration;
import java.util.ArrayList;
import javafx.application.Platform;
import org.reactfx.util.FxTimer;
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.MobileBase;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.util.ThreadUtil;
import com.neuronrobotics.sdk.addons.kinematics.IDriveEngine;
double stepOverHeight=5;
long stepOverTime=100;
Double zLock=-65;
Closure calcHome = { DHParameterKinematics leg -> leg.calcHome() }
ArrayList<Object> args = new ArrayList<Object>();
args.add(stepOverHeight)
args.add(stepOverTime)
args.add(zLock)
args.add(calcHome)
return ScriptingEngine.inlineGistScriptRun("bcb4760a449190206170", "GenericWalkingEngine.groovy" , args);
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment