Created
April 8, 2016 01:22
-
-
Save acamilo/2e08db0887e365607fe283815284c128 to your computer and use it in GitHub Desktop.
Carl The Hexapod
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> | |
<driveType>walking</driveType> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<driveEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>WalkingDriveEngine.groovy</file> | |
</driveEngine> | |
<name>CarlTheWalkingRobot</name> | |
<leg> | |
<name>Carl_One</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>-55.0</x> | |
<y>-50.0</y> | |
<z>1.9984014443252818E-15</z> | |
<rotw>0.3826834323650933</rotw> | |
<rotx>2.3820429953758186E-17</rotx> | |
<roty>1.813220358410302E-16</roty> | |
<rotz>-0.9238795325112843</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>Carl_Two</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.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>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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>-60.0</y> | |
<z>4.440892098500626E-15</z> | |
<rotw>0.7372773350054747</rotw> | |
<rotx>1.9333260774502692E-5</rotx> | |
<roty>-2.3744128324749153E-4</roty> | |
<rotz>-0.6755901675831932</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>Carl_Three</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>55.0</x> | |
<y>-50.0</y> | |
<z>8.215650382226158E-15</z> | |
<rotw>0.9762960071199334</rotw> | |
<rotx>-2.5493781137848553E-16</rotx> | |
<roty>-4.713185234563371E-17</roty> | |
<rotz>-0.21643961393810307</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>Carl_Four</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>55.0</x> | |
<y>50.0</y> | |
<z>-2.4424906541753444E-15</z> | |
<rotw>0.9762960071199334</rotw> | |
<rotx>-1.1820552112337377E-16</rotx> | |
<roty>-4.17168050452654E-17</roty> | |
<rotz>0.21643961393810285</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>Carl_Five</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>DefaultDhSolver.groovy</file> | |
</kinematics> | |
<link> | |
<name>basePan</name> | |
<deviceName>dyio</deviceName> | |
<type>servo-rotory</type> | |
<index>12</index> | |
<scale>0.33</scale> | |
<upperLimit>157.94776119402985</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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>60.0</y> | |
<z>-4.440892098500626E-15</z> | |
<rotw>0.7372773368101241</rotw> | |
<rotx>8.313372743244326E-17</rotx> | |
<roty>6.84434423870078E-17</roty> | |
<rotz>0.6755902076156602</rotz> | |
</baseToZframe> | |
</leg> | |
<leg> | |
<name>Carl_Six</name> | |
<cadEngine> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<file>ThreeDPrintCad.groovy</file> | |
</cadEngine> | |
<kinematics> | |
<git>https://gist.github.com/bcb4760a449190206170.git</git> | |
<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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>0.0</Theta> | |
<Radius>50.0</Radius> | |
<Alpha>0.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> | |
<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> | |
<DHParameters> | |
<Delta>0.0</Delta> | |
<Theta>90.0</Theta> | |
<Radius>70.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>-55.0</x> | |
<y>50.0</y> | |
<z>-7.771561172376096E-15</z> | |
<rotw>0.38268343236507907</rotw> | |
<rotx>-1.813220358410335E-16</rotx> | |
<roty>-2.3003042022369397E-16</roty> | |
<rotz>0.9238795325112925</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.5</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> | |
</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 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())); | |
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
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); | |
boolean usePhysics=(args.size()>4?((boolean)args.get(4)):false); | |
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++){ | |
//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(legs.get(i).getCurrentPoseTarget()); | |
//invert the target pose | |
TransformNR rotPoseinv = newPose.inverse(); | |
//apply the inverted target | |
TransformNR newTar = rotPoseinv.times(rotPose); | |
//un-do the orientation inversion to get final location | |
TransformNR incr =inverseRot.inverse().times(newTar); | |
feetLocations[i]=incr; | |
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 | |
// | |
//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 = lastGood.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); | |
} | |
if(!usePhysics) | |
source.setGlobalToFiducialTransform(global); | |
// 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(previousGLobalState); | |
//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 | |
} | |
} |
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 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.BowlerStudioController; | |
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; | |
import com.neuronrobotics.bowlerstudio.physics.*; | |
return new ICadGenerator(){ | |
//CSG servoReference= new MicroServo().toCSG(); | |
CSG dyioReference= (CSG)(ScriptingEngine.inlineGistScriptRun( | |
"fb4cf429372deeb36f52", | |
"dyioCad.groovy" , | |
null)) | |
// .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,12).toCSG(); | |
CSG hornBlank = new Cube(6,5,12).toCSG() | |
.toZMax() | |
.toYMin() | |
.movez(4) | |
CSG mountReference = (CSG)(ScriptingEngine.inlineGistScriptRun( | |
"ce4e7c95d516e265b91e", | |
"servoAttachment.groovy" , | |
[hornBlank])) | |
CSG mountScrewKeepaway = (CSG)(ScriptingEngine.inlineGistScriptRun( | |
"488e0ee249a5c16ae4d8", | |
"moutScrewKeepaway.groovy" , | |
null)) | |
private double attachmentRodWidth=10; | |
private double attachmentBaseWidth=15; | |
private double printerTollerence =0.5; | |
private double mountScrewKeepawaySize= 7.5; | |
private double mountScrewHoleKeepawaySize= 4.1; | |
private double mountScrewSeperationDistance=attachmentRodWidth/2+mountScrewHoleKeepawaySize/2+0.5; | |
double cylandarRadius = 13.5; | |
private double bearingPinRadius=3; | |
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 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(-14*1.2)) | |
.transformed(new Transform().translateZ(-cylindarKeepawayHeight/2)) | |
return attachmentbase; | |
} | |
private CSG getMountScrewKeepaway(){ | |
return mountScrewKeepaway.clone(); | |
} | |
private CSG getAttachment(){ | |
return mountReference.clone() | |
} | |
private CSG getFoot(){ | |
CSG attach = getAttachment(); | |
CSG foot = new Sphere(attachmentRodWidth).toCSG(); | |
return toXMax(attach.union(foot)); | |
} | |
private CSG reverseDHValues(CSG incoming,DHLink dh ){ | |
println "Reversing "+dh | |
return incoming | |
.rotx(Math.toDegrees(-dh.getAlpha())) | |
.movex(dh.getR()/2) | |
} | |
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()))); | |
} | |
/** | |
* Gets the all dh chains. | |
* | |
* @return the all dh chains | |
*/ | |
public ArrayList<DHParameterKinematics> getLimbDHChains(MobileBase base) { | |
ArrayList<DHParameterKinematics> copy = new ArrayList<DHParameterKinematics>(); | |
for(DHParameterKinematics l:base.getLegs()){ | |
copy.add(l); | |
} | |
for(DHParameterKinematics l:base.getAppendages() ){ | |
copy.add(l); | |
} | |
return copy; | |
} | |
ArrayList<CSG> generateBody(MobileBase base){ | |
ArrayList<CSG> allCad=new ArrayList<>(); | |
ArrayList<CSG> cutouts=new ArrayList<>(); | |
ArrayList<CSG> attach=new ArrayList<>(); | |
CSG attachUnion=null; | |
for(DHParameterKinematics l:getLimbDHChains(base)){ | |
TransformNR position = l.getRobotToFiducialTransform(); | |
Transform csgTrans = TransformFactory.nrToCSG(position) | |
cutouts.add(getAppendageMount() | |
.transformed(csgTrans) | |
); | |
CSG attachment = getAttachment() | |
.transformed(csgTrans) | |
attach.add(attachment); | |
if(attachUnion==null){ | |
attachUnion=attachment; | |
}else{ | |
attachUnion = attachUnion.union(attachment) | |
} | |
} | |
CSG upperBody = attachUnion.hull() | |
CSG myDyIO=dyioReference | |
.movez(upperBody.getMaxZ()+22.0) | |
.movex( upperBody.getMaxX()- | |
Math.abs(upperBody.getMinX())+ | |
dyioReference.getMaxX()- | |
dyioReference.getMinX()) | |
.movey( upperBody.getMaxY()- | |
Math.abs(upperBody.getMinY())+ | |
dyioReference.getMaxY()- | |
Math.abs(dyioReference.getMinY())) | |
upperBody=upperBody | |
.union(myDyIO) | |
.hull() | |
.difference(myDyIO) | |
upperBody= upperBody.difference(cutouts); | |
upperBody= upperBody.union(attach); | |
upperBody.setColor(Color.CYAN); | |
upperBody.setManipulator(base.getRootListener()); | |
upperBody.setManufactuing(new PrepForManufacturing() { | |
public CSG prep(CSG arg0) { | |
return arg0.toZMin(); | |
} | |
}); | |
allCad.add(upperBody) | |
return allCad; | |
} | |
public ArrayList<CSG> generateCad(DHParameterKinematics sourceLimb, int linkIndex){ | |
//printBed=true; | |
ArrayList<DHLink> dhLinks=sourceLimb.getChain().getLinks(); | |
ArrayList<CSG> csg = new ArrayList<CSG>(); | |
LinkConfiguration conf = sourceLimb.getLinkConfiguration(linkIndex); | |
CSG servoReference= Vitamins.get(conf.getElectroMechanicalType(),conf.getElectroMechanicalSize()) | |
.transformed(new Transform().rotZ(-90)) | |
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) | |
dh = dhLinks.get(linkIndex); | |
CSG nextAttachment=getAttachment(); | |
CSG servo=servoReference | |
.movez(-servoReference.getMaxZ()-6) | |
.union(servoKeepaway) | |
.transformed(new Transform().rotX(180))// allign to the horn | |
.transformed(new Transform().rotZ(-90))// allign to the horn | |
; | |
servo= servo.makeKeepaway(printerTollerence) | |
double totalServoExtention = Math.abs(servo.getMinY()) | |
boolean addNub=false; | |
println "Servo Keepaway Radius = "+cylandarRadius | |
double rOffsetForNextLinkStart=dh.getR()+mountScrewKeepawaySize+0.75; | |
double rOffsetForNextLink=rOffsetForNextLinkStart; | |
if(linkIndex==dhLinks.size()-1){ | |
rOffsetForNextLink = rOffsetForNextLink- | |
Math.abs(foot.toXMax().getBounds().getMin().x) | |
}else{ | |
rOffsetForNextLink = rOffsetForNextLink- | |
Math.abs(nextAttachment.toXMax().getBounds().getMin().x) | |
} | |
//println "Link # "+linkIndex+" offset = "+(rOffsetForNextLink)+" rad "+dh.getR() +" offset = " | |
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 | |
double ServoKeepawayRad = Math.sqrt((servoReference.getMinX()*servoReference.getMinX())+ | |
(servoReference.getMaxY()*servoReference.getMaxY())) +1 | |
CSG upperLink = toZMin(new Cylinder(ServoKeepawayRad,linkThickness,(int)20).toCSG()) | |
/* | |
if(addNub){ | |
totalServoExtention | |
upperLink=upperLink.union(new Cylinder(cylandarRadius,linkThickness,(int)20) | |
.toCSG() | |
.toZMin() | |
.movey(-totalServoExtention+cylandarRadius) | |
) | |
} | |
*/ | |
double screwsCloseY = 20; | |
double screwsFarY=rOffsetForNextLink+mountScrewKeepawaySize/2 | |
double screwsZ=upperLink.getBounds().getMax().z - linkThickness | |
double mountCylindarRad = (mountScrewKeepawaySize+1)/2 | |
CSG mountHoleAttachment = new Cylinder(mountCylindarRad, // Radius at the top | |
mountCylindarRad, // Radius at the bottom | |
linkThickness, // Height | |
(int)10 //resolution | |
).toCSG() | |
.toZMin() | |
mountHoleAttachment = mountHoleAttachment.movex(mountScrewSeperationDistance) | |
mountHoleAttachment =mountHoleAttachment.union(mountHoleAttachment.movex(-2*mountScrewSeperationDistance)) | |
CSG mountHoleAttachmentGroup = mountHoleAttachment | |
.movey(screwsFarY) | |
CSG upperScrews = getMountScrewKeepaway() | |
.movey(screwsFarY) | |
.movez(screwsZ) | |
if(dh.getR()>80){ | |
upperScrews =upperScrews.union( getMountScrewKeepaway() | |
.movey(screwsCloseY) | |
) | |
mountHoleAttachmentGroup=mountHoleAttachmentGroup | |
.union( | |
mountHoleAttachment | |
.movey(screwsCloseY) | |
) | |
} | |
// 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+12, | |
9+12, | |
attachmentBaseWidth+3 | |
).toCSG() | |
) | |
) | |
.toYMax() | |
.movey(rOffsetForNextLink+8)// allign to the NEXT ATTACHMENT | |
.movez(linkThickness)// allign to the NEXT ATTACHMENT | |
double upperLinkZOffset = Math.abs(servoReference.getBounds().getMax().z-3) | |
//Build the upper link | |
upperLink=upperLink.union(mountHoleAttachmentGroup,rod) | |
.hull()//smooth out the shape | |
CSG projection = upperLink | |
.scalez(10) | |
.intersect(clip) | |
upperLink=upperLink .union(projection.toZMax())// add the clip in | |
upperLink= upperLink.difference(upperScrews); | |
upperLink=upperLink.movez(upperLinkZOffset) | |
upperLink= moveDHValues(upperLink,dh).difference(servo); | |
// debugging | |
projection=moveDHValues( projection,dh) | |
projection.setManipulator(dh.getListener()); | |
//csg.add(projection);// view the clip | |
if(linkIndex== dhLinks.size()-1) | |
upperLink= upperLink.difference(foot.makeKeepaway(printerTollerence*2)); | |
else | |
upperLink= upperLink.difference(nextAttachment.makeKeepaway(printerTollerence*2)); | |
double LowerLinkThickness = attachmentRodWidth/2-2 | |
CSG lowerLink = new Cylinder( | |
cylandarRadius, | |
LowerLinkThickness +linkThickness+6, | |
(int)20) | |
.toCSG() | |
.toZMin() | |
.movez(-LowerLinkThickness) | |
CSG linkSweepCutout= new Cylinder( | |
cylandarRadius+1, | |
LowerLinkThickness +linkThickness+6, | |
(int)20) | |
.toCSG() | |
.toZMin() | |
.movez(-attachmentRodWidth/2) | |
CSG pinBlank = new Cylinder( bearingPinRadius-printerTollerence, | |
LowerLinkThickness, | |
(int)20) | |
.toCSG() | |
.toZMin() | |
.movez(-attachmentRodWidth/2) | |
linkSweepCutout= linkSweepCutout.difference( pinBlank) | |
//lowerLink=lowerLink.union(pinBlank) | |
lowerLink=lowerLink.transformed(new Transform().translateZ(-attachmentRodWidth/2)) | |
CSG lowerClip = | |
new Cube( | |
attachmentBaseWidth+3, | |
rOffsetForNextLink-2, | |
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, | |
mountHoleAttachmentGroup.movez(lowerLink.getMinZ()) | |
).hull(); | |
//Remove the divit or the bearing | |
lowerLink= lowerLink.difference( | |
nextAttachment | |
.makeKeepaway((double)-0.2) | |
.movez(-0.15), | |
upperScrews | |
.movez(6), | |
linkSweepCutout | |
)// 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(linkIndex== dhLinks.size()-1) | |
lowerLink= lowerLink.difference(foot.makeKeepaway(printerTollerence*2)); | |
else | |
lowerLink= lowerLink.difference(nextAttachment.makeKeepaway(printerTollerence*2)); | |
upperLink.setManufactuing(new PrepForManufacturing() { | |
public CSG prep(CSG arg0) { | |
return reverseDHValues(arg0,dhLinks.get(linkIndex)) | |
.roty(180) | |
.toZMin() | |
.toXMin() | |
} | |
}); | |
lowerLink.setManufactuing(new PrepForManufacturing() { | |
public CSG prep(CSG arg0) { | |
return reverseDHValues(arg0,dhLinks.get(linkIndex)) | |
.toZMin() | |
.toXMin() | |
} | |
}); | |
nextAttachment.setManufactuing(new PrepForManufacturing() { | |
public CSG prep(CSG arg0) { | |
return arg0 | |
//.transformed(new Transform().rotY(90)) | |
.toZMin() | |
.toXMin() | |
} | |
}); | |
nextAttachment.setManipulator(dh.getListener()); | |
nextAttachment.setColor(Color.CHOCOLATE); | |
servo.setManipulator(dh.getListener()); | |
upperLink.setColor(Color.GREEN); | |
upperLink.setManipulator(dh.getListener()); | |
lowerLink.setColor(Color.PURPLE); | |
lowerLink.setManipulator(dh.getListener()); | |
//csg.add(servo);// view the servo | |
upperScrews= moveDHValues(upperScrews.movez(upperLinkZOffset),dh) | |
upperScrews.setManipulator(dh.getListener()); | |
//csg.add(upperScrews);//view the screws | |
if(linkIndex<dhLinks.size()-1){ | |
csg.add(nextAttachment);//This is the root that attaches to the base | |
BowlerStudioController.addCsg(nextAttachment); | |
} | |
csg.add(upperLink);//This is the root that attaches to the base | |
csg.add(lowerLink);//White link forming the lower link | |
BowlerStudioController.addCsg(upperLink); | |
BowlerStudioController.addCsg(lowerLink); | |
if(linkIndex== dhLinks.size()-1){ | |
CSG foot=getFoot(); | |
//test for last link | |
foot.setManufactuing(new PrepForManufacturing() { | |
public CSG prep(CSG arg0) { | |
return arg0.transformed(new Transform().rotY(90)) | |
.toZMin() | |
.toXMin() | |
} | |
}); | |
foot.setManipulator(dhLinks.get(dhLinks.size()-1).getListener()); | |
foot.setColor(Color.GOLD); | |
csg.add(foot);//This is the root that attaches to the base | |
BowlerStudioController.addCsg(foot); | |
} | |
return csg; | |
} | |
}; |
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=5; | |
long stepOverTime=80; | |
Double zLock=-70; | |
Closure calcHome = { DHParameterKinematics leg -> | |
TransformNR h=leg.calcHome() | |
TransformNR tr = leg.forwardOffset(new TransformNR()) | |
tr.setZ(zLock) | |
return h; | |
} | |
boolean usePhysicsToMove = true; | |
return ScriptingEngine.inlineGistScriptRun("bcb4760a449190206170", | |
"GenericWalkingEngine.groovy" , | |
[stepOverHeight,stepOverTime,zLock,calcHome,usePhysicsToMove] | |
); | |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment