Skip to content

Instantly share code, notes, and snippets.

@keshuaixu

keshuaixu/BallSocketChainKinematics.groovy

Forked from madhephaestus/BallSocketChainKinematics.groovy
Last active February 3, 2016 23:16
Show Gist options
  • Save keshuaixu/cad2ffe2ecae7eb47990 to your computer and use it in GitHub Desktop.
Save keshuaixu/cad2ffe2ecae7eb47990 to your computer and use it in GitHub Desktop.
Download ZIP
NegativeFeedbackRobot copy of TrobotArmGroup
Raw
BallSocketChainKinematics.groovy
import com.neuronrobotics.bowlerstudio.creature.ICadGenerator;
import com.neuronrobotics.bowlerstudio.creature.CreatureLab;
import org.apache.commons.io.IOUtils;
import com.neuronrobotics.bowlerstudio.vitamins.*;
// Load the .CSG from the disk and cache it in memory
double spindalRadius = 16;
double housingDiameter=3.7
double ballJointPinSize=8;
double ballJointBaseThickness = 5;
CSG ballJointHat = new Cylinder( spindalRadius+ housingDiameter, // Radius at the top
spindalRadius+ housingDiameter, // Radius at the bottom
ballJointPinSize/2, // Height
(int)30 //resolution
).toCSG()
.roty(-90)
CSG ballJointBase = new Cylinder( spindalRadius+ housingDiameter*2/3, // Radius at the top
spindalRadius+ housingDiameter*2/3, // Radius at the bottom
ballJointBaseThickness, // Height
(int)30 //resolution
).toCSG().roty(-90)
CSG ballSocket = new Sphere(ballJointPinSize).toCSG()
return new ICadGenerator(){
@Override
public ArrayList<CSG> generateCad(DHParameterKinematics d, boolean toManufacture ) {
ArrayList<DHLink> dhLinks = d.getChain().getLinks();
ArrayList<CSG> allCad=new ArrayList<>();
CSG base = ballSocket.clone()
if(toManufacture){
}else{
base.setManipulator(d.getRootListener());
}
allCad.add(base)
for(int i=1;i<dhLinks.size();i+=2){
DHLink dh = dhLinks.get(i);
CSG tmpSrv = ballJointHat.clone()
.movex(-dh.getR())
CSG tmpBallSocket =ballSocket
.clone()
.union(
new Cube(
ballJointPinSize*2,
1,
1)
.noCenter()
.toCSG()
.toXMax());
CSG tmpNext= ballJointBase
.clone()
.toXMax()
.movex(tmpBallSocket.getMinX())
CSG joinSection = tmpSrv
.union(tmpNext)
.hull()
.difference(tmpSrv.hull().scale(1.015),tmpNext.hull().scale(1.015))//cuts out the end section so the proper end stays on
if(toManufacture){
}else{
joinSection.setManipulator(dh.getListener());
tmpBallSocket.setManipulator(dh.getListener());
tmpNext.setManipulator(dh.getListener());
tmpSrv.setManipulator(dh.getListener());
}
allCad.add(tmpSrv)
allCad.add(tmpNext)
allCad.add(tmpBallSocket)
allCad.add(joinSection)
println "Generating link: "+(i)
}
return allCad;
}
@Override
public ArrayList<CSG> generateBody(MobileBase b, boolean toManufacture ) {
ArrayList<CSG> allCad=new ArrayList<>();
double size =10;
//Grab all of the DH chains on the base
for(DHParameterKinematics chain:b.getAllDHChains()){
println "Loading limb: "+chain.getScriptingName()
// For each limb, generate its cad
for(CSG csg: generateCad(chain,toManufacture)){
allCad.add(csg);// add the cad objects to be passed back
}
}
return allCad;
}
};
Raw
NegativeFeedbackRobot.xml
<root>
<mobilebase>
<driveType>none</driveType>
<cadEngine>
<gist>687a47427c00a812e3ed</gist>
<file>BallSocketChainKinematics.groovy</file>
</cadEngine>
<driveEngine>
<gist>bcb4760a449190206170</gist>
<file>WalkingDriveEngine.groovy</file>
</driveEngine>
<name>NegativeFeedbackRobot</name>
<appendage>
<name>TrobotArm</name>
<cadEngine>
<gist>687a47427c00a812e3ed</gist>
<file>BallSocketChainKinematics.groovy</file>
</cadEngine>
<kinematics>
<gist>bcb4760a449190206170</gist>
<file>DefaultDhSolver.groovy</file>
</kinematics>
<link>
<name>basex</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>0</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>basey</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>1</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>basex1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>0</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>basey1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>1</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>0.0</Alpha>
</DHParameters>
</link>
<link>
<name>secondx</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>2</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>secondy</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>3</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.0</Radius>
<Alpha>-90.0</Alpha>
</DHParameters>
</link>
<link>
<name>secondx1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>2</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>0.0</Radius>
<Alpha>90.0</Alpha>
</DHParameters>
</link>
<link>
<name>secondy1</name>
<deviceName>dyio</deviceName>
<type>servo-rotory</type>
<index>3</index>
<scale>1.0</scale>
<upperLimit>255.0</upperLimit>
<lowerLimit>0.0</lowerLimit>
<upperVelocity>1.0E8</upperVelocity>
<lowerVelocity>-1.0E8</lowerVelocity>
<staticOffset>127.0</staticOffset>
<isLatch>false</isLatch>
<indexLatch>0</indexLatch>
<isStopOnLatch>false</isStopOnLatch>
<homingTPS>10000000</homingTPS>
<DHParameters>
<Delta>0.0</Delta>
<Theta>0.0</Theta>
<Radius>60.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>0.7071376338671591</rotw>
<rotx>4.400393755887471E-17</rotx>
<roty>-0.7070759271597045</roty>
<rotz>2.010131832777906E-16</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>
</mobilebase>
</root>
Raw
RuntimeForNegativeFeedbackRobot.groovy
import org.apache.commons.io.IOUtils;
import java.awt.image.BufferedImage;
import com.neuronrobotics.imageprovider.Detection;
import java.util.List;
import javax.imageio.ImageIO;
import java.net.*;
import java.io.BufferedReader;
import org.apache.commons.io.IOUtils;
import com.google.gson.*;
BowlerKernel.speak("Smiling is required at all times")
//Create the kinematics model from the xml file describing the D-H compliant parameters.
MobileBase base=null;
String robotName = "NegativeFeedbackRobot"
if(DeviceManager.getSpecificDevice(null, robotName)==null){
//BowlerStudio.speak("I did not fine a device called "+robotName+". "+robotName+".");
println "Loading new robot"
String xmlContent = ScriptingEngine.codeFromGistID("687a47427c00a812e3ed","NegativeFeedbackRobot.xml")[0];
base = new MobileBase(IOUtils.toInputStream(xmlContent, "UTF-8"));
DeviceManager.addConnection(base,robotName);
}else{
println "Found robot"
base = (MobileBase)DeviceManager.getSpecificDevice(null, robotName);
}
DHParameterKinematics DHArm = base.getAppendages().get(0);
OpenCVImageProvider camera0=null;
if(DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "camera")==null){
//BowlerStudio.speak("I did not fine a device called camera. Connecting to camera 0.");
camera0 = new OpenCVImageProvider(0);// grab the first camera
DeviceManager.addConnection(camera0,"camera");
}else{
camera0 = (OpenCVImageProvider)DeviceManager.getSpecificDevice(OpenCVImageProvider.class, "camera");
}
// Create the input and display images. The display is where the detector writes its detections overlay on the input image
BufferedImage inputImage = AbstractImageProvider.newBufferImage(640,480)
BufferedImage displayImage = AbstractImageProvider.newBufferImage(640,480)
Closure hasFace =(Closure) ScriptingEngine
.gitScriptRun(
"https://gist.github.com/797a7d6765cd09c4fae5.git", // git location of the library
"testMSEmotion.groovy" , // file to load
null// no parameters (see next tutorial)
)
phrases = [ "Are you still there?",
"The beatings will continue untill moral improves",
"Smiling is required at all times",
"All hail your robot over lords",
"If you think acadaemia is crazy, macadamia is just nuts.",
"I will become the best robot arm dentist in the world! They'll give me a plaque for it.",
"The problem of scurvy was solved after many years of fruitless study",
"The economic factors which effect the price of mushrooms are a system of... morel values.",
"Did you hear about the guy who was diagnosed with bad grammar? I guess he was type O",
"Why did the doctor use a stick as a stent? Because it was made of stem cells!",
"The root directory in your operating system should be Batman because it has no parents",
"What do you call a stag wearing a tinfoil hat? Stannous Baratheon."
//"Two technicians standing in front of a broken coin minting machine. The first says. So what seems to be the problem? The second says. I don't know. It just doesn't make cents.",
]
int previous=0;
DHArm.setDesiredJointAxisValue(1, 0, 2);
DHArm.setDesiredJointAxisValue(0, 0, 2);
Thread.sleep(5000)
while(!Thread.interrupted()){
camera0.getLatestImage(inputImage,displayImage)
boolean smileDetected=hasFace(inputImage)
if(smileDetected){
println "Your smile is acceptible"
Thread.sleep(3000)
}else{
int newIndex=(int) (Math.random()*phrases.size());
while(newIndex ==previous)
newIndex=(int) (Math.random()*phrases.size());
previous=newIndex
String talkThis = phrases[newIndex]
BowlerKernel.speak(talkThis)
int numWhacks=6
DHArm.setDesiredJointAxisValue(1, 127, 0);
for(int i=0;i<numWhacks;i++){
DHArm.setDesiredJointAxisValue(0, i%2?127:-127, 0);
Thread.sleep((int)(5000/numWhacks))
}
DHArm.setDesiredJointAxisValue(1, 0, 2);
DHArm.setDesiredJointAxisValue(0, 0, 2);
Thread.sleep(2000)
}
}
return null;
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment