home links recoursivly to get follower links

CommonWealthRobotics · Mar 22, 2024 · 016e116 · 016e116
1 parent f883383
commit 016e116
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 21 deletions.
diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/CreaturePhysicsWidget.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/CreaturePhysicsWidget.java
@@ -54,9 +54,9 @@ public CreaturePhysicsWidget(MobileBase base){
 
 			@Override
 			public void onDisconnect(BowlerAbstractDevice arg0) {
-				//BowlerStudio.runLater(()->{
-					stop();
-				//});
+				BowlerStudio.runLater(()->{
+					stopPhysics();
+				});
 			}
 
 			@Override
@@ -86,7 +86,7 @@ public void onConnect(BowlerAbstractDevice arg0) {}
 		});
 		runstop.setOnAction(event->{
 			if(isRun()){
-				stop();
+				stopPhysics();
 //				new Thread(){
 //					public void run(){
 //						ThreadUtil.wait(50);
@@ -134,7 +134,7 @@ public void run(){
 							mujoco.generateNewModel();
 						} catch (Exception e) {
 							// TODO Auto-generated catch block
-							e.printStackTrace();
+							e.printStackTrace(System.out);
 							close();
 							return;
 						} // generate model before start counting time
@@ -167,6 +167,7 @@ private void close() {
 						if(mujoco!=null)
 							mujoco.close();
 						mujoco=null;
+						stopPhysics();
 					}
 				}.start();
 
@@ -175,7 +176,7 @@ private void close() {
 		});
 
 	}
-	private void stop() {
+	private void stopPhysics() {
 		runstop.setGraphic(AssetFactory.loadIcon("Run.png"));
 		runstop.setText("Run");
 		if(physicsThread!=null)

diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/JogMobileBase.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/JogMobileBase.java
@@ -424,15 +424,23 @@ private void handle(final Button button) {
 	public void home() {
 
 		getMobilebase().setGlobalToFiducialTransform(new TransformNR());
-		for (DHParameterKinematics c : getMobilebase().getAllDHChains()) {
+		homeBase( getMobilebase());
+
+	}
+
+	private void homeBase(MobileBase mb) {
+		for (DHParameterKinematics c : mb.getAllDHChains()) {
 			homeLimb(c);
 		}
 	}
 
-	private void homeLimb(AbstractKinematicsNR c) {
+	private void homeLimb(DHParameterKinematics c) {
 		double[] joints = c.getCurrentJointSpaceVector();
 		for (int i = 0; i < c.getNumberOfLinks(); i++) {
 			joints[i] = 0;
+			if(c.getFollowerMobileBase(i)!=null) {
+				homeBase(c.getFollowerMobileBase(i));
+			}
 		}
 		try {
 			c.setDesiredJointSpaceVector(joints, 0);

diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/JogWidget.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/JogWidget.java
@@ -264,23 +264,27 @@ public void home() {
 		}).start();
 	}
 
-	private void homeLimb(AbstractKinematicsNR c) {
+	private void homeBase(MobileBase mb) {
+		for (DHParameterKinematics c : mb.getAllDHChains()) {
+			homeLimb(c);
+		}
+	}
 
-		TransformNR t = c.calcHome();
+	private void homeLimb(DHParameterKinematics c) {
+		double[] joints = c.getCurrentJointSpaceVector();
+		for (int i = 0; i < c.getNumberOfLinks(); i++) {
+			joints[i] = 0;
+			if(c.getFollowerMobileBase(i)!=null) {
+				homeBase(c.getFollowerMobileBase(i));
+			}
+		}
 		try {
-			c.setDesiredTaskSpaceTransform(t, 0);
+			c.setDesiredJointSpaceVector(joints, 0);
 		} catch (Exception e) {
-			double[] joints = c.getCurrentJointSpaceVector();
-			for (int i = 0; i < c.getNumberOfLinks(); i++) {
-				joints[i] = 0;
-			}
-			try {
-				c.setDesiredJointSpaceVector(joints, 0);
-			} catch (Exception ex) {
-				// TODO Auto-generated catch block
-				ex.printStackTrace();
-			}
+			// TODO Auto-generated catch block
+			e.printStackTrace();
 		}
+
 	}
 
 	@Override