Skip to content

Instantly share code, notes, and snippets.

@madhephaestus

madhephaestus/DefaultDhSolver.groovy

Forked from NotOctogonapus/DefaultDhSolver.groovy
Last active June 21, 2019 21:25
Show Gist options
  • Save madhephaestus/89bc8d74adab661dc5213b1b0820b145 to your computer and use it in GitHub Desktop.
Save madhephaestus/89bc8d74adab661dc5213b1b0820b145 to your computer and use it in GitHub Desktop.
Download ZIP
InputArmBase_copy copy of InputArmBase
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;
import com.neuronrobotics.kinematicschef.InverseKinematicsEngine
return InverseKinematicsEngine.instance
/*
return new DhInverseSolver() {
@Override
public double[] inverseKinematics(
TransformNR target,
double[] jointSpaceVector,
DHChain chain
) {
ArrayList<DHLink> links = chain.getLinks();
if (links.size() < 3) {
return jointSpaceVector
}
// 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 the d offset between the shoulder CoR and the wrist CoR
// r is RED, d is GREEN
double offsetFromShoulderCoRToWristCoR = links.get(1).getD() - links.get(2).getD();
double shoulderHorizontalOffset = links.get(0).getR();
// Length to the target from the origin in the XY plane
double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(), 2) + Math.pow(target.getY(), 2));
// Angle from link 0 to link 1
double angleXYPlaneVect = Math.asin(target.getY() / lengthXYPlaneVect);
double angleRectangleAdjustedXY = Math.asin(offsetFromShoulderCoRToWristCoR / lengthXYPlaneVect);
double lengthRectangleAdjustedXY = lengthXYPlaneVect * Math.cos(angleRectangleAdjustedXY) - shoulderHorizontalOffset;
double orientation = angleXYPlaneVect - angleRectangleAdjustedXY;
if (Math.abs(Math.toDegrees(orientation)) < 0.01){
orientation = 0;
}
// These become the wrist center
double ySet = lengthRectangleAdjustedXY * Math.sin(orientation);
double xSet = lengthRectangleAdjustedXY * Math.cos(orientation);
double zSet = target.getZ() - links.get(0).getD();
if (links.size() >= 6) {
double tipAngulationSum = Math.toDegrees(
links.get(1).getTheta() +
links.get(2).getTheta() +
links.get(5).getTheta()
)
if (tipAngulationSum == 90) {
zSet += links.get(5).getD();
} else {
xSet -= links.get(5).getR() * Math.cos(orientation);
ySet -= links.get(5).getR() * Math.sin(orientation);
}
} else if (links.size() >= 5) {
double tipAngulationSum = Math.toDegrees(
links.get(1).getTheta() +
links.get(2).getTheta() +
links.get(4).getTheta()
)
if (tipAngulationSum == 90) {
zSet += links.get(4).getD();
} else {
xSet -= links.get(4).getR() * Math.cos(orientation);
ySet -= links.get(4).getR() * Math.sin(orientation);
}
} else if (links.size() >= 4) {
double tipAngulationSum = Math.toDegrees(
links.get(1).getTheta() +
links.get(2).getTheta() +
links.get(3).getTheta()
)
if (tipAngulationSum == 90) {
zSet += links.get(3).getR();
} else {
xSet -= links.get(3).getR() * Math.cos(orientation);
ySet -= links.get(3).getR() * Math.sin(orientation);
}
}
// Actual target for analytical solution is above the target minus the z offset
TransformNR overGripper = new TransformNR(
xSet, ySet, zSet,
target.getRotation()
);
double l1 = links.get(1).getR();
double l2 = links.get(2).getR();
double l3 = links.get(3).getD();
double l5 = links.get(5).getD();
double lengthFromShoulderToWristCenter = Math.sqrt(
xSet * xSet +
ySet * ySet +
zSet * zSet
);
if (lengthFromShoulderToWristCenter < 0) {
def errorMsg = "lengthFromShoulderToWristCenter less than zero: " + lengthFromShoulderToWristCenter + "\n"
print(errorMsg)
throw new RuntimeException(errorMsg);
} else if (lengthRectangleAdjustedXY < 0) {
def errorMsg = "lengthRectangleAdjustedXY less than zero: " + lengthRectangleAdjustedXY + "\n"
print(errorMsg)
throw new RuntimeException(errorMsg);
}
//from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
double a = l2;
double b = l1;
double c = lengthFromShoulderToWristCenter;
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 / lengthFromShoulderToWristCenter);
inv[0] = Math.toDegrees(orientation);
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());
} else if((int)links.get(1).getAlpha() == 0){
inv[2] = -Math.toDegrees(C) +
Math.toDegrees(links.get(2).getTheta());
}
if (links.size() > 3) {
// keep it parallel
// the wrist twist will always be 0 for this model because we assume that 3 links won't have a twist
inv[3] = -(inv[1] + inv[2]);
} else if(links.size() > 4) {
// keep the tool orientation parallel to the base
// this link points in the same direction as the first link
inv[4] = inv[0];
}
for (int i = 0; i < inv.length; i++){
if (Math.abs(inv[i]) < 0.01){
inv[i] = 0;
}
}
for (int i = 0; i < linkNum; i++) {
print(inv[i] + ", ")
}
return inv;
}
};
*/
Raw
InputArmBase_copy.xml
<root>
<mobilebase>
<cadEngine>
<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
<file>LinkedCadEngine.groovy</file>
</cadEngine>
<driveEngine>
<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
<file>WalkingDriveEngine.groovy</file>
</driveEngine>
<name>InputArmBase_copy1</name>
<appendage>
<name>InputArm</name>
<cadEngine>
<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
<file>LinkedCadEngine.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>14</index>
<scale>-0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>614.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>13.0</Delta>
<Theta>180.0</Theta>
<Radius>32.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>15</index>
<scale>0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>526.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>25.0</Delta>
<Theta>-90.0</Theta>
<Radius>93.0</Radius>
<Alpha>180.0</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>20</index>
<scale>-0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>148.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>11.0</Delta>
<Theta>90.0</Theta>
<Radius>24.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist1</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>9</index>
<scale>-0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>458.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>128.0</Delta>
<Theta>-90.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist2</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>10</index>
<scale>0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>603.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist3</name>
<deviceName>dyio1</deviceName>
<type>dummy</type>
<index>11</index>
<scale>0.2200489</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>450.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>25.0</Delta>
<Theta>90.0</Theta>
<Radius>0.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<ZframeToRAS
> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz>
</ZframeToRAS>
<baseToZframe>
<x>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>
</appendage>
<appendage>
<name>PUMA</name>
<cadEngine>
<git>https://gist.github.com/c3fc39308a506d4cb1cd7297193c41e7.git</git>
<file>LinkedCadEngine.groovy</file>
</cadEngine>
<kinematics>
<git>https://github.com/madhephaestus/carl-the-hexapod.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>128.43283582089552</staticOffset>
<isLatch>true</isLatch>
<indexLatch>235</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>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>128.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>128</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>14.9</Delta>
<Theta>0.0</Theta>
<Radius>43.2</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>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>121.0</staticOffset>
<isLatch>true</isLatch>
<indexLatch>121</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>0.0</rotx>
<roty>0.0</roty>
<rotz>0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>2.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>3</index>
<scale>1.0</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>43.2</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist2</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>4</index>
<scale>1.0</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>wrist3</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>5</index>
<scale>1.0</scale>
<upperLimit>100000.0</upperLimit>
<lowerLimit>-100000.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>0.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<vitamins>
<vitamin>
<name>electroMechanical</name>
<type>hobbyServo</type>
<id>standardMicro</id>
</vitamin>
<vitamin>
<name>shaft</name>
<type>hobbyServoHorn</type>
<id>standardMicro1</id>
</vitamin>
</vitamins>
<passive>false</passive>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<DHParameters>
<Delta>5.6</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<ZframeToRAS
> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz>
</ZframeToRAS>
<baseToZframe>
<x>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>
</appendage>
<ZframeToRAS>
<x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz>
</ZframeToRAS>
<baseToZframe>
<x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz>
</baseToZframe>
<mass>0.01</mass>
<centerOfMassFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></centerOfMassFromCentroid>
<imuFromCentroid> <x>0.0</x>
<y>0.0</y>
<z>0.0</z>
<rotw>1.0</rotw>
<rotx>-0.0</rotx>
<roty>-0.0</roty>
<rotz>-0.0</rotz></imuFromCentroid>
<vitamins>
</vitamins>
</mobilebase>
</root>
Raw
LinkedCadEngine.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
import javafx.scene.transform.Affine;
import com.neuronrobotics.bowlerstudio.physics.TransformFactory;
return new ICadGenerator(){
private CSG reverseDHValues(CSG incoming,DHLink dh ){
println "Reversing "+dh
TransformNR step = new TransformNR(dh.DhStep(0))
Transform move = TransformFactory.nrToCSG(step)
return incoming.transformed(move)
}
private CSG moveDHValues(CSG incoming,DHLink dh ){
TransformNR step = new TransformNR(dh.DhStep(0)).inverse()
Transform move = TransformFactory.nrToCSG(step)
return incoming.transformed(move)
}
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
ArrayList<DHLink> dhLinks = d.getChain().getLinks()
DHLink dh = dhLinks.get(linkIndex)
// Transform used by the UI to render the location of the object
Affine manipulator = dh.getListener();
CSG rLink =new Cube(dh.getRadius(),5,5).toCSG().toXMax()
//rLink = moveDHValues(rLink, dh)
rLink.setColor(javafx.scene.paint.Color.RED)
CSG dLink = new Cube(5, 5, dh.getDelta()).toCSG().toZMin()
dLink = moveDHValues(dLink, dh)
dLink.setColor(javafx.scene.paint.Color.GREEN)
ArrayList<CSG> allCad=[dLink, rLink];
for(CSG cad:allCad){
cad.setManipulator(manipulator)
}
//If you want you can add things here
//allCad.add(myCSG);
return allCad;
}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {
println "Making Base "
ArrayList<CSG> allCad=[]; //new Cube(10).toCSG()
// Transform used by the UI to render the location of the object
Affine manipulator = b.getRootListener();
for(CSG cad:allCad){
cad.setManipulator(manipulator)
}
//If you want you can add things here
//allCad.add(myCSG);
return allCad;
}
};
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=200;
Double zLock=5;
Closure calcHome = { DHParameterKinematics leg ->
TransformNR h=leg.calcHome()
TransformNR legRoot= leg.getRobotToFiducialTransform()
TransformNR tr = leg.forwardOffset(new TransformNR())
tr.setZ(zLock)
//Bambi-on-ice the legs a bit
if(legRoot.getY()>0){
//tr.translateY(-5)
}else{
//tr.translateY(5)
}
return h;
}
boolean usePhysicsToMove = true;
long stepCycleTime =5000
long walkingTimeout =stepCycleTime*2
int numStepCycleGroups = 2
double standardHeadTailAngle = -20
double staticPanOffset = 10
double coriolisGain = 1
boolean headStable = false
double maxBodyDisplacementPerStep = 20
double minBodyDisplacementPerStep = 15
def ar = [stepOverHeight,
stepOverTime,
zLock,
calcHome,
usePhysicsToMove,
stepCycleTime,
numStepCycleGroups,
standardHeadTailAngle,
staticPanOffset,
coriolisGain,
headStable,
maxBodyDisplacementPerStep,
minBodyDisplacementPerStep,
walkingTimeout]
return ScriptingEngine
.gitScriptRun(
"https://github.com/keionbis/SmallKat.git", // git location of the library
"Bowler/DynamicWalking.groovy" , // file to load
ar
)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment