diff --git a/pom.xml b/pom.xml index 864edc374..e7e73fa43 100644 --- a/pom.xml +++ b/pom.xml @@ -6,7 +6,7 @@ com.marginallyclever RobotOverlord - 2.105.1 + 2.105.8 Robot Overlord A friendly 3D user interface for controlling robots. https://www.marginallyclever.com/ @@ -302,6 +302,31 @@ maven-project-info-reports-plugin 2.8 + + + org.jacoco + jacoco-maven-plugin + 0.8.11 + + + com/marginallyclever/robotoverlord/** + + + + + + prepare-agent + + + + generate-code-coverage-report + test + + report + + + + @@ -388,6 +413,14 @@ test + + + org.mockito + mockito-core + 5.8.0 + test + + diff --git a/src/main/java/com/marginallyclever/ro3/Registry.java b/src/main/java/com/marginallyclever/ro3/Registry.java index 7e8699872..3b2c99858 100644 --- a/src/main/java/com/marginallyclever/ro3/Registry.java +++ b/src/main/java/com/marginallyclever/ro3/Registry.java @@ -4,6 +4,7 @@ import com.marginallyclever.ro3.mesh.MeshFactory; import com.marginallyclever.ro3.node.nodes.*; import com.marginallyclever.ro3.node.Node; +import com.marginallyclever.ro3.node.nodes.limbplanner.LimbPlanner; import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; import com.marginallyclever.ro3.node.nodes.marlinrobotarm.MarlinRobotArm; import com.marginallyclever.ro3.node.nodes.pose.AttachmentPoint; @@ -32,7 +33,6 @@ public class Registry { public static final ListWithEvents cameras = new ListWithEvents<>(); private static Camera activeCamera = null; public static final ListWithEvents selection = new ListWithEvents<>(); - public static final Clipboard clipboard = Toolkit.getDefaultToolkit().getSystemClipboard(); public static void start() { nodeFactory.clear(); @@ -44,7 +44,7 @@ public static void start() { nodule.add("Material", Material::new); nodule.add("MeshInstance", MeshInstance::new); nodule.add("Motor", Motor::new); - nodule.add("TargetPlanner", TargetPlanner::new); + nodule.add("LimbPlanner", LimbPlanner::new); Factory.Category pose = nodule.add("Pose", Pose::new); pose.add("Camera", Camera::new); pose.add("LookAt", LookAt::new); diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/ExportScene.java b/src/main/java/com/marginallyclever/ro3/apps/actions/ExportScene.java index 6c1190baf..1b5b922a6 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/ExportScene.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/ExportScene.java @@ -9,6 +9,7 @@ import java.io.*; +import java.security.InvalidParameterException; import java.util.*; import javax.swing.*; import javax.swing.filechooser.FileNameExtensionFilter; @@ -42,6 +43,7 @@ public ExportScene(JFileChooser chooser) { */ @Override public void actionPerformed(ActionEvent e) { + if( chooser == null ) throw new InvalidParameterException("file chooser cannot be null"); chooser.setFileFilter(ZIP_FILTER); Component source = (Component) e.getSource(); JFrame parentFrame = (JFrame)SwingUtilities.getWindowAncestor(source); diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/ImportScene.java b/src/main/java/com/marginallyclever/ro3/apps/actions/ImportScene.java index 8011441f9..9380116a3 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/ImportScene.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/ImportScene.java @@ -8,6 +8,7 @@ import javax.swing.*; import java.awt.*; import java.awt.event.ActionEvent; +import java.security.InvalidParameterException; import java.util.Objects; /** @@ -36,6 +37,7 @@ public ImportScene(JFileChooser chooser) { */ @Override public void actionPerformed(ActionEvent e) { + if( chooser == null ) throw new InvalidParameterException("file chooser cannot be null"); chooser.setFileFilter(RO3Frame.FILE_FILTER); Component source = (Component) e.getSource(); diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java b/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java index 182e38301..a2c1e9ec7 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java @@ -1,5 +1,6 @@ package com.marginallyclever.ro3.apps.actions; +import com.marginallyclever.ro3.apps.RO3Frame; import com.marginallyclever.ro3.apps.UndoSystem; import com.marginallyclever.ro3.apps.RecentFilesMenu; import com.marginallyclever.ro3.Registry; @@ -76,6 +77,7 @@ public void actionPerformed(ActionEvent e) { private File runFileDialog(Component source) { if( chooser == null ) throw new InvalidParameterException("file chooser cannot be null"); + chooser.setFileFilter(RO3Frame.FILE_FILTER); JFrame parentFrame = (JFrame)SwingUtilities.getWindowAncestor(source); if (chooser.showOpenDialog(parentFrame) == JFileChooser.APPROVE_OPTION) { return chooser.getSelectedFile(); diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/PasteNode.java b/src/main/java/com/marginallyclever/ro3/apps/actions/PasteNode.java index 1ffc60f74..f6c4b34b0 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/PasteNode.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/PasteNode.java @@ -8,6 +8,7 @@ import javax.swing.*; import java.awt.*; +import java.awt.datatransfer.Clipboard; import java.awt.datatransfer.FlavorEvent; import java.awt.event.ActionEvent; import java.util.ArrayList; @@ -18,6 +19,7 @@ */ public class PasteNode extends AbstractAction { private final Logger logger = LoggerFactory.getLogger(PasteNode.class); + private final Clipboard clipboard = Toolkit.getDefaultToolkit().getSystemClipboard(); public PasteNode() { super(); @@ -25,19 +27,19 @@ public PasteNode() { putValue(Action.SMALL_ICON,new ImageIcon(Objects.requireNonNull(getClass().getResource("icons8-paste-16.png")))); putValue(SHORT_DESCRIPTION,"Paste the selected node(s)."); - Registry.clipboard.addFlavorListener(this::clipboardChanged); + clipboard.addFlavorListener(this::clipboardChanged); setEnabled(false); } private void clipboardChanged(FlavorEvent flavorEvent) { // if the clipboard has changed, update the menu item - if(flavorEvent.getSource()==Registry.clipboard) { + if(flavorEvent.getSource()==clipboard) { if(KeyboardFocusManager.getCurrentKeyboardFocusManager().getActiveWindow() == null) { setEnabled(false); return; } try { - setEnabled(Registry.clipboard.isDataFlavorAvailable(JSONHelper.JSON_FLAVOR)); + setEnabled(clipboard.isDataFlavorAvailable(JSONHelper.JSON_FLAVOR)); } catch (IllegalStateException ignored) { // if this clipboard is currently unavailable } diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/SaveAsScene.java b/src/main/java/com/marginallyclever/ro3/apps/actions/SaveAsScene.java index fa82391eb..e6e51100e 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/SaveAsScene.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/SaveAsScene.java @@ -1,6 +1,7 @@ package com.marginallyclever.ro3.apps.actions; import com.marginallyclever.ro3.Registry; +import com.marginallyclever.ro3.apps.RO3Frame; import com.marginallyclever.ro3.apps.RecentFilesMenu; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -12,6 +13,7 @@ import java.io.File; import java.io.FileWriter; import java.io.IOException; +import java.security.InvalidParameterException; import java.util.Objects; /** @@ -54,6 +56,9 @@ public void actionPerformed(ActionEvent e) { } private String askUserForDestinationPath(Component source) { + if( chooser == null ) throw new InvalidParameterException("file chooser cannot be null"); + chooser.setFileFilter(RO3Frame.FILE_FILTER); + JFrame parentFrame = (JFrame)SwingUtilities.getWindowAncestor(source); int response; diff --git a/src/main/java/com/marginallyclever/ro3/apps/commands/CopyNode.java b/src/main/java/com/marginallyclever/ro3/apps/commands/CopyNode.java index 4d72b3331..cc48495d3 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/commands/CopyNode.java +++ b/src/main/java/com/marginallyclever/ro3/apps/commands/CopyNode.java @@ -9,6 +9,8 @@ import javax.swing.undo.AbstractUndoableEdit; import javax.swing.undo.CannotUndoException; +import java.awt.*; +import java.awt.datatransfer.Clipboard; import java.awt.datatransfer.StringSelection; import java.awt.datatransfer.Transferable; import java.util.List; @@ -20,10 +22,12 @@ public class CopyNode extends AbstractUndoableEdit { private final Logger logger = LoggerFactory.getLogger(com.marginallyclever.ro3.apps.actions.CopyNode.class); private final List selection; private final Transferable before; + private final Clipboard clipboard = Toolkit.getDefaultToolkit().getSystemClipboard(); + public CopyNode(List selection) { super(); this.selection = selection; - this.before = Registry.clipboard.getContents(null); + this.before = clipboard.getContents(null); execute(); } @@ -48,7 +52,7 @@ public void execute() { jsonWrapper.put("copied",list); // store the json in the clipboard. StringSelection stringSelection = new StringSelection(jsonWrapper.toString()); - Registry.clipboard.setContents(stringSelection, null); + clipboard.setContents(stringSelection, null); } @Override @@ -58,6 +62,6 @@ public void undo() throws CannotUndoException { } public void reverse() { - Registry.clipboard.setContents(before, null); + clipboard.setContents(before, null); } } diff --git a/src/main/java/com/marginallyclever/ro3/apps/commands/PasteNode.java b/src/main/java/com/marginallyclever/ro3/apps/commands/PasteNode.java index 212d74b4f..f17641412 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/commands/PasteNode.java +++ b/src/main/java/com/marginallyclever/ro3/apps/commands/PasteNode.java @@ -9,6 +9,8 @@ import javax.swing.undo.AbstractUndoableEdit; import javax.swing.undo.CannotUndoException; +import java.awt.*; +import java.awt.datatransfer.Clipboard; import java.awt.datatransfer.Transferable; import java.util.ArrayList; import java.util.List; @@ -18,9 +20,10 @@ */ public class PasteNode extends AbstractUndoableEdit { private final Logger logger = LoggerFactory.getLogger(PasteNode.class); - Transferable transfer; private final List children = new ArrayList<>(); private final List parents; + private final Clipboard clipboard = Toolkit.getDefaultToolkit().getSystemClipboard(); + private final Transferable transfer; /** * Paste the copied nodes as children of the parent nodes. @@ -29,7 +32,7 @@ public class PasteNode extends AbstractUndoableEdit { public PasteNode(List parents) { super(); this.parents = parents; - transfer = Registry.clipboard.getContents(null); + transfer = clipboard.getContents(null); execute(); } diff --git a/src/main/java/com/marginallyclever/ro3/apps/viewport/viewporttools/move/RotateToolOneAxis.java b/src/main/java/com/marginallyclever/ro3/apps/viewport/viewporttools/move/RotateToolOneAxis.java index 1839fac6e..7ef65f207 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/viewport/viewporttools/move/RotateToolOneAxis.java +++ b/src/main/java/com/marginallyclever/ro3/apps/viewport/viewporttools/move/RotateToolOneAxis.java @@ -106,6 +106,7 @@ public RotateToolOneAxis(ColorRGB color) { this.color = color; buildMarkerMesh(); buildAngleMesh(); + ringMesh.setRenderStyle(GL3.GL_LINE_LOOP); } private void buildAngleMesh() { @@ -389,8 +390,11 @@ private void drawWhileDragging(GL3 gl,ShaderProgram shaderProgram) { private void drawMainRingAndHandles(GL3 gl,ShaderProgram shaderProgram) { Matrix4d m = new Matrix4d(pivotMatrix); - m.transpose(); - shaderProgram.setMatrix4d(gl,"modelMatrix",m); + + Matrix4d scale = MatrixHelper.createScaleMatrix4(getRingRadiusScaled()); + scale.mul(m,scale); + scale.transpose(); + shaderProgram.setMatrix4d(gl,"modelMatrix",scale); float colorScale = cursorOverHandle ? 1:0.5f; float red = color.red * colorScale / 255f; @@ -399,6 +403,9 @@ private void drawMainRingAndHandles(GL3 gl,ShaderProgram shaderProgram) { shaderProgram.set4f(gl, "objectColor", red, green, blue, 1.0f); ringMesh.render(gl,1,360); + m.transpose(); + shaderProgram.setMatrix4d(gl,"modelMatrix",m); + Matrix4d m2 = MatrixHelper.createScaleMatrix4(getGripRadiusScaled()); m2.m03 = getHandleLengthScaled(); m2.m13 = getHandleOffsetYScaled(); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java b/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java index fb29aef2a..ae470f8bb 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/DHParameter.java @@ -178,18 +178,34 @@ public void fromJSON(JSONObject from) { if(from.has("alpha")) alpha = from.getDouble("alpha"); } + public void setD(double d) { + this.d = d; + } + public double getD() { return d; } + public void setR(double r) { + this.r = r; + } + public double getR() { return r; } + public void setAlpha(double alpha) { + this.alpha = alpha; + } + public double getAlpha() { return alpha; } + public void setTheta(double theta) { + this.theta = theta; + } + public double getTheta() { return theta; } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java b/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java index a26250f6f..fb0a24e2a 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Material.java @@ -63,7 +63,7 @@ public void getComponents(List list) { textureChooserDialog.setSelectedItem(texture); int result = textureChooserDialog.run(pane); if(result == JFileChooser.APPROVE_OPTION) { - texture = textureChooserDialog.getSelectedItem(); + setTexture(textureChooserDialog.getSelectedItem()); setTextureButtonLabel(button); } }); @@ -120,6 +120,10 @@ public void getComponents(List list) { } + public void setTexture(TextureWithMetadata selectedItem) { + texture = selectedItem; + } + private JComponent createShininessSlider() { JPanel container = new JPanel(new BorderLayout()); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java index 3bf059f67..63f4d43aa 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java @@ -35,13 +35,6 @@ public void update(double dt) { } } - private HingeJoint addHinge() { - HingeJoint hinge = new HingeJoint("Motor Hinge"); - Node parent = getParent(); - if(parent!=null) parent.addChild(hinge); - return hinge; - } - @Override public void getComponents(List list) { JPanel pane = new JPanel(new GridLayout(0,2)); @@ -83,6 +76,10 @@ public HingeJoint getHinge() { return hinge.getSubject(); } + /** + * Set the hinge this motor will drive. the hinge must be in the same node tree as this motor. + * @param hinge the hinge this motor will drive. + */ public void setHinge(HingeJoint hinge) { this.hinge.setRelativePath(this, hinge); } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlanner.java b/src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlanner.java similarity index 79% rename from src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlanner.java rename to src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlanner.java index 2ce83d1e4..e30bd648b 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlanner.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlanner.java @@ -1,7 +1,8 @@ -package com.marginallyclever.ro3.node.nodes; +package com.marginallyclever.ro3.node.nodes.limbplanner; import com.marginallyclever.ro3.node.Node; import com.marginallyclever.ro3.node.NodePath; +import com.marginallyclever.ro3.node.nodes.Pose; import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; import org.json.JSONObject; import org.slf4j.Logger; @@ -13,12 +14,11 @@ import java.util.List; /** - * {@link TargetPlanner} knows about a {@link LimbSolver}. It moves the {@link LimbSolver#target} to a destination. - * It waits for the LimbSolver to reach the destination before moving on to the next destination (by subscribing to - * {@link LimbSolver} ActionEvent "arrivedAtGoal". + * {@link LimbPlanner} knows about a {@link LimbSolver}. It moves the {@link LimbSolver#target} to a destination. + * It then waits for the {@link ActionEvent} "arrivedAtGoal" before moving on to the next destination. */ -public class TargetPlanner extends Node implements ActionListener { - private static final Logger logger = LoggerFactory.getLogger(TargetPlanner.class); +public class LimbPlanner extends Node implements ActionListener { + private static final Logger logger = LoggerFactory.getLogger(LimbPlanner.class); private final NodePath solver = new NodePath<>(this, LimbSolver.class); private final NodePath pathStart = new NodePath<>(this, Node.class); private final NodePath nextGoal = new NodePath<>(this, Pose.class); // relative to pathStart @@ -26,21 +26,24 @@ public class TargetPlanner extends Node implements ActionListener { private double executionTime = 0; private double previousExecutionTime = 0; - public TargetPlanner() { - this("TargetPlanner"); + public LimbPlanner() { + this("LimbPlanner"); } - public TargetPlanner(String name) { + public LimbPlanner(String name) { super(name); } @Override public void getComponents(List list) { - list.add(new TargetPlannerPanel(this)); + list.add(new LimbPlannerPanel(this)); super.getComponents(list); } public void startRun() { + if(solver.getSubject()==null) throw new IllegalArgumentException("Solver is null."); + if(pathStart.getSubject()==null) throw new IllegalArgumentException("PathStart is null."); + logger.debug("Starting run"); previousExecutionTime = executionTime; executionTime = 0; @@ -130,7 +133,11 @@ private void setNextGoal(Pose pose) { } public void stopRun() { - logger.debug("Stopping run"); + if(!isRunning) { + logger.debug("Already stopped."); + return; + } + logger.debug("Stopping run at "+executionTime+" seconds."); isRunning=false; nextGoal.setPath(pathStart.getPath()); @@ -210,4 +217,28 @@ public NodePath getSolver() { public NodePath getNextGoal() { return nextGoal; } + + /** + * Set the solver to use. + * solver must be in the same node tree as this instance. + * @param limbSolver the solver to use. + */ + public void setSolver(LimbSolver limbSolver) { + solver.setRelativePath(this,limbSolver); + } + + /** + * Set the start of the path. + * pose must be in the same node tree as this instance. + * @param pose the pose to use. + */ + public void setPathStart(Pose pose) { + pathStart.setRelativePath(this,pose); + } + + public void setLinearVelocity(double v) { + if(solver.getSubject()!=null) { + solver.getSubject().setLinearVelocity(v); + } + } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlannerPanel.java b/src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerPanel.java similarity index 64% rename from src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlannerPanel.java rename to src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerPanel.java index e8e86e783..08f2d0c48 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/TargetPlannerPanel.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerPanel.java @@ -1,10 +1,10 @@ -package com.marginallyclever.ro3.node.nodes; +package com.marginallyclever.ro3.node.nodes.limbplanner; import com.marginallyclever.convenience.helpers.StringHelper; import com.marginallyclever.ro3.node.Node; import com.marginallyclever.ro3.node.NodePanelHelper; +import com.marginallyclever.ro3.node.nodes.Pose; import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; -import com.marginallyclever.ro3.view.View; import org.slf4j.Logger; import org.slf4j.LoggerFactory; @@ -15,22 +15,21 @@ import java.awt.event.HierarchyEvent; /** - *

