Merge branch 'main' of https://github.com/OperationSmallKat/Marcos.git …

…into main

Marcos.xml

@@ -896,10 +896,10 @@
 	<x>-85.0</x>
 	<y>40.25</y>
 	<z>137.0</z>
-	<rotw>-0.4999999999993643</rotw>
-	<rotx>-0.5000000000006358</rotx>
-	<roty>-0.49999999999936434</roty>
-	<rotz>0.5000000000006358</rotz>
+	<rotw>-0.4999999999993642</rotw>
+	<rotx>-0.5000000000006357</rotx>
+	<roty>-0.49999999999936423</roty>
+	<rotz>0.5000000000006357</rotz>
 </baseToZframe>
 
 </appendage>
@@ -1269,8 +1269,8 @@
 
 	</vitamins>
 	<passive>false</passive>
-	<mass>0.01</mass>
-	<centerOfMassFromCentroid>	<x>0.0</x>
+	<mass>0.1</mass>
+	<centerOfMassFromCentroid>	<x>0.4</x>
 	<y>0.0</y>
 	<z>0.0</z>
 	<rotw>1.0</rotw>
@@ -1592,8 +1592,8 @@
 	<roty>-0.0</roty>
 	<rotz>-0.0</rotz>
 </baseToZframe>
-	<mass>0.01</mass>
-	<centerOfMassFromCentroid>	<x>-50.0</x>
+	<mass>0.65</mass>
+	<centerOfMassFromCentroid>	<x>10.0</x>
 	<y>0.0</y>
 	<z>110.0</z>
 	<rotw>1.0</rotw>

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<CSG> back =[body, bodyCOver]
+		ArrayList<CSG> 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,

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
 					}

README.md

@@ -1,3 +1,5 @@
 # Marcos
 
 BowlerStudio native source for SmallKat Marcos model
+
+![SmallkatMarcosCAD.png](SmallkatMarcosCAD.png)

mujoco.xml

@@ -25,7 +25,7 @@
   </default>
   <worldbody>
     <geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>
-    <body name="Marcos_base" pos="0.0 0.0 0.13843452850341798" childclass="body">
+    <body name="Marcos_base" pos="0.0 0.0 0.14144048515319824" childclass="body">
       <freejoint name="root"/>
       <geom name="Marcos_base_1" pos="0.07749500000000001 -0.046755000000000005 0.119995" size="0.01"/>
       <body name="basePan_RightFront" pos="0.0775 -0.04675 0.12"  quat="6.171209208705922E-5 -0.7071684825084507 6.170132223498458E-5 0.7070450690947923">

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,