Pass in a transform provider for the tip of the limb

CommonWealthRobotics · Apr 14, 2024 · 66b5798 · 66b5798
1 parent 031c66b
commit 66b5798
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 7 deletions.
diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/MobleBaseMenueFactory.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/MobleBaseMenueFactory.java
@@ -61,7 +61,7 @@ public static String[] copyGitFile(String sourceGit, String targetGit, String fi
 
 	public static void addVitamins(IVitaminHolder vitamins,  TreeItem<String> rootItem,
 			HashMap<TreeItem<String>, Runnable> callbackMapForTreeitems,
-			HashMap<TreeItem<String>, Group> widgetMapForTreeitems) {
+			HashMap<TreeItem<String>, Group> widgetMapForTreeitems,ITransformProvider tfp) {
 		TreeItem<String> vitaminsMenu = new TreeItem<String>("Vitamins Add/Remove",
 				AssetFactory.loadIcon("Vitamins.png"));
 
@@ -73,7 +73,7 @@ public static void addVitamins(IVitaminHolder vitamins,  TreeItem<String> rootIt
 					//loader.setClassLoader(VitatminWidget.class.getClassLoader());
 					Parent w = loader.load();
 					VitatminWidget tw = loader.getController();
-					tw.setVitaminProvider(vitamins);
+					tw.setVitaminProvider(vitamins,tfp);
 					widgetMapForTreeitems.put(vitaminsMenu, new Group(w));
 				} catch (Exception e) {
 					// TODO Auto-generated catch block
@@ -489,7 +489,9 @@ public void onTransformChaging(TransformNR newTrans) {
 			});
 
 			rootItem.getChildren().addAll(bodymass, imuCenter,PlaceLimb);
-			addVitamins( device,   rootItem, callbackMapForTreeitems, widgetMapForTreeitems);
+			addVitamins( device,   rootItem, callbackMapForTreeitems, widgetMapForTreeitems, ()->{
+				 return device.forwardOffset(new TransformNR()); 
+			});
 			if (root)
 				rootItem.getChildren().addAll(  printable,arrangeBed, kinematics);
 			rootItem.getChildren().addAll(addArm, addleg, addFixed, addsteerable);
@@ -972,7 +974,9 @@ public void onSliderDoneMoving(EngineeringUnitsSliderWidget source, double newAn
 
 
 		link.getChildren().addAll(design);
-		addVitamins( dh.getLinkConfiguration(linkIndex),   link, callbackMapForTreeitems, widgetMapForTreeitems);
+		addVitamins( dh.getLinkConfiguration(linkIndex),   link, callbackMapForTreeitems, widgetMapForTreeitems,()->{
+			 return dh.getLinkTip(linkIndex); 
+		});
 
 
 		link.getChildren().addAll(slaves, remove);

diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/VitaminWidgetTest.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/VitaminWidgetTest.java
@@ -6,6 +6,7 @@
 import com.neuronrobotics.bowlerstudio.assets.AssetFactory;
 import com.neuronrobotics.bowlerstudio.scripting.ScriptingEngine;
 import com.neuronrobotics.sdk.addons.kinematics.MobileBase;
+import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
 
 import eu.mihosoft.vrl.v3d.JavaFXInitializer;
 import javafx.application.Application;
@@ -56,7 +57,9 @@ public static void main(String[] args) {
 				try {
 					MobileBase mb = (MobileBase) ScriptingEngine
 							.gitScriptRun("https://github.com/NeuronRobotics/NASACurisoity.git", "NASA_Curiosity.xml");
-					tw.setVitaminProvider(mb.getAllDHChains().get(0).getLinkConfiguration(0));
+					tw.setVitaminProvider(mb.getAllDHChains().get(0).getLinkConfiguration(0),()->{
+						 return mb.forwardOffset(new TransformNR()); 
+					});
 				} catch (Exception e) {
 					// TODO Auto-generated catch block
 					e.printStackTrace();

diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/creature/VitatminWidget.java b/src/main/java/com/neuronrobotics/bowlerstudio/creature/VitatminWidget.java
@@ -19,6 +19,7 @@
 import com.neuronrobotics.sdk.addons.kinematics.IVitaminHolder;
 import com.neuronrobotics.sdk.addons.kinematics.VitaminFrame;
 import com.neuronrobotics.sdk.addons.kinematics.VitaminLocation;
+import com.neuronrobotics.sdk.addons.kinematics.math.RotationNR;
 import com.neuronrobotics.sdk.addons.kinematics.math.TransformNR;
 
 import javafx.event.ActionEvent;
@@ -61,6 +62,7 @@ public class VitatminWidget implements IOnTransformChange {
 	private IVitaminHolder holder;
 	private HashMap<GridPane, VitaminLocation> locationMap = new HashMap<>();
 	private VitaminLocation selectedVitamin;
+	private ITransformProvider currentTipProvider;
 
 	@FXML
 	void onAdd(ActionEvent event) {
@@ -178,14 +180,17 @@ private void fireVitaminSelectedUpdate() {
 		MobileBaseCadManager manager = MobileBaseCadManager.get(holder);
 		try {
 			Affine af = manager.getVitaminAffine(selectedVitamin);
-			BowlerStudioController.setSelectedAffine(af);
+			TransformNR poseToMove = currentTipProvider.get().copy();
+			//poseToMove.setRotation(new RotationNR());
+			BowlerStudioController.targetAndFollow(poseToMove,af);
 		} catch (Exception e) {
 			//e.printStackTrace();
 		}
 	}
 
-	public void setVitaminProvider(IVitaminHolder h) {
+	public void setVitaminProvider(IVitaminHolder h,ITransformProvider currentTipProvider) {
 		this.holder = h;
+		this.currentTipProvider = currentTipProvider;
 		for (VitaminLocation l : h.getVitamins()) {
 			add(l);
 		}