{@link TargetPlannerPanel} displays controls to run a {@link TargetPlanner}.

+ *

{@link LimbPlannerPanel} displays controls to run a {@link LimbPlanner}.

*/ -@View(of=TargetPlanner.class) -public class TargetPlannerPanel extends JPanel implements ActionListener { - private static final Logger logger = LoggerFactory.getLogger(TargetPlannerPanel.class); - private final TargetPlanner targetPlanner; +public class LimbPlannerPanel extends JPanel implements ActionListener { + private static final Logger logger = LoggerFactory.getLogger(LimbPlannerPanel.class); + private final LimbPlanner limbPlanner; private final JToggleButton runButton = new JToggleButton(); private Timer timer; private final JLabel previousExecutionTimeLabel = new JLabel(); private final JLabel executionTimeLabel = new JLabel(); - public TargetPlannerPanel(TargetPlanner targetPlanner) { + public LimbPlannerPanel(LimbPlanner limbPlanner) { super(new GridBagLayout()); - this.targetPlanner = targetPlanner; + this.limbPlanner = limbPlanner; - this.setName(TargetPlanner.class.getSimpleName()); + this.setName(LimbPlanner.class.getSimpleName()); GridBagConstraints gbc = new GridBagConstraints(); gbc.weightx = 1.0; @@ -40,23 +39,23 @@ public TargetPlannerPanel(TargetPlanner targetPlanner) { gbc.gridy=0; gbc.gridwidth=1; - NodePanelHelper.addNodeSelector(this, "LimbSolver", targetPlanner.getSolver(), LimbSolver.class, gbc,targetPlanner); + NodePanelHelper.addNodeSelector(this, "LimbSolver", limbPlanner.getSolver(), LimbSolver.class, gbc, limbPlanner); gbc.gridy++; - NodePanelHelper.addNodeSelector(this, "Path start", targetPlanner.getPathStart(), Node.class, gbc,targetPlanner); + NodePanelHelper.addNodeSelector(this, "Path start", limbPlanner.getPathStart(), Node.class, gbc, limbPlanner); gbc.gridy++; - var selector = NodePanelHelper.addNodeSelector(this, "Next goal", targetPlanner.getNextGoal(), Pose.class, gbc,targetPlanner); + var selector = NodePanelHelper.addNodeSelector(this, "Next goal", limbPlanner.getNextGoal(), Pose.class, gbc, limbPlanner); selector.setEditable(false); // Run button gbc.gridy++; runButton.addActionListener(e -> { - boolean isRunning = targetPlanner.isRunning(); + boolean isRunning = limbPlanner.isRunning(); if (!isRunning) { - targetPlanner.startRun(); - previousExecutionTimeLabel.setText(StringHelper.formatTime(targetPlanner.getPreviousExecutionTime())); + limbPlanner.startRun(); + previousExecutionTimeLabel.setText(StringHelper.formatTime(limbPlanner.getPreviousExecutionTime())); setRunButtonText(); } else { - targetPlanner.stopRun(); + limbPlanner.stopRun(); setRunButtonText(); } }); @@ -74,16 +73,16 @@ public TargetPlannerPanel(TargetPlanner targetPlanner) { this.add(runButton,gbc); gbc.gridy++; gbc.gridwidth=1; - executionTimeLabel.setText( StringHelper.formatTime(targetPlanner.getExecutionTime()) ); + executionTimeLabel.setText( StringHelper.formatTime(limbPlanner.getExecutionTime()) ); NodePanelHelper.addLabelAndComponent(this, "Execution time", executionTimeLabel, gbc); gbc.gridy++; - previousExecutionTimeLabel.setText( StringHelper.formatTime(targetPlanner.getPreviousExecutionTime()) ); + previousExecutionTimeLabel.setText( StringHelper.formatTime(limbPlanner.getPreviousExecutionTime()) ); NodePanelHelper.addLabelAndComponent(this, "Previous time", previousExecutionTimeLabel, gbc); } private void setRunButtonText() { - runButton.setSelected(targetPlanner.isRunning()); - runButton.setText(targetPlanner.isRunning() ? "Stop" : "Run"); + runButton.setSelected(limbPlanner.isRunning()); + runButton.setText(limbPlanner.isRunning() ? "Stop" : "Run"); } @Override @@ -91,19 +90,19 @@ public void addNotify() { super.addNotify(); // timer timer = new Timer(100, (e)-> { - if (targetPlanner.isRunning()) { - executionTimeLabel.setText(StringHelper.formatTime(targetPlanner.getExecutionTime())); + if (limbPlanner.isRunning()) { + executionTimeLabel.setText(StringHelper.formatTime(limbPlanner.getExecutionTime())); } }); timer.start(); - targetPlanner.addActionListener(this); + limbPlanner.addActionListener(this); } @Override public void removeNotify() { super.removeNotify(); timer.stop(); - targetPlanner.removeActionListener(this); + limbPlanner.removeActionListener(this); } @Override diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolver.java b/src/main/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolver.java index 94137fa67..31fb9ec00 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolver.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolver.java @@ -48,6 +48,11 @@ public Pose getTarget() { return target.getSubject(); } + /** + * Set the target to move towards. + * target must be in the same node tree as this instance. + * @param target the target to move towards + */ public void setTarget(Pose target) { this.target.setRelativePath(this,target); } @@ -77,6 +82,11 @@ public Limb getLimb() { return limb.getSubject(); } + /** + * Set the limb to be controlled by this instance. + * limb must be in the same node tree as this instance. + * @param limb the limb to control + */ public void setLimb(Limb limb) { this.limb.setRelativePath(this,limb); } @@ -86,11 +96,18 @@ private Pose getEndEffector() { return limb!=null ? limb.getEndEffector() : null; } + /** + * @return true if the solver has a limb, an end effector, and a target. Does not guarantee that a solution exists. + */ + public boolean readyToSolve() { + return getLimb()!=null && getEndEffector()!=null && getTarget()!=null; + } + private void moveTowardsTarget() { - if(getLimb()==null || getEndEffector()==null || getTarget()==null ) { - // no limb, no end effector, or no target. Do nothing. + if(!readyToSolve()) { return; } + if(linearVelocity<0.0001) { // no velocity. Make sure the arm doesn't drift. getLimb().setAllJointVelocities(new double[getLimb().getNumJoints()]); diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java index 6872813a4..23028a3db 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArm.java @@ -259,11 +259,11 @@ private String getM114() { return "M114"+getMotorsAndFeedrateAsString(); } - private Limb getLimb() { + public Limb getLimb() { return limb.getSubject(); } - private LimbSolver getSolver() { + public LimbSolver getSolver() { return solver.getSubject(); } @@ -483,4 +483,22 @@ private void fireMarlinMessage(String message) { listener.messageFromMarlin(message); } } + + /** + * Set the limb to be controlled by this instance. + * limb must be in the same node tree as this instance. + * @param limb the limb to control + */ + public void setLimb(Limb limb) { + this.limb.setRelativePath(this,limb); + } + + /** + * Set the solver to be used by this instance. + * solver must be in the same node tree as this instance. + * @param solver the solver to use + */ + public void setSolver(LimbSolver solver) { + this.solver.setRelativePath(this, solver); + } } diff --git a/src/main/java/com/marginallyclever/ro3/view/ViewFinder.java b/src/main/java/com/marginallyclever/ro3/view/ViewFinder.java index f3079b776..369bf9836 100644 --- a/src/main/java/com/marginallyclever/ro3/view/ViewFinder.java +++ b/src/main/java/com/marginallyclever/ro3/view/ViewFinder.java @@ -8,11 +8,16 @@ import java.util.stream.Collectors; /** - * Finds all classes annotated with {@link View} that are for the given target class. + * Finds all classes annotated with {@link View} that are 'of' the given target class. */ public class ViewFinder { + /** + * Finds all @View(of=targetClass) classes. + * @param targetClass the class to find views for + * @return a set of view providers + */ public static Set> findViews(Class targetClass) { - Reflections reflections = new Reflections("com.marginallyclever", Scanners.TypesAnnotated); + Reflections reflections = new Reflections("com.marginallyclever.ro3", Scanners.TypesAnnotated); Set> annotatedClasses = reflections.getTypesAnnotatedWith(View.class); return annotatedClasses.stream() diff --git a/src/main/resources/com/marginallyclever/ro3/apps/viewport/renderpasses/mesh.frag b/src/main/resources/com/marginallyclever/ro3/apps/viewport/renderpasses/mesh.frag index 547d24068..35438294b 100644 --- a/src/main/resources/com/marginallyclever/ro3/apps/viewport/renderpasses/mesh.frag +++ b/src/main/resources/com/marginallyclever/ro3/apps/viewport/renderpasses/mesh.frag @@ -70,7 +70,8 @@ void main() { // Specular vec3 viewDir = normalize(cameraPos - fs_in.fragmentPosition); vec3 reflectDir = reflect(-lightDir, norm); - float spec = pow(max(dot(viewDir, reflectDir), 0.0), shininess); + float spec = 1.0; + if( shininess > 1.0 ) spec = pow(max(dot(viewDir, reflectDir), 0.0), shininess); vec4 specularLight = spec * specularColor * lightColor; // Shadow diff --git a/src/test/java/com/marginallyclever/misc/FontTest.java b/src/test/java/com/marginallyclever/misc/FontTest.java index 9e53bd6d9..c462053f8 100644 --- a/src/test/java/com/marginallyclever/misc/FontTest.java +++ b/src/test/java/com/marginallyclever/misc/FontTest.java @@ -10,7 +10,7 @@ import java.awt.*; import java.util.Arrays; -@DisabledIfEnvironmentVariable(named = "CI", matches = "true") +@DisabledIfEnvironmentVariable(named = "CI", matches = "true", disabledReason = "headless environment") public class FontTest { private static final Logger logger = LoggerFactory.getLogger(FontTest.class); diff --git a/src/test/java/com/marginallyclever/ro3/node/NodeListenerTest.java b/src/test/java/com/marginallyclever/ro3/node/NodeListenerTest.java new file mode 100644 index 000000000..fded1308f --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/NodeListenerTest.java @@ -0,0 +1,55 @@ +package com.marginallyclever.ro3.node; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.mockito.Mockito; + +import static org.mockito.Mockito.*; + +public class NodeListenerTest { + private Node parentNode; + private Node childNode; + private NodeAttachListener attachListener; + private NodeDetachListener detachListener; + private NodeReadyListener readyListener; + private NodeRenameListener renameListener; + + @BeforeEach + public void setup() { + parentNode = new Node("Parent"); + childNode = new Node("Child"); + attachListener = Mockito.mock(NodeAttachListener.class); + detachListener = Mockito.mock(NodeDetachListener.class); + readyListener = Mockito.mock(NodeReadyListener.class); + renameListener = Mockito.mock(NodeRenameListener.class); + + parentNode.addAttachListener(attachListener); + parentNode.addDetachListener(detachListener); + parentNode.addReadyListener(readyListener); + parentNode.addRenameListener(renameListener); + } + + @Test + public void testNodeAttachListener() { + parentNode.addChild(childNode); + verify(attachListener, times(1)).nodeAttached(childNode); + } + + @Test + public void testNodeDetachListener() { + parentNode.addChild(childNode); + parentNode.removeChild(childNode); + verify(detachListener, times(1)).nodeDetached(childNode); + } + + @Test + public void testNodeReadyListener() { + parentNode.addChild(childNode); + verify(readyListener, times(1)).nodeReady(childNode); + } + + @Test + public void testNodeRenameListener() { + parentNode.setName("NewName"); + verify(renameListener, times(1)).nodeRenamed(parentNode); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/NodePathTest.java b/src/test/java/com/marginallyclever/ro3/node/NodePathTest.java new file mode 100644 index 000000000..6aefbfed1 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/NodePathTest.java @@ -0,0 +1,45 @@ +package com.marginallyclever.ro3.node; +import com.marginallyclever.convenience.PathCalculator; +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class NodePathTest { + @Test + void testConstructor() { + Node owner = new Node(); + NodePath nodePath = new NodePath<>(owner, Node.class); + assertNotNull(nodePath); + } + + @Test + void testGetPath() { + Node owner = new Node(); + NodePath nodePath = new NodePath<>(owner, Node.class, "/path/to/node"); + assertEquals("/path/to/node", nodePath.getPath()); + } + + @Test + void testSetPath() { + Node owner = new Node(); + NodePath nodePath = new NodePath<>(owner, Node.class); + nodePath.setPath("/new/path/to/node"); + assertEquals("/new/path/to/node", nodePath.getPath()); + } + + @Test + void testGetSubject() { + Node owner = new Node(); + NodePath nodePath = new NodePath<>(owner, Node.class, "/path/to/node"); + assertEquals(owner.findNodeByPath("/path/to/node", Node.class), nodePath.getSubject()); + } + + @Test + void testSetRelativePath() { + Node owner = new Node(); + Node goal = new Node(); + owner.addChild(goal); + NodePath nodePath = new NodePath<>(owner, Node.class); + nodePath.setRelativePath(owner, goal); + assertEquals(PathCalculator.getRelativePath(owner, goal), nodePath.getPath()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/NodeTest.java b/src/test/java/com/marginallyclever/ro3/node/NodeTest.java new file mode 100644 index 000000000..49a2f1e5c --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/NodeTest.java @@ -0,0 +1,142 @@ +package com.marginallyclever.ro3.node; + +import com.marginallyclever.ro3.node.nodes.Pose; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import java.util.UUID; + +import static org.junit.jupiter.api.Assertions.*; + +public class NodeTest { + private Node parentNode; + private Node childNode; + + @BeforeEach + public void setup() { + parentNode = new Node("Parent"); + childNode = new Node("Child"); + } + + @Test + public void testAddChild() { + parentNode.addChild(childNode); + assertTrue(parentNode.getChildren().contains(childNode)); + assertEquals(parentNode, childNode.getParent()); + } + + @Test + public void testRemoveChild() { + parentNode.addChild(childNode); + parentNode.removeChild(childNode); + assertFalse(parentNode.getChildren().contains(childNode)); + assertNull(childNode.getParent()); + } + + @Test + public void testGetName() { + assertEquals("Parent", parentNode.getName()); + } + + @Test + public void testGetParent() { + parentNode.addChild(childNode); + assertEquals(parentNode, childNode.getParent()); + } + + @Test + public void testGetNodeID() { + UUID id = parentNode.getNodeID(); + assertNotNull(id); + } + + @Test + public void testSetName() { + parentNode.setName("NewName"); + assertEquals("NewName", parentNode.getName()); + } + + @Test + public void testGetChildren() { + parentNode.addChild(childNode); + assertEquals(1, parentNode.getChildren().size()); + assertEquals(childNode, parentNode.getChildren().get(0)); + } + + @Test + public void testFindParent() { + Node grandParentNode = new Node("GrandParent"); + grandParentNode.addChild(parentNode); + parentNode.addChild(childNode); + assertEquals(grandParentNode, childNode.findParent("GrandParent")); + } + + @Test + public void testFindChild() { + parentNode.addChild(childNode); + assertEquals(childNode, parentNode.findChild("Child")); + } + + @Test + public void testGet() { + parentNode.addChild(childNode); + assertEquals(childNode, parentNode.get("Child")); + } + + @Test + public void testGetRootNode() { + Node grandParentNode = new Node("GrandParent"); + grandParentNode.addChild(parentNode); + parentNode.addChild(childNode); + assertEquals(grandParentNode, childNode.getRootNode()); + } + + @Test + public void testGetAbsolutePath() { + Node grandParentNode = new Node("GrandParent"); + grandParentNode.addChild(parentNode); + parentNode.addChild(childNode); + assertEquals("/GrandParent/Parent/Child", childNode.getAbsolutePath()); + } + + @Test + public void testIsNameUsedBySibling() { + parentNode.addChild(childNode); + Node siblingNode = new Node("Child"); + parentNode.addChild(siblingNode); + assertTrue(siblingNode.isNameUsedBySibling("Child")); + } + + @Test + public void testFindFirstChild() { + parentNode.addChild(childNode); + assertEquals(childNode, parentNode.findFirstChild(Node.class)); + } + + @Test + public void testFindFirstSibling() { + Pose siblingNode = new Pose("Sibling"); + parentNode.addChild(childNode); + parentNode.addChild(siblingNode); + assertEquals(siblingNode, childNode.findFirstSibling(Pose.class)); + } + + @Test + public void testHasParent() { + parentNode.addChild(childNode); + assertTrue(childNode.hasParent(parentNode)); + } + + @Test + public void testFindNodeByID() { + parentNode.addChild(childNode); + String id = childNode.getNodeID().toString(); + assertEquals(childNode, parentNode.findNodeByID(id, Node.class)); + } + + @Test + public void testFindNodeByPath() { + parentNode.addChild(childNode); + assertEquals(childNode, parentNode.findNodeByPath("Child", Node.class)); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/TestCamera.java b/src/test/java/com/marginallyclever/ro3/node/nodes/CameraTest.java similarity index 80% rename from src/test/java/com/marginallyclever/ro3/node/nodes/TestCamera.java rename to src/test/java/com/marginallyclever/ro3/node/nodes/CameraTest.java index 6a7f7171f..75332973b 100644 --- a/src/test/java/com/marginallyclever/ro3/node/nodes/TestCamera.java +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/CameraTest.java @@ -9,8 +9,7 @@ import javax.vecmath.Matrix4d; import javax.vecmath.Vector3d; -@DisabledIfEnvironmentVariable(named = "CI", matches = "true", disabledReason = "headless environment") -public class TestCamera { +public class CameraTest { @Test public void testPanTiltInverses() { Registry.start(); @@ -22,8 +21,7 @@ public void testPanTiltInverses() { Matrix3d panTiltMatrix = camera.buildPanTiltMatrix(before); Matrix4d matrix = new Matrix4d(panTiltMatrix, new Vector3d(), 1.0); double[] after = camera.getPanTiltFromMatrix(matrix); - - System.out.println("before="+before[0]+","+before[1]+" after="+after[0]+","+after[1]); + //System.out.println("before="+before[0]+","+before[1]+" after="+after[0]+","+after[1]); Assertions.assertArrayEquals(before, after, 0.01); } } diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/DHParameterTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/DHParameterTest.java new file mode 100644 index 000000000..45e2f6b67 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/DHParameterTest.java @@ -0,0 +1,33 @@ +package com.marginallyclever.ro3.node.nodes; +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class DHParameterTest { + @Test + void testGetD() { + DHParameter dhParameter = new DHParameter(); + dhParameter.setD(10.0); + assertEquals(10.0, dhParameter.getD()); + } + + @Test + void testGetR() { + DHParameter dhParameter = new DHParameter(); + dhParameter.setR(20.0); + assertEquals(20.0, dhParameter.getR()); + } + + @Test + void testGetAlpha() { + DHParameter dhParameter = new DHParameter(); + dhParameter.setAlpha(30.0); + assertEquals(30.0, dhParameter.getAlpha()); + } + + @Test + void testGetTheta() { + DHParameter dhParameter = new DHParameter(); + dhParameter.setTheta(40.0); + assertEquals(40.0, dhParameter.getTheta()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/HingeJointTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/HingeJointTest.java new file mode 100644 index 000000000..a79afe3b1 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/HingeJointTest.java @@ -0,0 +1,44 @@ +package com.marginallyclever.ro3.node.nodes; +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class HingeJointTest { + @Test + void testGetSetAngle() { + HingeJoint hingeJoint = new HingeJoint(); + hingeJoint.setAngle(45.0); + assertEquals(45.0, hingeJoint.getAngle()); + } + + @Test + void testGetMinAngle() { + HingeJoint hingeJoint = new HingeJoint(); + assertEquals(0.0, hingeJoint.getMinAngle()); + } + + @Test + void testGetMaxAngle() { + HingeJoint hingeJoint = new HingeJoint(); + assertEquals(360.0, hingeJoint.getMaxAngle()); + } + + @Test + void testGetSetVelocity() { + HingeJoint hingeJoint = new HingeJoint(); + hingeJoint.setVelocity(10.0); + assertEquals(10.0, hingeJoint.getVelocity()); + } + + @Test + void testGetSetAcceleration() { + HingeJoint hingeJoint = new HingeJoint(); + hingeJoint.setAcceleration(20.0); + assertEquals(20.0, hingeJoint.getAcceleration()); + } + + @Test + void testGetAxle() { + HingeJoint hingeJoint = new HingeJoint(); + assertNull(hingeJoint.getAxle()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/MaterialTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/MaterialTest.java new file mode 100644 index 000000000..53ab72f4c --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/MaterialTest.java @@ -0,0 +1,61 @@ +package com.marginallyclever.ro3.node.nodes; + +import com.marginallyclever.ro3.Registry; +import com.marginallyclever.ro3.texture.TextureWithMetadata; +import org.junit.jupiter.api.Test; +import java.awt.Color; +import java.awt.image.BufferedImage; + +import static org.junit.jupiter.api.Assertions.*; + + +class MaterialTest { + @Test + void testGetTexture() { + Material material = new Material(); + TextureWithMetadata texture = new TextureWithMetadata(new BufferedImage(1,1,BufferedImage.TYPE_INT_RGB), "test"); + material.setTexture(texture); + assertEquals(texture, material.getTexture()); + } + + @Test + void testGetSetDiffuseColor() { + Material material = new Material(); + Color color = new Color(255, 0, 0); + material.setDiffuseColor(color); + assertEquals(color, material.getDiffuseColor()); + } + + @Test + void testGetSetSpecularColor() { + Material material = new Material(); + Color color = new Color(0, 255, 0); + material.setSpecularColor(color); + assertEquals(color, material.getSpecularColor()); + } + + @Test + void testGetSetEmissionColor() { + Material material = new Material(); + Color color = new Color(0, 0, 255); + material.setEmissionColor(color); + assertEquals(color, material.getEmissionColor()); + } + + @Test + void testGetSetShininess() { + Material material = new Material(); + int shininess = 50; + material.setShininess(shininess); + assertEquals(shininess, material.getShininess()); + } + + @Test + void testIsSetLit() { + Material material = new Material(); + material.setLit(true); + assertTrue(material.isLit()); + material.setLit(false); + assertFalse(material.isLit()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/MotorTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/MotorTest.java new file mode 100644 index 000000000..51edbebf7 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/MotorTest.java @@ -0,0 +1,32 @@ +package com.marginallyclever.ro3.node.nodes; + +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class MotorTest { + @Test + void testUpdate() { + Motor motor = new Motor(); + motor.update(1.0); + // Add assertions based on the expected state of the motor after calling update + } + + @Test + void testGetSetHinge() { + Motor motor = new Motor(); + HingeJoint hingeJoint = new HingeJoint(); + motor.addChild(hingeJoint); + motor.setHinge(hingeJoint); + assertEquals(hingeJoint, motor.getHinge()); + } + + @Test + void testHasHinge() { + Motor motor = new Motor(); + assertFalse(motor.hasHinge()); + HingeJoint hingeJoint = new HingeJoint(); + motor.addChild(hingeJoint); + motor.setHinge(hingeJoint); + assertTrue(motor.hasHinge()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/PoseTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/PoseTest.java new file mode 100644 index 000000000..7f3d00b33 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/PoseTest.java @@ -0,0 +1,67 @@ +package com.marginallyclever.ro3.node.nodes; + +import com.marginallyclever.convenience.helpers.MatrixHelper; +import org.junit.jupiter.api.Assertions; +import org.junit.jupiter.api.Test; +import javax.vecmath.Matrix4d; +import javax.vecmath.Vector3d; +import static org.junit.jupiter.api.Assertions.*; + +class PoseTest { + @Test + void testGetSetLocal() { + Pose pose = new Pose(); + Matrix4d matrix = MatrixHelper.createIdentityMatrix4(); + pose.setLocal(matrix); + assertEquals(matrix, pose.getLocal()); + } + + @Test + void testGetSetWorld1() { + Pose a = new Pose(); + Pose b = new Pose(); + a.addChild(b); + Matrix4d matrix = MatrixHelper.createIdentityMatrix4(); + matrix.setTranslation(new Vector3d(1,2,3)); + a.setLocal(matrix); + b.setLocal(matrix); + matrix.setTranslation(new Vector3d(2,4,6)); + assertEquals(matrix, b.getWorld()); + } + + @Test + void testGetSetWorld2() { + Pose a = new Pose(); + Pose b = new Pose(); + a.addChild(b); + Matrix4d matrix = MatrixHelper.createIdentityMatrix4(); + matrix.setTranslation(new Vector3d(1,2,3)); + a.setLocal(matrix); + b.setWorld(matrix); + assertEquals(matrix, b.getWorld()); + matrix.setTranslation(new Vector3d(0,0,0)); + assertEquals(matrix, b.getLocal()); + } + + @Test + void testGetSetRotationEuler() { + Pose pose = new Pose(); + Vector3d vector = new Vector3d(1.0, 2.0, 3.0); + pose.setRotationEuler(vector, MatrixHelper.EulerSequence.YXZ); + var result = pose.getRotationEuler(MatrixHelper.EulerSequence.YXZ); + Assertions.assertEquals(vector.x, result.x,1e-4); + Assertions.assertEquals(vector.y, result.y,1e-4); + Assertions.assertEquals(vector.z, result.z,1e-4); + } + + @Test + void testGetSetPosition() { + Pose pose = new Pose(); + Vector3d vector = new Vector3d(1.0, 2.0, 3.0); + pose.setPosition(vector); + var result = pose.getPosition(); + Assertions.assertEquals(vector.x, result.x,1e-4); + Assertions.assertEquals(vector.y, result.y,1e-4); + Assertions.assertEquals(vector.z, result.z,1e-4); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerTest.java new file mode 100644 index 000000000..b8546d6ea --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/limbplanner/LimbPlannerTest.java @@ -0,0 +1,123 @@ +package com.marginallyclever.ro3.node.nodes.limbplanner; + +import com.marginallyclever.convenience.helpers.StringHelper; +import com.marginallyclever.ro3.Registry; +import com.marginallyclever.ro3.apps.actions.LoadScene; +import com.marginallyclever.ro3.apps.commands.ImportScene; +import com.marginallyclever.ro3.node.nodes.Pose; +import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; +import com.marginallyclever.ro3.node.nodes.pose.Limb; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; +import org.junit.jupiter.api.condition.DisabledIfEnvironmentVariable; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import javax.vecmath.Matrix4d; +import javax.vecmath.Vector3d; +import java.awt.event.ActionEvent; +import java.io.File; +import java.util.Arrays; + +import static org.junit.jupiter.api.Assertions.*; + +class LimbPlannerTest { + private static final Logger logger = LoggerFactory.getLogger(LimbPlannerTest.class); + Limb limb; + LimbSolver limbSolver; + LimbPlanner limbPlanner; + Pose pathStart; + + private Limb build6AxisArm() throws Exception { + var load = new LoadScene(null,null); + File file = new File("src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO"); + load.commitLoad(file); + return (Limb) Registry.getScene().get("./Sixi3"); + } + + @BeforeEach + void setUp() throws Exception { + try { + Registry.start(); + } catch(Exception e) { + logger.error("Failed to start Registry",e); + } + limb = build6AxisArm(); + + // the Sixi3-5.RO file has a limb named "Sixi3" which has a LimbSolver. + + limbSolver = limb.findFirstChild(LimbSolver.class); + + limbPlanner = new LimbPlanner(); + limb.addChild(limbPlanner); + limbPlanner.setSolver(limbSolver); + + pathStart = new Pose(); + limbPlanner.addChild(pathStart); + limbPlanner.setPathStart(pathStart); + limb.addChild(pathStart); + } + + @Test + void testStartRun1() { + limbPlanner.startRun(); + assertFalse(limbPlanner.isRunning()); + } + + @Test + void testStartRun2() { + pathStart.addChild(new Pose()); + limbPlanner.startRun(); + assertTrue(limbPlanner.isRunning()); + } + + @Test + void testStopRun() { + limbPlanner.startRun(); + limbPlanner.stopRun(); + assertFalse(limbPlanner.isRunning()); + } + + @Test + void testGetExecutionTimeAndPreviousExecutionTime() { + // start at world origin + pathStart.addChild(new Pose("start")); + + // end 1 unit in the X direction away + var end = new Pose("end"); + var endPosition = new Matrix4d(); + endPosition.setTranslation(new Vector3d(10, 0, 0)); + end.setLocal(endPosition); + pathStart.addChild(end); + + // turn on the planner + limbPlanner.setLinearVelocity(1.0); + limbPlanner.startRun(); + // The Sixi3-5 arm is perfectly vertical, it cannot reach the start position. + // it is guaranteed to run for 1 second. + double dt = 0.1; + double sum=0; + for(int i=0;i<10;++i) { + sum+=dt; + logger.debug(StringHelper.formatTime(sum)+" Move "+Arrays.toString(limb.getAllJointAngles())); + limb.update(dt); + if(!limbPlanner.isRunning()) break; + } + if(limbPlanner.isRunning()) { + limbPlanner.stopRun(); + } + logger.debug(StringHelper.formatTime(sum)+" End "+Arrays.toString(limb.getAllJointAngles())); + + // confirm we moved for 1 second. + assertEquals(1.0, limbPlanner.getExecutionTime(),1e-4); + assertEquals(0.0, limbPlanner.getPreviousExecutionTime()); + } + + @Test + void testActionPerformed() { + limbPlanner.startRun(); + ActionEvent event = new ActionEvent(limbPlanner, ActionEvent.ACTION_PERFORMED, "arrivedAtGoal"); + limbPlanner.actionPerformed(event); + assertFalse(limbPlanner.isRunning()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolverTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolverTest.java new file mode 100644 index 000000000..5b465bc76 --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/limbsolver/LimbSolverTest.java @@ -0,0 +1,53 @@ +package com.marginallyclever.ro3.node.nodes.limbsolver; + +import com.marginallyclever.ro3.node.nodes.Pose; +import com.marginallyclever.ro3.node.nodes.pose.Limb; +import org.junit.jupiter.api.Test; +import static org.junit.jupiter.api.Assertions.*; + +class LimbSolverTest { + @Test + void testGetSetTarget() { + LimbSolver limbSolver = new LimbSolver(); + Pose pose = new Pose(); + limbSolver.addChild(pose); + limbSolver.setTarget(pose); + assertEquals(pose, limbSolver.getTarget()); + } + + @Test + void testUpdate() { + LimbSolver limbSolver = new LimbSolver(); + limbSolver.update(1.0); + // Add assertions based on the expected state of the limbSolver after calling update + } + + @Test + void testGetSetLimb() { + LimbSolver limbSolver = new LimbSolver(); + Limb limb = new Limb(); + limbSolver.addChild(limb); + limbSolver.setLimb(limb); + assertEquals(limb, limbSolver.getLimb()); + } + + @Test + void testGetSetLinearVelocity() { + LimbSolver limbSolver = new LimbSolver(); + limbSolver.setLinearVelocity(10.0); + assertEquals(10.0, limbSolver.getLinearVelocity()); + } + + @Test + void testGetDistanceToTarget() { + LimbSolver limbSolver = new LimbSolver(); + // Add assertions based on the expected distance to target + } + + @Test + void testGetSetGoalMarginOfError() { + LimbSolver limbSolver = new LimbSolver(); + limbSolver.setGoalMarginOfError(0.1); + assertEquals(0.1, limbSolver.getGoalMarginOfError()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianTest.java index f4eef49a9..d2dc68741 100644 --- a/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianTest.java +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianTest.java @@ -22,9 +22,7 @@ public void setup() { } private Limb build6AxisArm() throws Exception { - // TODO load a robot from a file. var load = new LoadScene(null,null); - // find file {project root}/src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO File file = new File("src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO"); load.commitLoad(file); return (Limb) Registry.getScene().get("./Sixi3/MarlinRobotArm"); diff --git a/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArmTest.java b/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArmTest.java new file mode 100644 index 000000000..cd58183cb --- /dev/null +++ b/src/test/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/MarlinRobotArmTest.java @@ -0,0 +1,49 @@ +package com.marginallyclever.ro3.node.nodes.marlinrobotarm; + +import com.marginallyclever.ro3.node.nodes.Pose; +import com.marginallyclever.ro3.node.nodes.pose.Limb; +import com.marginallyclever.ro3.node.nodes.limbsolver.LimbSolver; +import org.junit.jupiter.api.Test; +import org.mockito.Mockito; + +import static org.junit.jupiter.api.Assertions.*; + +class MarlinRobotArmTest { + @Test + void testGetTarget() { + Limb limb = new Limb(); + LimbSolver limbSolver = new LimbSolver(); + MarlinRobotArm marlinRobotArm = new MarlinRobotArm(); + Pose target = new Pose(); + limb.addChild(marlinRobotArm); + limb.addChild(limbSolver); + limb.addChild(target); + limbSolver.setTarget(target); + marlinRobotArm.setSolver(limbSolver); + assertEquals(target, marlinRobotArm.getTarget()); + } + + @Test + void testSendGCode() { + MarlinRobotArm marlinRobotArm = new MarlinRobotArm(); + MarlinListener mockListener = Mockito.mock(MarlinListener.class); + marlinRobotArm.addMarlinListener(mockListener); + marlinRobotArm.sendGCode("M114"); + + Mockito.verify(mockListener).messageFromMarlin(Mockito.startsWith("Ok: M114")); + + } + + @Test + void testSetLimbAndSolver() { + Limb limb = new Limb(); + MarlinRobotArm marlinRobotArm = new MarlinRobotArm(); + LimbSolver limbSolver = new LimbSolver(); + limb.addChild(marlinRobotArm); + limb.addChild(limbSolver); + marlinRobotArm.setLimb(limb); + marlinRobotArm.setSolver(limbSolver); + assertEquals(limb, marlinRobotArm.getLimb()); + assertEquals(limbSolver, marlinRobotArm.getSolver()); + } +} \ No newline at end of file diff --git a/src/test/java/com/marginallyclever/ro3/view/ViewProviderTest.java b/src/test/java/com/marginallyclever/ro3/view/ViewProviderTest.java index 80f56364b..2aaef24f2 100644 --- a/src/test/java/com/marginallyclever/ro3/view/ViewProviderTest.java +++ b/src/test/java/com/marginallyclever/ro3/view/ViewProviderTest.java @@ -1,6 +1,7 @@ package com.marginallyclever.ro3.view; import org.junit.Test; +import org.junit.jupiter.api.Assertions; import org.reflections.Reflections; import org.reflections.scanners.Scanners; import org.slf4j.Logger; @@ -20,14 +21,14 @@ public void testViewProviders() { for (Class cls : getAllClasses()) { count++; logger.info("View {}",cls.getSimpleName()); - assert !ViewProvider.class.isAssignableFrom(cls) : - "Class " + cls.getName() + " must implement ViewProvider"; + Assertions.assertTrue(ViewProvider.class.isAssignableFrom(cls), + "Class " + cls.getName() + " must implement ViewProvider"); } logger.info("Found {} views.",count); } private Set> getAllClasses() { - Reflections reflections = new Reflections("com.marginallyclever", Scanners.TypesAnnotated); + Reflections reflections = new Reflections("com.marginallyclever.ro3", Scanners.TypesAnnotated); return reflections.getTypesAnnotatedWith(View.class); } } diff --git a/src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO b/src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO index 83e4317c9..25f7a0309 100644 --- a/src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO +++ b/src/test/resources/com/marginallyclever/ro3/apps/node/nodes/marlinrobotarm/Sixi3-5.RO @@ -1 +1 @@ -{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"acceleration":0,"children":[{"children":[{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"dfdf791e-3d49-425e-b716-4905aaa8f91a","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"right arm.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-65536,"nodeID":"292604b1-8511-4df4-90c7-30476af428fd"}],"name":"RightJaw","type":"Pose","nodeID":"b2f9740d-d883-4a6e-b982-e09db1f82e88","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]}],"maxAngle":0,"name":"GripperServo","angle":0,"minAngle":-90,"velocity":0,"type":"HingeJoint","nodeID":"8fa21884-a806-472f-acdb-8363580129f1","axle":"b2f9740d-d883-4a6e-b982-e09db1f82e88"}],"name":"offset","type":"Pose","nodeID":"2440972a-52ea-4f2a-8c58-43d98e834211","local":[1,0,0,0.015,0,0.7071067811865476,0.7071067811865475,-1.405,0,-0.7071067811865475,0.7071067811865476,3.3256,0,0,0,1]},{"children":[{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"0b7c9c49-af85-4205-8ad6-959274bbfe8f","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"left arm.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-26317,"nodeID":"ecd23f91-3d0d-45fb-a96d-b79fe8268d7c"}],"name":"FixedLeftJaw","type":"Pose","nodeID":"10f67c7f-e71e-47df-87f2-67e6be91c2dc","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"Jaw target","type":"Pose","nodeID":"ad7e8c0c-2a8d-40e2-9742-d665fd3f6cd0","local":[1,0,0,0.63,0,1,0,-3.9147,0,0,1,6.4288,0,0,0,1]}],"name":"ScrewGripper","type":"Pose","nodeID":"6511b56b-b426-4fc1-b439-6c643fe1b60b","local":[6.123233995736766E-17,1,0,0,-1,6.123233995736766E-17,0,0,0,0,1,0,0,0,0,1]}],"name":"End Effector","type":"Pose","nodeID":"72cd0146-0aff-4544-8730-27948a80fce2","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"8449f6a3-4168-4b82-ba40-96ce7549749e","local":[-1,-7.498798913309288E-33,1.2246467991473532E-16,-2.838976209783394E-15,7.498798913309288E-33,-1,-9.18338008663387E-49,9.129999999999999,1.2246467991473532E-16,9.18338008663387E-49,1,-43.464,0,0,0,1],"mesh":"j5.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"5b40d155-723f-438d-bfb4-ffafe1ef3c3d"}],"name":"j5","type":"Pose","nodeID":"d4ca58f5-d1d4-4a37-88a0-d72ce120779e","local":[1,0,0,0,0,1,0,0,0,0,1,5.1,0,0,0,1]},{"r":0,"d":5.1,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"cf4ad47b-4526-48e5-8fce-adc1aad27823","theta":0}],"name":"a5","type":"Pose","nodeID":"9a56a94a-bfb2-4eba-a694-5464e862fed9","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"3bebfb52-6fb5-4c72-ad6d-a15b22e559c6","local":[-1,-7.498798913309288E-33,1.2246467991473532E-16,-2.8389762097833943E-15,7.498798913309288E-33,-1,-9.18338008663387E-49,9.129999999999999,1.2246467991473532E-16,9.18338008663387E-49,1,-38.364,0,0,0,1],"mesh":"j4.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"5a189f00-14e9-4dde-84cc-4a96c3b31cf6"},{"acceleration":0,"children":[],"maxAngle":360,"name":"HingeJoint5","angle":0,"minAngle":0,"velocity":0,"type":"HingeJoint","nodeID":"b6190267-1136-4c80-9b01-cb70b1b2a05b","axle":"9a56a94a-bfb2-4eba-a694-5464e862fed9"}],"name":"j4","type":"Pose","nodeID":"07fa214e-5cfe-4f18-aa93-eb05ae65f8d5","local":[6.123233995736766E-17,6.123233995736766E-17,1,0,-1,3.749399456654644E-33,6.123233995736766E-17,-0,0,-1,6.123233995736766E-17,0,0,0,0,1]},{"r":0,"d":0,"children":[],"alpha":-90,"name":"DH Parameter","type":"DHParameter","nodeID":"dcf481c0-330c-46d3-a75a-706833d7be38","theta":-90}],"name":"a4","type":"Pose","nodeID":"cec604c9-8f09-4bb0-afe1-65ce3a0ebec3","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"32a4cdc8-82dc-4d4d-8082-0ae589879e89","local":[6.123233995736766E-17,-6.123233995736766E-17,1,-38.364,1,3.749399456654644E-33,-6.123233995736766E-17,4.898587196589414E-16,0,1,6.123233995736766E-17,-9.13,0,0,0,1],"mesh":"j3.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"ee8b6ae6-a9da-4839-9721-404b10b18fc8"},{"acceleration":0,"children":[],"maxAngle":440,"name":"HingeJoint4","angle":0,"minAngle":100,"velocity":0,"type":"HingeJoint","nodeID":"0b11bdde-6f32-44dd-ba30-fe83ada876cf","axle":"cec604c9-8f09-4bb0-afe1-65ce3a0ebec3"}],"name":"j3","type":"Pose","nodeID":"f2805245-dce5-4c20-8c04-d01564a149ef","local":[1,0,0,12.455,0,1,0,0,0,0,1,0,0,0,0,1]},{"r":12.455,"d":0,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"34b4c6d5-b5f2-43cf-bfaf-8480d54486d8","theta":0}],"name":"a3","type":"Pose","nodeID":"ab22a713-9f61-46e8-bb73-5966559b4dff","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"663c1042-df69-4cdf-8cdc-6296d4a9c43d","local":[6.123233995736766E-17,-6.123233995736766E-17,1,-25.909,1,3.749399456654644E-33,-6.123233995736766E-17,4.898587196589414E-16,0,1,6.123233995736766E-17,-9.13,0,0,0,1],"mesh":"j2.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"c97e741a-4cc0-400c-9d47-a5ef40bbc74e"},{"acceleration":0,"children":[],"maxAngle":150,"name":"HingeJoint3","angle":0,"minAngle":-150,"velocity":0,"type":"HingeJoint","nodeID":"f05afef6-65fc-4ec8-9109-049686b6b363","axle":"ab22a713-9f61-46e8-bb73-5966559b4dff"}],"name":"j2","type":"Pose","nodeID":"47978d1e-34fa-4283-a4c2-74dbf35b7ffc","local":[6.123233995736766E-17,1,0,1.0966099762964973E-15,-1,6.123233995736766E-17,0,-17.909,0,0,1,9.13,0,0,0,1]},{"r":17.909,"d":9.13,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"d153fcfe-9d51-47d7-9eb4-b87dcee75a5d","theta":-90}],"name":"a2","type":"Pose","nodeID":"9278c3c4-3a07-4cf0-881c-6680d9c4a470","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"814781b4-f050-48ca-981b-4e6e5d5a3711","local":[1,0,0,0,-0,6.123233995736766E-17,-1,8,0,1,6.123233995736766E-17,-4.898587196589413E-16,0,0,0,1],"mesh":"j1.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"cfc200e6-bd64-45c1-92e0-39daa3dce42d"},{"acceleration":0,"children":[],"maxAngle":90,"name":"HingeJoint2","angle":0,"minAngle":-90,"velocity":0,"type":"HingeJoint","nodeID":"a205abb3-9a41-44e2-bd90-c1ce2e23e3f5","axle":"9278c3c4-3a07-4cf0-881c-6680d9c4a470"}],"name":"j1","type":"Pose","nodeID":"83895d18-83dd-4b3b-bf27-d3830f63d8e4","local":[1,0,0,0,0,6.123233995736766E-17,1,0,0,-1,6.123233995736766E-17,8,0,0,0,1]},{"r":0,"d":8,"children":[],"alpha":-90,"name":"DH Parameter","type":"DHParameter","nodeID":"8b9d9beb-6eb9-409a-b2d5-9dc633761da7","theta":0}],"name":"a1","type":"Pose","nodeID":"fa0ac4d0-b1ab-42e0-9248-46eac402e76a","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"d666f268-f9ab-4763-8d60-3944280b965e","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"j0.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"51aa54e5-f0d8-4c1c-8472-caa92fea0374"},{"acceleration":0,"children":[],"maxAngle":170,"name":"HingeJoint1","angle":0,"minAngle":-170,"velocity":0,"type":"HingeJoint","nodeID":"e629f9ad-670b-4f6b-aa57-e138a15478a0","axle":"fa0ac4d0-b1ab-42e0-9248-46eac402e76a"},{"endEffector":"ad7e8c0c-2a8d-40e2-9742-d665fd3f6cd0","children":[{"hinge":"e629f9ad-670b-4f6b-aa57-e138a15478a0","children":[],"name":"X","type":"Motor","nodeID":"d6aab78e-fc45-4efe-9d98-51ad9f3d1bf6"},{"hinge":"a205abb3-9a41-44e2-bd90-c1ce2e23e3f5","children":[],"name":"Y","type":"Motor","nodeID":"65a06b4f-ee3f-4034-bdce-be05e86466c0"},{"hinge":"f05afef6-65fc-4ec8-9109-049686b6b363","children":[],"name":"Z","type":"Motor","nodeID":"cb94e23f-7ffd-47fe-b910-a65670ecd61c"},{"hinge":"0b11bdde-6f32-44dd-ba30-fe83ada876cf","children":[],"name":"U","type":"Motor","nodeID":"e681ccbf-4de2-4b49-ab1c-d861aa79ddac"},{"hinge":"b6190267-1136-4c80-9b01-cb70b1b2a05b","children":[],"name":"V","type":"Motor","nodeID":"66a01ec7-20a4-452a-b9b5-ba077dcc404d"},{"hinge":"8fa21884-a806-472f-acdb-8363580129f1","children":[],"name":"S","type":"Motor","nodeID":"01606993-54ff-4093-bb12-7d08d1132d0a"},{"children":[],"name":"target","type":"Pose","nodeID":"595e063b-0838-4e99-af1b-1484737c8103","local":[-1.6081226496766366E-16,0.4999999999999998,0.8660254037844387,30.360314680159,1,-2.791400186590845E-17,2.0180616542759E-16,9.76,1.2507731745095792E-16,0.8660254037844387,-0.4999999999999998,5,0,0,0,1]}],"name":"MarlinRobotArm","gripperMotor":"01606993-54ff-4093-bb12-7d08d1132d0a","type":"MarlinRobotArm","motors":["d6aab78e-fc45-4efe-9d98-51ad9f3d1bf6","65a06b4f-ee3f-4034-bdce-be05e86466c0","cb94e23f-7ffd-47fe-b910-a65670ecd61c","e681ccbf-4de2-4b49-ab1c-d861aa79ddac","66a01ec7-20a4-452a-b9b5-ba077dcc404d",null],"nodeID":"0fedfbfe-02f0-40fa-99ae-b2c0ada758ab","linearVelocity":0,"target":"595e063b-0838-4e99-af1b-1484737c8103"}],"name":"Sixi3","type":"Node","nodeID":"d9535f49-6aed-4e89-913a-d637171c69bc"}],"name":"Scene","type":"Node","nodeID":"e6e9591b-8ee4-4906-a9a1-6a3fdff82073"} \ No newline at end of file +{"children":[{"endEffector":"Base/a1/j1/a2/j2/a3/j3/a4/j4/a5/j5/End Effector/ScrewGripper/Jaw target","children":[{"children":[{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"d666f268-f9ab-4763-8d60-3944280b965e","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j0.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"51aa54e5-f0d8-4c1c-8472-caa92fea0374"},{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"children":[{"acceleration":0,"children":[{"children":[{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"dfdf791e-3d49-425e-b716-4905aaa8f91a","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\right arm.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-65536,"nodeID":"292604b1-8511-4df4-90c7-30476af428fd"}],"name":"RightJaw","type":"Pose","nodeID":"b2f9740d-d883-4a6e-b982-e09db1f82e88","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]}],"maxAngle":0,"name":"GripperServo","angle":0,"minAngle":-90,"velocity":0,"type":"HingeJoint","nodeID":"8fa21884-a806-472f-acdb-8363580129f1","version":1,"axle":"RightJaw"}],"name":"offset","type":"Pose","nodeID":"2440972a-52ea-4f2a-8c58-43d98e834211","local":[1,0,0,0.015,0,0.7071067811865476,0.7071067811865475,-1.405,0,-0.7071067811865475,0.7071067811865476,3.3256,0,0,0,1]},{"children":[{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"0b7c9c49-af85-4205-8ad6-959274bbfe8f","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\left arm.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-26317,"nodeID":"ecd23f91-3d0d-45fb-a96d-b79fe8268d7c"}],"name":"FixedLeftJaw","type":"Pose","nodeID":"10f67c7f-e71e-47df-87f2-67e6be91c2dc","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"Jaw target","type":"Pose","nodeID":"ad7e8c0c-2a8d-40e2-9742-d665fd3f6cd0","local":[1,0,0,0.63,0,1,0,-3.9147,0,0,1,6.4288,0,0,0,1]}],"name":"ScrewGripper","type":"Pose","nodeID":"6511b56b-b426-4fc1-b439-6c643fe1b60b","local":[6.123233995736766E-17,1,0,0,-1,6.123233995736766E-17,0,0,0,0,1,0,0,0,0,1]}],"name":"End Effector","type":"Pose","nodeID":"72cd0146-0aff-4544-8730-27948a80fce2","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"8449f6a3-4168-4b82-ba40-96ce7549749e","local":[-1,-7.498798913309288E-33,1.2246467991473532E-16,-2.838976209783394E-15,7.498798913309288E-33,-1,-9.18338008663387E-49,9.129999999999999,1.2246467991473532E-16,9.18338008663387E-49,1,-43.464,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j5.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"5b40d155-723f-438d-bfb4-ffafe1ef3c3d"}],"name":"j5","type":"Pose","nodeID":"d4ca58f5-d1d4-4a37-88a0-d72ce120779e","local":[1,0,0,0,0,1,0,0,0,0,1,5.1,0,0,0,1]},{"r":0,"d":5.1,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"cf4ad47b-4526-48e5-8fce-adc1aad27823","theta":0}],"name":"a5","type":"Pose","nodeID":"9a56a94a-bfb2-4eba-a694-5464e862fed9","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"3bebfb52-6fb5-4c72-ad6d-a15b22e559c6","local":[-1,-7.498798913309288E-33,1.2246467991473532E-16,-2.8389762097833943E-15,7.498798913309288E-33,-1,-9.18338008663387E-49,9.129999999999999,1.2246467991473532E-16,9.18338008663387E-49,1,-38.364,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j4.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"5a189f00-14e9-4dde-84cc-4a96c3b31cf6"},{"acceleration":0,"children":[],"maxAngle":360,"name":"HingeJoint5","angle":0,"minAngle":0,"velocity":0,"type":"HingeJoint","nodeID":"b6190267-1136-4c80-9b01-cb70b1b2a05b","version":1,"axle":"../a5"}],"name":"j4","type":"Pose","nodeID":"07fa214e-5cfe-4f18-aa93-eb05ae65f8d5","local":[6.123233995736766E-17,6.123233995736766E-17,1,0,-1,3.749399456654644E-33,6.123233995736766E-17,-0,0,-1,6.123233995736766E-17,0,0,0,0,1]},{"r":0,"d":0,"children":[],"alpha":-90,"name":"DH Parameter","type":"DHParameter","nodeID":"dcf481c0-330c-46d3-a75a-706833d7be38","theta":-90}],"name":"a4","type":"Pose","nodeID":"cec604c9-8f09-4bb0-afe1-65ce3a0ebec3","local":[0.9999987371446676,0.00158924795738581,0,0,-0.00158924795738581,0.9999987371446676,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"32a4cdc8-82dc-4d4d-8082-0ae589879e89","local":[6.123233995736766E-17,-6.123233995736766E-17,1,-38.364,1,3.749399456654644E-33,-6.123233995736766E-17,4.898587196589414E-16,0,1,6.123233995736766E-17,-9.13,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j3.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"ee8b6ae6-a9da-4839-9721-404b10b18fc8"},{"acceleration":0,"children":[],"maxAngle":440,"name":"HingeJoint4","angle":359.9089427611113,"minAngle":100,"velocity":0,"type":"HingeJoint","nodeID":"0b11bdde-6f32-44dd-ba30-fe83ada876cf","version":1,"axle":"../a4"}],"name":"j3","type":"Pose","nodeID":"f2805245-dce5-4c20-8c04-d01564a149ef","local":[1,0,0,12.455,0,1,0,0,0,0,1,0,0,0,0,1]},{"r":12.455,"d":0,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"34b4c6d5-b5f2-43cf-bfaf-8480d54486d8","theta":0}],"name":"a3","type":"Pose","nodeID":"ab22a713-9f61-46e8-bb73-5966559b4dff","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"663c1042-df69-4cdf-8cdc-6296d4a9c43d","local":[6.123233995736766E-17,-6.123233995736766E-17,1,-25.909,1,3.749399456654644E-33,-6.123233995736766E-17,4.898587196589414E-16,0,1,6.123233995736766E-17,-9.13,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j2.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"c97e741a-4cc0-400c-9d47-a5ef40bbc74e"},{"acceleration":0,"children":[],"maxAngle":150,"name":"HingeJoint3","angle":0,"minAngle":-150,"velocity":0,"type":"HingeJoint","nodeID":"f05afef6-65fc-4ec8-9109-049686b6b363","version":1,"axle":"../a3"}],"name":"j2","type":"Pose","nodeID":"47978d1e-34fa-4283-a4c2-74dbf35b7ffc","local":[6.123233995736766E-17,1,0,1.0966099762964973E-15,-1,6.123233995736766E-17,0,-17.909,0,0,1,9.13,0,0,0,1]},{"r":17.909,"d":9.13,"children":[],"alpha":0,"name":"DH Parameter","type":"DHParameter","nodeID":"d153fcfe-9d51-47d7-9eb4-b87dcee75a5d","theta":-90}],"name":"a2","type":"Pose","nodeID":"9278c3c4-3a07-4cf0-881c-6680d9c4a470","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"children":[],"name":"MeshInstance","type":"MeshInstance","nodeID":"814781b4-f050-48ca-981b-4e6e5d5a3711","local":[1,0,0,0,-0,6.123233995736766E-17,-1,8,0,1,6.123233995736766E-17,-4.898587196589413E-16,0,0,0,1],"mesh":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\j1.obj"},{"shininess":10,"isLit":true,"specularColor":-1,"children":[],"texture":"C:\\Users\\aggra\\Desktop\\RO3 test scenes\\Sixi3-5\\SIXI3_BASE.png","name":"Material","emissionColor":-16777216,"type":"Material","diffuseColor":-1,"nodeID":"cfc200e6-bd64-45c1-92e0-39daa3dce42d"},{"acceleration":0,"children":[],"maxAngle":90,"name":"HingeJoint2","angle":0,"minAngle":-90,"velocity":0,"type":"HingeJoint","nodeID":"a205abb3-9a41-44e2-bd90-c1ce2e23e3f5","version":1,"axle":"../a2"}],"name":"j1","type":"Pose","nodeID":"83895d18-83dd-4b3b-bf27-d3830f63d8e4","local":[1,0,0,0,0,6.123233995736766E-17,1,0,0,-1,6.123233995736766E-17,8,0,0,0,1]},{"r":0,"d":8,"children":[],"alpha":-90,"name":"DH Parameter","type":"DHParameter","nodeID":"8b9d9beb-6eb9-409a-b2d5-9dc633761da7","theta":0}],"name":"a1","type":"Pose","nodeID":"fa0ac4d0-b1ab-42e0-9248-46eac402e76a","local":[1,-0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"acceleration":0,"children":[],"maxAngle":170,"name":"HingeJoint1","angle":0,"minAngle":-170,"velocity":0,"type":"HingeJoint","nodeID":"e629f9ad-670b-4f6b-aa57-e138a15478a0","version":1,"axle":"../a1"}],"name":"Base","type":"Pose","nodeID":"93de1ee4-d8fc-4afa-8131-0f35158c8245","local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]},{"hinge":"../Base/HingeJoint1","children":[],"name":"X","type":"Motor","nodeID":"d6aab78e-fc45-4efe-9d98-51ad9f3d1bf6","version":1},{"hinge":"../Base/a1/j1/HingeJoint2","children":[],"name":"Y","type":"Motor","nodeID":"65a06b4f-ee3f-4034-bdce-be05e86466c0","version":1},{"hinge":"../Base/a1/j1/a2/j2/HingeJoint3","children":[],"name":"Z","type":"Motor","nodeID":"cb94e23f-7ffd-47fe-b910-a65670ecd61c","version":1},{"hinge":"../Base/a1/j1/a2/j2/a3/j3/HingeJoint4","children":[],"name":"U","type":"Motor","nodeID":"e681ccbf-4de2-4b49-ab1c-d861aa79ddac","version":1},{"hinge":"../Base/a1/j1/a2/j2/a3/j3/a4/j4/HingeJoint5","children":[],"name":"V","type":"Motor","nodeID":"66a01ec7-20a4-452a-b9b5-ba077dcc404d","version":1},{"children":[],"name":"MarlinRobotArm","type":"MarlinRobotArm","nodeID":"0fedfbfe-02f0-40fa-99ae-b2c0ada758ab","version":2},{"limb":"..","children":[{"children":[],"name":"target","type":"Pose","nodeID":"595e063b-0838-4e99-af1b-1484737c8103","local":[-6.113494925857745E-17,-0.9999987371446676,-0.0015892479573856877,3.8963729344491234,1,-6.113502658616447E-17,7.732758702731381E-23,9.76,-9.723604361616526E-20,-0.0015892479573856877,0.9999987371446676,49.89900686977222,0,0,0,1]}],"name":"LimbSolver","type":"LimbSolver","nodeID":"0fedfbfe-02f0-40fa-99ae-b2c0ada758ab","version":2,"linearVelocity":0,"target":"target"}],"name":"Sixi3","type":"Limb","motors":["X","Y","Z","U","V",null],"nodeID":"0fedfbfe-02f0-40fa-99ae-b2c0ada758ab","version":1,"local":[1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1]}],"name":"Scene","type":"Node","nodeID":"e6e9591b-8ee4-4906-a9a1-6a3fdff82073"} \ No newline at end of file diff --git a/src/test/resources/logback-test.xml b/src/test/resources/logback-test.xml index df5d7852d..dd281f3ff 100644 --- a/src/test/resources/logback-test.xml +++ b/src/test/resources/logback-test.xml @@ -11,7 +11,7 @@ - +