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,