diff --git a/Body.stl b/Body.stl
deleted file mode 100644
index 777931b7..00000000
Binary files a/Body.stl and /dev/null differ
diff --git a/BodyCover.stl b/BodyCover.stl
deleted file mode 100644
index d85d0f9f..00000000
Binary files a/BodyCover.stl and /dev/null differ
diff --git a/BodyRib.stl b/BodyRib.stl
new file mode 100644
index 00000000..e3a821cf
Binary files /dev/null and b/BodyRib.stl differ
diff --git a/CoverHinged.stl b/CoverHinged.stl
new file mode 100644
index 00000000..8203e375
Binary files /dev/null and b/CoverHinged.stl differ
diff --git a/Latch.stl b/Latch.stl
new file mode 100644
index 00000000..cf7c90ac
Binary files /dev/null and b/Latch.stl differ
diff --git a/Marcos.xml b/Marcos.xml
index 0c69352e..50921d8c 100644
--- a/Marcos.xml
+++ b/Marcos.xml
@@ -896,10 +896,10 @@
-85.0
40.25
137.0
- -0.4999999999993643
- -0.5000000000006358
- -0.49999999999936434
- 0.5000000000006358
+ -0.4999999999993642
+ -0.5000000000006357
+ -0.49999999999936423
+ 0.5000000000006357
@@ -1269,8 +1269,8 @@
false
- 0.01
- 0.0
+ 0.1
+ 0.4
0.0
0.0
1.0
@@ -1592,8 +1592,8 @@
-0.0
-0.0
- 0.01
- -50.0
+ 0.65
+ 10.0
0.0
110.0
1.0
diff --git a/MarcosCad.groovy b/MarcosCad.groovy
index 30f31b68..d5586945 100644
--- a/MarcosCad.groovy
+++ b/MarcosCad.groovy
@@ -782,13 +782,13 @@ class cadGenMarcos implements ICadGenerator{
}
//reorent the horn for resin printing
myServoHorn.setManufacturing({incoming ->
- return reverseDHValues(incoming, d, linkIndex).roty(linkIndex==0?(front?180:0):(left?180:0)).toZMin()
+ return null;//reverseDHValues(incoming, d, linkIndex).roty(linkIndex==0?(front?180:0):(left?180:0)).toZMin()
//.roty(45)
//.movez(5)
})
- myServoHorn.getStorage().set("bedType", "resin")
- myServoHorn.setPrintBedNumber(4)
- myServoHorn.setName("Resin Horn "+linkIndex+" "+d.getScriptingName())
+ //myServoHorn.getStorage().set("bedType", "resin")
+ //myServoHorn.setPrintBedNumber(4)
+ //myServoHorn.setName("Resin Horn "+linkIndex+" "+d.getScriptingName())
// attach this links manipulator
myServoHorn.setManipulator(dGetLinkObjectManipulator)
back.add(myServoHorn)
@@ -1185,10 +1185,10 @@ class cadGenMarcos implements ICadGenerator{
.rotz(angle)
.movex(-linkLen)
- CSG foot = Vitamins.get(ScriptingEngine.fileFromGit(
- "https://github.com/OperationSmallKat/Marcos.git",
- "Foot.stl"))
- .rotx(180)
+// CSG foot = Vitamins.get(ScriptingEngine.fileFromGit(
+// "https://github.com/OperationSmallKat/Marcos.git",
+// "Foot.stl"))
+// .rotx(180)
double c3 = Math.sin(Math.toRadians(angle))*linklen
double c1 = Math.cos(Math.toRadians(angle))*linklen
double c2= linkLen-c1;
@@ -1301,17 +1301,20 @@ class cadGenMarcos implements ICadGenerator{
CSG body = Vitamins.get(ScriptingEngine.fileFromGit(
"https://github.com/OperationSmallKat/Marcos.git",
- "Body.stl")).movez(zCenterLine);
+ "BodyRib.stl")).movez(zCenterLine);
CSG bodyCOver = Vitamins.get(ScriptingEngine.fileFromGit(
"https://github.com/OperationSmallKat/Marcos.git",
- "BodyCover.stl")).movez(zCenterLine);
+ "CoverHinged.stl")).movez(zCenterLine);
+ CSG bodyLatch = Vitamins.get(ScriptingEngine.fileFromGit(
+ "https://github.com/OperationSmallKat/Marcos.git",
+ "Latch.stl")).movez(zCenterLine);
CSG topCOver = Vitamins.get(ScriptingEngine.fileFromGit(
"https://github.com/OperationSmallKat/Marcos.git",
"BodyServoCoverTop.stl")).movez(zCenterLine);
CSG BottomCover = Vitamins.get(ScriptingEngine.fileFromGit(
"https://github.com/OperationSmallKat/Marcos.git",
"BodyCoverBottom.stl")).movez(zCenterLine);
- ArrayList back =[body, bodyCOver]
+ ArrayList back =[body, bodyCOver,bodyLatch]
for(CSG c:back) {
c.getStorage().set("bedType", "ff-One")
c.setPrintBedNumber(5)
@@ -1395,6 +1398,10 @@ class cadGenMarcos implements ICadGenerator{
bodyCOver.setName("BodyCover")
bodyCOver.addAssemblyStep(6, new Transform().movez(80))
body.setName("Body")
+ bodyLatch.setName("Latch")
+ bodyLatch.setManufacturing({ incoming ->
+ return incoming.toZMin().toXMin().toYMin()
+ })
body.setManufacturing({ incoming ->
return incoming.rotx(180).toZMin().toXMin().toYMin()
})
@@ -1560,7 +1567,7 @@ class cadGenMarcos implements ICadGenerator{
spars.setColor(Color.DARKRED)
back.add(spars)
spars.getStorage().set("no-physics", true);
-
+ motherboard.getStorage().set("no-physics", true);
back.addAll([
battery,
batteryInterface,
diff --git a/MarcosWalk.groovy b/MarcosWalk.groovy
index b97872d9..71003609 100644
--- a/MarcosWalk.groovy
+++ b/MarcosWalk.groovy
@@ -4,6 +4,7 @@ import com.neuronrobotics.sdk.addons.kinematics.IDriveEngine
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.addons.kinematics.time.TimeKeeper
import com.neuronrobotics.sdk.common.DeviceManager
import com.neuronrobotics.sdk.common.Log
@@ -36,7 +37,7 @@ IDriveEngine engine = new IDriveEngine () {
boolean firstRun=true
double zoffsetOfFeetHome = -18
- double xOffsetOfFeetHome = 2.5
+ double xOffsetOfFeetHome = 7
double ySplayOut = 5
double stepOverHeight = 10
public void DriveArc(MobileBase source,TransformNR newPose,double seconds) {
@@ -56,11 +57,18 @@ IDriveEngine engine = new IDriveEngine () {
})
return bc;
})
+ if(Math.abs(con.tiltAngleGlobal)>4) {
+ println "Tilting Recovery "+con.tiltAngleGlobal
+ newPose=newPose.translateY(con.tiltAngleGlobal/10.0)
+ }else {
+ println "Safe "+con.tiltAngleGlobal
+ }
+ con.time = source
con.stepOverHeight=stepOverHeight
con.source=source
con.incomingPose=newPose
con.incomingSeconds=seconds
- con.timeOfMostRecentCommand=System.currentTimeMillis()
+ con.timeOfMostRecentCommand=source.currentTimeMillis()
}catch(Throwable t) {
t.printStackTrace()
}
@@ -98,6 +106,7 @@ class BodyController{
int numberOfInterpolatedPointsInALoop = numPointsInLoop*(numberOfInterpolationPoints+1)
MobileBase source=null;
MobileBase lastSource=null
+ TimeKeeper time;
TransformNR newPose=null;
TransformNR incomingPose=null;
double incomingSeconds=0;
@@ -113,9 +122,15 @@ class BodyController{
long timeOfStartOfCycle = System.currentTimeMillis()
double tailDefaultLift=0;
boolean isVirtualMode = false;
+ double tiltAngleGlobal = 0;
private void loop() {
- double timeElapsedSinceLastCommand = ((double)(System.currentTimeMillis()-timeOfMostRecentCommand))/1000.0
+ long now ;
+ if(time!=null)
+ now= time.currentTimeMillis()
+ else
+ now=System.currentTimeMillis()
+ double timeElapsedSinceLastCommand = ((double)(now-timeOfMostRecentCommand))/1000.0
switch(state) {
case CycleState.waiting:
if(source==null) {
@@ -175,8 +190,9 @@ class BodyController{
//no break
case CycleState.cycleStart:
//println "Walk cycle starting "+cycleTime + " last Took "+(System.currentTimeMillis()-timeOfStartOfCycle);
- timeOfStartOfCycle = System.currentTimeMillis()
+ timeOfStartOfCycle = now
pontsIndex=0;
+
newPose=incomingPose
seconds=incomingSeconds
setupCycle()
@@ -210,7 +226,13 @@ class BodyController{
break;
}
}
-
+ private void forceSteps() {
+ incomingPose=new TransformNR(0,tiltAngleGlobal,0)
+ incomingSeconds=seconds
+ timeOfMostRecentCommand=time.currentTimeMillis()
+ source=lastSource
+ //println "Forcing steps"
+ }
private void runDynamics() {
if(measuredPose!=null) {
double tiltAngle = Math.toDegrees(measuredPose.getRotation().getRotationTilt())
@@ -231,6 +253,7 @@ class BodyController{
else
tiltAngle=-max
}
+ tiltAngleGlobal=tiltAngle
def coriolisIndexCoriolisDivisionsScale = coriolisIndex*coriolisDivisionsScale
def coriolisIndexCoriolisDivisionsScaleTiltAngle = coriolisIndexCoriolisDivisionsScale+tiltAngle
@@ -250,9 +273,12 @@ class BodyController{
double[] vect =tail.getCurrentJointSpaceVector()
vect[0]=computedTilt
vect[1]=computedPan//+tiltAngle
- if(Math.abs(tiltAngle)<10)
+ if(Math.abs(tiltAngle)<6)
vect[1]+=tiltAngle*2;
else {
+ if(state == CycleState.waiting) {
+ forceSteps();
+ }
vect[1]+=tiltAngle>0?20:-20;
}
tail.setDesiredJointSpaceVector(vect, 0)
@@ -380,23 +406,35 @@ class BodyController{
// initialize real-time initial conditions
// run body controller until disconnected
while(availible) {
- long start = System.currentTimeMillis();
-
+ long start;
+ if (time!=null)
+ start = time.currentTimeMillis();
+ else
+ start = System.currentTimeMillis();
// update the gait generation
loop();
// update the dynamics controller
runDynamics();
// compute the real-time condition
- long elapsed = System.currentTimeMillis()-(start )
+ long end;
+ if (time!=null)
+ end = time.currentTimeMillis();
+ else
+ end = System.currentTimeMillis();
+ long elapsed = end-(start )
def numMsOfLoopElapsed = numMsOfLoop-elapsed
// check for real-time overrun
+ if(source==null) {
+ Thread.sleep(16);
+ continue;
+ }
if(numMsOfLoopElapsed<0) {
println "Real time in Body Controller broken! Loop took:"+elapsed+" sleep time "+numMsOfLoopElapsed
// this controller must run slower than the UI thread
- Thread.sleep(16);
+ source.sleep(16);
}else {
// sleep for the computed amount of time to keep the start of loop consistant
- Thread.sleep(numMsOfLoopElapsed);
+ source.sleep(numMsOfLoopElapsed);
}
// update the real-time index
}
diff --git a/README.md b/README.md
index 957f9ffa..6498308e 100644
--- a/README.md
+++ b/README.md
@@ -1,3 +1,5 @@
# Marcos
BowlerStudio native source for SmallKat Marcos model
+
+![SmallkatMarcosCAD.png](SmallkatMarcosCAD.png)
\ No newline at end of file
diff --git a/SmallkatMarcosCAD.png b/SmallkatMarcosCAD.png
new file mode 100644
index 00000000..65f17942
Binary files /dev/null and b/SmallkatMarcosCAD.png differ
diff --git a/mujoco.xml b/mujoco.xml
index 71b39a0e..ce685586 100644
--- a/mujoco.xml
+++ b/mujoco.xml
@@ -25,7 +25,7 @@
-
+
diff --git a/printbed.json b/printbed.json
index a0736c82..a6c1c967 100644
--- a/printbed.json
+++ b/printbed.json
@@ -1,7 +1,7 @@
{
"locations": {
"BodyCover": {
- "x": -163.51738832868395,
+ "x": -171.05033226993706,
"y": 121.12000274658203,
"z": 0.0,
"rotation": {
@@ -79,8 +79,8 @@
}
},
"Head_Head": {
- "x": -97.70369162302464,
- "y": 130.5737533569336,
+ "x": -114.29362746337146,
+ "y": 143.96000904231553,
"z": 0.0,
"rotation": {
"storage": {
@@ -1040,6 +1040,19 @@
}
}
},
+ "Latch": {
+ "x": -156.48960846775893,
+ "y": 405.7274849075315,
+ "z": 0.0,
+ "rotation": {
+ "storage": {
+ "q0": 1.0,
+ "q1": 0.0,
+ "q2": 0.0,
+ "q3": 0.0
+ }
+ }
+ },
"Resin Horn 2 LeftRear": {
"x": 54.01005893814598,
"y": 31.011142291845015,