Skip to content

Instantly share code, notes, and snippets.

@mdiblasi

mdiblasi/KickOff.groovy

Forked from madhephaestus/KickOff.groovy
Last active October 8, 2017 18:51
Show Gist options
  • Save mdiblasi/9925fd3572b30419c425 to your computer and use it in GitHub Desktop.
Save mdiblasi/9925fd3572b30419c425 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
KickOff.groovy
import java.io.File;
import com.neuronrobotics.bowlerstudio.robots.FormacarumRover;
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.dyio.DyIO;
import com.neuronrobotics.sdk.util.ThreadUtil;
//run the rover initialization scripts, ignore the display returns
ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/RoverStartup.groovy"), null);
DyIO dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio");
FormacarumRover rover = (FormacarumRover)DeviceManager.getSpecificDevice(FormacarumRover.class, "rover");
System.out.println("Initialized OK, waiting for un-pause");
while(dyio.getValue(23)>0){
ThreadUtil.wait(10);// wait in pause state
}
System.out.println("Starting run!");
rover.DriveStraight(10000, 15.0);
ThreadUtil.wait(15000);
rover.SetDriveVelocity(0);
boolean b = false;
while(true){
if(dyio.getValue(23)==0){
b=!b;
System.out.println("Running Seek");
//we are unpaused for this loop, run robot code
ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/SeekObjects.groovy"), null);
rover.DriveArc(b?1:-1*1000,b?1:-1*90, 5);
ThreadUtil.wait(10000);
}
ThreadUtil.wait(100);
}
Raw
RoverStartup.groovy
import com.neuronrobotics.sdk.serial.SerialConnection; // Needed for serial.
import com.neuronrobotics.jniloader.OpenCVImageProvider
import com.neuronrobotics.jniloader.SalientDetector
import com.neuronrobotics.replicator.driver.BowlerBoardDevice; // Bowler talk.
import gnu.io.NRSerialPort; // Needed for the laser range finder.
import com.neuronrobotics.addons.driving.AckermanConfiguration
import com.neuronrobotics.addons.driving.AckermanDefaultKinematics;
import com.neuronrobotics.addons.driving.DataPoint;
import com.neuronrobotics.addons.driving.HokuyoURGDevice; // Needed for the laser range finder.
import com.neuronrobotics.addons.driving.IAckermanBotKinematics;
import com.neuronrobotics.addons.driving.URG2Packet;
import com.neuronrobotics.sdk.common.BowlerAbstractDevice
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.common.NonBowlerDevice
import com.neuronrobotics.sdk.dyio.DyIO
import com.neuronrobotics.sdk.dyio.peripherals.DigitalInputChannel
import com.neuronrobotics.sdk.dyio.peripherals.DigitalOutputChannel
import com.neuronrobotics.sdk.dyio.peripherals.IDigitalInputListener;
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel
import com.neuronrobotics.sdk.namespace.bcs.pid.IPidControlNamespace;
import com.neuronrobotics.sdk.pid.PIDChannel; // Needed for PID.
import com.neuronrobotics.sdk.pid.PIDConfiguration;
import com.neuronrobotics.bowlerstudio.robots.FormacarumRover
import com.neuronrobotics.bowlerstudio.robots.ObjectDetectionDataTableElement
import com.neuronrobotics.bowlerstudio.robots.RoverDataTable
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.sdk.addons.kinematics.DHChain;
import com.neuronrobotics.sdk.addons.kinematics.DHLink;
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics;
import com.neuronrobotics.sdk.addons.kinematics.DhInverseSolver
import com.neuronrobotics.sdk.addons.kinematics.ServoRotoryLink;
import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR
import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
import com.neuronrobotics.sdk.pid.VirtualGenericPIDDevice;
import com.neuronrobotics.sdk.ui.ConnectionDialog;
import com.neuronrobotics.sdk.util.ThreadUtil;
import eu.mihosoft.vrl.v3d.CSG;
import eu.mihosoft.vrl.v3d.Cube;
import eu.mihosoft.vrl.v3d.Sphere;
import eu.mihosoft.vrl.v3d.Transform;
import java.nio.file.Files;
import java.nio.file.Paths;
import java.nio.file.StandardCopyOption;
import java.time.Duration;
import java.util.ArrayList;
import javafx.application.Platform;
import org.apache.commons.io.IOUtils;
import org.reactfx.util.FxTimer;
boolean useVirtual = false;
//useVirtual=true;
public class ComputedGeometricModel implements DhInverseSolver{
private DHChain dhChain;
public ComputedGeometricModel(DHChain dhChain){
this.dhChain=dhChain;
}
private static final double M_PI = Math.PI;
public double[] inverseKinematics(TransformNR target,double[] jointSpaceVector ) {
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
// Actual target for anylitical solution is above the target minus the z offset
TransformNR overGripper = new TransformNR(target.getX(),
target.getY(),
target.getZ() -
dhChain.getLinks().get(0).getD(),
target.getRotation());
double l1 = dhChain.getLinks().get(1).getR();// First link length
double l2 = dhChain.getLinks().get(2).getR();
double xSet = overGripper.getX();
double ySet = overGripper.getY();
double zSet = overGripper.getZ();
double vect = Math.sqrt(xSet*xSet+ySet*ySet+zSet*zSet)
double xYvect = Math.sqrt(xSet*xSet+ySet*ySet);
if (vect > l1+l2) {
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*b*c));
double B =Math.acos((Math.pow(c,2)+ Math.pow(a,2) - Math.pow(b,2)) / (2*a*c));
double C =Math.PI-A-B;//Rule of triangles
double elevation = Math.asin(zSet/vect);
double orentation = Math.asin(ySet/xYvect);
System.out.println("Orentation: "+Math.toDegrees(orentation)+", Links: ,"+l1+","+l2+" vector distance: "+vect+", z: "+zSet);
System.out.println("A: "+Math.toDegrees(A));
System.out.println("elevation: "+Math.toDegrees(elevation));
System.out.println("l1 from x/y plane: "+Math.toDegrees(A+elevation));
System.out.println("l2 from l1: "+Math.toDegrees(C));
inv[0] = Math.toDegrees(orentation);
inv[1] = -Math.toDegrees((A+elevation-dhChain.getLinks().get(1).getTheta()));
inv[2] = (180-Math.toDegrees(C))-//interior angle of the triangle, map to external angle
Math.toDegrees(dhChain.getLinks().get(2).getTheta());// offset for kinematics
inv[3] =-(inv[1] +inv[2]);// keep it parallell
// We know the wrist twist will always be 0 for this model
inv[4] = inv[0];//keep the camera orentation paralell from the base
//copy over remaining links so they do not move
for(int i=5;i<inv.length;i++){
inv[i]=jointSpaceVector[i];
}
return inv;
}
}
DyIO dyio=null;
if(DeviceManager.getSpecificDevice(DyIO.class, "dyio")==null){
// Adding in the DyIO.
if(useVirtual)
dyio = new DyIO(ConnectionDialog.promptConnection()); // This is the DyIO to talk to.
if(dyio==null ){
dyio = new DyIO(new SerialConnection("/dev/BowlerDevice.74F7260D0500")); // This is the DyIO to talk to.
}
dyio.connect(); // Connect to it.
DeviceManager.addConnection(dyio,"dyio");
}else{
dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio")
}
//OpenCVImageProvider clawCam;
//if(DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam")==null){
// // Adding the camera
// clawCam = new OpenCVImageProvider(0);
// DeviceManager.addConnection(clawCam,"clawCam");
//}else{
// clawCam = (OpenCVImageProvider)DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam");
//}
String xmlContent=null;
//define the file to load
//xmlContent = ScriptingEngineWidget.codeFromGistID("9bbfbcae11130cdd4c3d","trobotServo.xml")[0];
//Or use a local file
if(xmlContent==null)
xmlContent=new String(Files.readAllBytes(Paths.get(System.getProperty("user.home")+"/git/9bbfbcae11130cdd4c3d/trobotServo.xml")));
System.err.println("Starting Model")
//Create the model
DHParameterKinematics model=null;
if(DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm")==null){
model = new DHParameterKinematics(new VirtualGenericPIDDevice(100000),
IOUtils.toInputStream(xmlContent, "UTF-8"),
IOUtils.toInputStream(xmlContent, "UTF-8"));
//Add the custom inverse solver
model.setInverseSolver(new ComputedGeometricModel(model.getDhChain()));
DeviceManager.addConnection(model,"DHArm");
}else{
model = (DHParameterKinematics)DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm");
}
//Return new model to UI
//Creating a list of objects, one for each link
ArrayList<Object> links = new ArrayList<Object>();
model.setDesiredJointAxisValue(0, 0.1, 0);
for(DHLink dh : model.getDhChain().getLinks() ){
System.out.println("Link D-H values = "+dh);
// Create an axis to represent the link
double y = dh.d>0?dh.d:20;
double x= dh.r>0?dh.r:20;
CSG cube = new Cube(x,y,20).toCSG();
cube=cube.transformed(new Transform().translateX(-x/2));
cube=cube.transformed(new Transform().translateY(-y/2));
//add listner to axis
cube.setManipulator(dh.getListener());
// add ax to list of objects to be returned
links.add(cube);
}
return links;
Raw
SeekObjects.groovy
import java.awt.image.BufferedImage
import java.io.File;
import com.neuronrobotics.bowlerstudio.robots.FormacarumRover
import com.neuronrobotics.bowlerstudio.robots.RoverDataTable
import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngineWidget;
import com.neuronrobotics.jniloader.Detection
import com.neuronrobotics.jniloader.OpenCVImageProvider
import com.neuronrobotics.jniloader.SalientDetector
import com.neuronrobotics.sdk.addons.kinematics.DHParameterKinematics
import com.neuronrobotics.sdk.common.DeviceManager;
import com.neuronrobotics.sdk.dyio.DyIO
import com.neuronrobotics.sdk.dyio.peripherals.ServoChannel
import com.neuronrobotics.sdk.util.ThreadUtil;
import com.neuronrobotics.jniloader.AbstractImageProvider;
OpenCVImageProvider clawCam = (OpenCVImageProvider)DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "clawCam");
DHParameterKinematics model= (DHParameterKinematics)DeviceManager.getSpecificDevice(DHParameterKinematics.class, "DHArm"); // control of arm;
RoverDataTable datatable = (RoverDataTable)DeviceManager.getSpecificDevice(RoverDataTable.class, "datatable"); // list of all the stuff returned, ever *************************
FormacarumRover rover = (FormacarumRover)DeviceManager.getSpecificDevice(FormacarumRover.class, "rover");
DyIO dyio =(DyIO)DeviceManager.getSpecificDevice(DyIO.class, "dyio")
BufferedImage inputImage = AbstractImageProvider.newBufferImage(640,480);
BufferedImage outImage = AbstractImageProvider.newBufferImage(640,480);
SalientDetector FindStuff = (SalientDetector)datatable.getSpecificDetection("WhiteCylindar").getDetector();
double [] start = model.getCurrentJointSpaceVector();
start[0]=0;
start[1]=25;
start[2]=-30;
start[3]=-85;
start[4]=0;
model.setDesiredJointSpaceVector(start, 0)
// Adding the wrist-bend
ServoChannel wristBend = new ServoChannel (dyio.getChannel(9));
wristBend.SetPosition(41);
wristBend.flush();
ThreadUtil.wait(5000);
clawCam.captureNewImage(inputImage);
ArrayList<Detection> detect = FindStuff.getObjects(inputImage, outImage)
double degPerStep=5;
double startStep=0;
double negStep=-90;
double posStep =90;
while(dyio.getValue(23)==0){
long startScan = System.currentTimeMillis();
while(detect==null || detect.size()==0){
if(dyio.getValue(23)==0){
clawCam.captureNewImage(inputImage);
detect = FindStuff.getObjects(inputImage, outImage)
startStep+=degPerStep;
if(startStep>posStep || startStep<negStep)
degPerStep*=-1;
model.setDesiredJointAxisValue(0, startStep, 1);
ThreadUtil.wait(20000)
}
if( System.currentTimeMillis()-startScan<30000){
return;
}
}
if(detect.get(0).getConfidence() > 0.5){
println "Objects Found!!"
model.setDesiredJointAxisValue(3, -5, 1000)
ThreadUtil.wait(20000)
System.out.println("Running BITE!");
ScriptingEngineWidget.inlineFileScriptRun(new File(System.getProperty("user.home")+"/git/090e52d5b30090ddd636/Bite.groovy"), null);
}else{
//
//drive a bit
rover.DriveArc(200,((detect.get(0).getX()-320)/640)* 10 , 1000);
}
}
Raw
trobotServo.xml
<root>
<link>
<name>basePan</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>11</index>
<scale>.796</scale>
<upperLimit>196</upperLimit>
<lowerLimit>41</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>189</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>13</Delta>
<Theta>180</Theta>
<Radius>32</Radius>
<Alpha>-90</Alpha>
</DHParameters>
</link>
<link>
<name>baseTilt</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>10</index>
<scale>.796</scale>
<upperLimit>205</upperLimit>
<lowerLimit>41</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>90</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>25</Delta>
<Theta>-90</Theta>
<Radius>93</Radius>
<Alpha>180</Alpha>
</DHParameters>
</link>
<link>
<name>elbow</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>9</index>
<scale>.796</scale>
<upperLimit>205</upperLimit>
<lowerLimit>41</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>189</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>11</Delta>
<Theta>90</Theta>
<Radius>24</Radius>
<Alpha>90</Alpha>
</DHParameters>
</link>
<link>
<name>wrist1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>8</index>
<scale>.756</scale>
<upperLimit>197</upperLimit>
<lowerLimit>0</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>111</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>128</Delta>
<Theta>-90</Theta>
<Radius>0</Radius>
<Alpha>90</Alpha>
</DHParameters>
</link>
<link>
<name>wrist2</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>7</index>
<scale>.756</scale>
<upperLimit>240</upperLimit>
<lowerLimit>0</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>121</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>0</Delta>
<Theta>0</Theta>
<Radius>0</Radius>
<Alpha>-90</Alpha>
</DHParameters>
</link>
<link>
<name>wrist3</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>6</index>
<scale>.756</scale>
<upperLimit>255</upperLimit>
<lowerLimit>0</lowerLimit>
<isLatch>true</isLatch>
<indexLatch>171</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>500</homingTPS>
<DHParameters>
<Delta>25</Delta>
<Theta>90</Theta>
<Radius>0</Radius>
<Alpha>0</Alpha>
</DHParameters>
</link>
<baseToZframe>
<x>0</x>
<y>0</y>
<z>0</z>
<rotw>1</rotw>
<rotx>0</rotx>
<roty>0</roty>
<rotz>0</rotz>
</baseToZframe>
</root>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment