Skip to content

Instantly share code, notes, and snippets.

@Octogonapus

Octogonapus/DefaultDhSolver.groovy

Created December 13, 2019 03:07
Show Gist options
  • Save Octogonapus/40b38acfb57266cf06abe41c34d1efcf to your computer and use it in GitHub Desktop.
Save Octogonapus/40b38acfb57266cf06abe41c34d1efcf 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;
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];
if(linkNum<3){
for(int i=i;i<linkNum;i++){
inv[i]=jointSpaceVector[i]+0.0001
}
}
// this is an ad-hock kinematic model for d-h parameters and only works for specific configurations
double d = links.get(1).getD()- links.get(2).getD();
double r = links.get(0).getR();
double lengthXYPlaneVect = Math.sqrt(Math.pow(target.getX(),2)+Math.pow(target.getY(),2));
double angleXYPlaneVect = Math.asin(target.getY()/lengthXYPlaneVect);
double angleRectangleAdjustedXY =Math.asin(d/lengthXYPlaneVect);
double lengthRectangleAdjustedXY = lengthXYPlaneVect* Math.cos(angleRectangleAdjustedXY)-r;
double orentation = angleXYPlaneVect-angleRectangleAdjustedXY;
if(Math.abs(Math.toDegrees(orentation))<0.01){
orentation=0;
}
double ySet = lengthRectangleAdjustedXY*Math.sin(orentation);
double xSet = lengthRectangleAdjustedXY*Math.cos(orentation);
double zSet = target.getZ() - links.get(0).getD();
if(links.size()==5){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(4).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(4).getD();
else{
double tipySet = links.get(4).getR()*Math.sin(orentation);
double tipxSet = links.get(4).getR()*Math.cos(orentation);
//println "5 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
if(links.size()==4){
double tipAngulationSum =Math.toDegrees( links.get(1).getTheta()+
links.get(2).getTheta()+
links.get(3).getTheta())
//println "Tip angulation orentation = "+tipAngulationSum
if(tipAngulationSum==90)
zSet+=links.get(3).getR();
else{
double tipySet = links.get(3).getR()*Math.sin(orentation);
double tipxSet = links.get(3).getR()*Math.cos(orentation);
//println "4 links back Setting tip x="+tipxSet+" y="+tipySet
xSet-=tipxSet
ySet-=tipySet
}
}
// Actual target for anylitical solution is above the target minus the z offset
TransformNR overGripper = new TransformNR(
xSet,
ySet,
zSet,
target.getRotation());
double l1 = links.get(1).getR();// First link length
double l2 = links.get(2).getR();
double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet);
/*
println ( "TO: "+target);
println ( "Trangular TO: "+overGripper);
println ( "lengthXYPlaneVect: "+lengthXYPlaneVect);
println( "angleXYPlaneVect: "+Math.toDegrees(angleXYPlaneVect));
println( "angleRectangleAdjustedXY: "+Math.toDegrees(angleRectangleAdjustedXY));
println( "lengthRectangleAdjustedXY: "+lengthRectangleAdjustedXY);
println( "r: "+r);
println( "d: "+d);
println( "x Correction: "+xSet);
println( "y Correction: "+ySet);
println( "Orentation: "+Math.toDegrees(orentation));
println( "z: "+zSet);
*/
if (vect > l1+l2 || vect<0 ||lengthRectangleAdjustedXY<0 ) {
throw new RuntimeException("Hypotenus too long: "+vect+" longer then "+l1+l2);
}
//from https://www.mathsisfun.com/algebra/trig-solving-sss-triangles.html
double a=l2;
double b=l1;
double c=vect;
double A =Math.acos((Math.pow(b,2)+ Math.pow(c,2) - Math.pow(a,2)) / (2.0*b*c));
double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2.0*a*c));
double C =Math.PI-A-B;//Rule of triangles
double elevation = Math.asin(zSet/vect);
/*
println( "vect: "+vect);
println( "A: "+Math.toDegrees(A));
println( "elevation: "+Math.toDegrees(elevation));
println( "l1 from x/y plane: "+Math.toDegrees(A+elevation));
println( "l2 from l1: "+Math.toDegrees(C));
*/
inv[0] = Math.toDegrees(orentation);
inv[1] = -Math.toDegrees((A+elevation+links.get(1).getTheta()));
if((int)links.get(1).getAlpha() ==180){
inv[2] = (Math.toDegrees(C))-180-//interior angle of the triangle, map to external angle
Math.toDegrees(links.get(2).getTheta());// offset for kinematics
}
if((int)links.get(1).getAlpha() ==0){
inv[2] = -(Math.toDegrees(C))+Math.toDegrees(links.get(2).getTheta());// offset for kinematics
}
if(links.size()>3)
inv[3] =-(inv[1] + inv[2]);// keep it parallell
// We know the wrist twist will always be 0 for this model
if(links.size()>4)
inv[4] = inv[0];//keep the tool orentation paralell from the base
for(int i=0;i<inv.length;i++){
if(Math.abs(inv[i]) < 0.01){
inv[i]=0;
}
// println( "Link#"+i+" is set to "+inv[i]);
}
int i=3;
if(links.size()>3)
i=5;
//copy over remaining links so they do not move
for(;i<inv.length && i<jointSpaceVector.length ;i++){
inv[i]=jointSpaceVector[i];
}
return inv;
}
};
Raw
InputArmBase_copy.xml
<root>
<mobilebase>
<cadEngine>
<git>https://gist.github.com/40b38acfb57266cf06abe41c34d1efcf.git</git>
<file>LinkedCadEngine.groovy</file>
</cadEngine>
<driveEngine>
<git>https://gist.github.com/40b38acfb57266cf06abe41c34d1efcf.git</git>
<file>WalkingDriveEngine.groovy</file>
</driveEngine>
<name>InputArmBase_copy</name>
<appendage>
<name>InputArm</name>
<cadEngine>
<git>https://gist.github.com/40b38acfb57266cf06abe41c34d1efcf.git</git>
<file>LinkedCadEngine.groovy</file>
</cadEngine>
<kinematics>
<git>https://gist.github.com/40b38acfb57266cf06abe41c34d1efcf.git</git>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basePan</name>
<deviceName>dyio1</deviceName>
<type>analog-rotory</type>
<index>14</index>
<scale>-0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>analog-rotory</type>
<index>15</index>
<scale>0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>analog-rotory</type>
<index>8</index>
<scale>-0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>analog-rotory</type>
<index>9</index>
<scale>-0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>analog-rotory</type>
<index>10</index>
<scale>0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>analog-rotory</type>
<index>11</index>
<scale>0.2200489</scale>
<upperLimit>1024.0</upperLimit>
<lowerLimit>0.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>
<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>
<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 com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
//First we load teh default cad generator script
ICadGenerator defaultCadGen=(ICadGenerator) ScriptingEngine
.gitScriptRun(
"https://github.com/madhephaestus/carl-the-hexapod.git", // git location of the library
"ThreeDPrintCad.groovy" , // file to load
null
)
return new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, int linkIndex) {
ArrayList<CSG> allCad=defaultCadGen.generateCad(d,linkIndex);
//If you want you can add things here
//allCad.add(myCSG);
return allCad;
}
@Override
public ArrayList<CSG> generateBody(MobileBase b ) {
ArrayList<CSG> allCad=defaultCadGen.generateBody(b);
//If you want you can add things here
//allCad.add(myCSG);
return allCad;
}
};
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/CommonWealthRobotics/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