diff --git a/README.PNG b/README.PNG deleted file mode 100644 index 3d6a5d14f..000000000 Binary files a/README.PNG and /dev/null differ diff --git a/README.md b/README.md index e89865d2c..9b3d95786 100644 --- a/README.md +++ b/README.md @@ -1,40 +1,30 @@ +# Robot Overlord ![workflow](https://github.com/MarginallyClever/Robot-Overlord-App/actions/workflows/main.yml/badge.svg) [![Javadoc](https://img.shields.io/badge/JavaDoc-Online-green)](https://marginallyclever.github.io/robot-overlord-app/javadoc/) # Robot Overlord # ![Preview image](Screenshot_2023-12-23_120520.png) + Robot Overlord is 3D control software for robots. It is intended to be easier than ROS. It was started by [Marginally Clever Robots, Ltd.](http://www.marginallyclever.com/) We would love to see your robot run in the app. Please joint our [Discord channel](https://discord.gg/Q5TZFmB) and talk live with a human! Some of the robots it controls are: - - Sixi 2+3, 6DOF arms - - Arm3, a 3DOF arm - - Thor, a 5DOF arm - - Rotary Stewart Platforms, like flight simulators - - Delta Robot 3, aka a Kossel - - Spidee, a 6 legged crab-style walker. - - Dog Robot, a generic 4 legged walker. +- Sixi 3, AR4, Mecademic Meca500, and other 6DOF arms +- Rotary Stewart Platforms, like flight simulators +- Dog robots. +- Spidee, a 6 legged crab-style walker. -# Why +## Why -The short answer? ROS is too hard for most people. We want to make it easier. +The short answer? ROS is too hard. We want to make it easier. [Our philosophy about Robot Overlord](https://github.com/MarginallyClever/Robot-Overlord-App/wiki/Why-Robot-Overlord%3F). -## Get Started! - -Steps to get started: - -1. Install The latest Java -2. Install Robot Overlord App from [the "Releases" section of the Github repository](https://github.com/MarginallyClever/Robot-Overlord-App/releases) - -Then you should be able to run the application. - -## More +## Get Started today! -If you're reading this, make an issue ticket, and we will respond to it promptly. +[Read our friendly manual](https://mcr.dozuki.com/c/Robot_Overlord_3). It has pictures with arrows and everything! ## Icons diff --git a/Screenshot.png b/Screenshot.png new file mode 100644 index 000000000..e15656a5e Binary files /dev/null and b/Screenshot.png differ diff --git a/Screenshot_2023-12-23_120520.png b/Screenshot_2023-12-23_120520.png deleted file mode 100644 index aaef6ea6a..000000000 Binary files a/Screenshot_2023-12-23_120520.png and /dev/null differ diff --git a/pom.xml b/pom.xml index 625267e37..ee95d06c0 100644 --- a/pom.xml +++ b/pom.xml @@ -6,7 +6,7 @@ com.marginallyclever RobotOverlord - 2.98.0 + 2.99.0 Robot Overlord A friendly 3D user interface for controlling robots. https://www.marginallyclever.com/ diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/CheckForUpdateAction.java b/src/main/java/com/marginallyclever/ro3/apps/actions/CheckForUpdateAction.java index a386ec83b..e3b553803 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/CheckForUpdateAction.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/CheckForUpdateAction.java @@ -1,8 +1,6 @@ package com.marginallyclever.ro3.apps.actions; -import com.marginallyclever.ro3.RO3; import com.marginallyclever.ro3.apps.RO3Frame; -import com.marginallyclever.robotoverlord.RobotOverlord; import org.slf4j.Logger; import org.slf4j.LoggerFactory; 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 03d259ba2..a4d93624f 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/LoadScene.java @@ -94,8 +94,12 @@ public void commitLoad(File selectedFile) { // if the json is bad, this will throw an exception before removing the previous scene. JSONObject json = new JSONObject(content); - Registry.reset(); + // reset everything + NewScene newScene = new NewScene(); + newScene.commitNewScene(); + + // do it! String newCWD = selectedFile.getParent() + File.separator; String oldCWD = System.getProperty("user.dir"); System.setProperty("user.dir",newCWD); @@ -106,7 +110,7 @@ public void commitLoad(File selectedFile) { System.setProperty("user.dir",oldCWD); - menu.addPath(selectedFile.getAbsolutePath()); + if(menu!=null) menu.addPath(selectedFile.getAbsolutePath()); } catch (IOException e) { logger.error("Error loading file.", e); } diff --git a/src/main/java/com/marginallyclever/ro3/apps/actions/UndoAction.java b/src/main/java/com/marginallyclever/ro3/apps/actions/UndoAction.java index 13ca29fa1..066fd4886 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/actions/UndoAction.java +++ b/src/main/java/com/marginallyclever/ro3/apps/actions/UndoAction.java @@ -1,9 +1,5 @@ package com.marginallyclever.ro3.apps.actions; -import com.marginallyclever.robotoverlord.swing.translator.Translator; -import org.slf4j.Logger; -import org.slf4j.LoggerFactory; - import javax.swing.*; import javax.swing.undo.UndoManager; import java.awt.event.ActionEvent; diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java b/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java index f31ff0198..143213aaa 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/Viewport.java @@ -383,9 +383,8 @@ private void renderViewportTools() { private void renderAllPasses() { // renderPasses that are always on for(RenderPass pass : renderPasses.getList()) { - if(pass.getActiveStatus()==RenderPass.ALWAYS) { - pass.draw(this); - } + if(pass.getActiveStatus()==RenderPass.NEVER) continue; + pass.draw(this); } } diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawBoundingBoxes.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawBoundingBoxes.java index c82903f8f..6eb7448d7 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawBoundingBoxes.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawBoundingBoxes.java @@ -94,7 +94,6 @@ public void draw(Viewport viewport) { shader.setVector3d(gl3,"cameraPos",cameraWorldPos); // Camera position in world space shader.setVector3d(gl3,"lightPos",cameraWorldPos); // Light position in world space shader.setColor(gl3,"lightColor", Color.WHITE); - shader.setColor(gl3,"objectColor",new Color(255,255,255,64)); shader.setColor(gl3,"specularColor",Color.GRAY); shader.setColor(gl3,"ambientColor",new Color(255/5,255/5,255/5,255)); shader.set1i(gl3,"useVertexColor",0); @@ -105,41 +104,46 @@ public void draw(Viewport viewport) { gl3.glDisable(GL3.GL_TEXTURE_2D); gl3.glDisable(GL3.GL_DEPTH_TEST); - // find all MeshInstance nodes in Registry - List toScan = new ArrayList<>(Registry.getScene().getChildren()); + Color unselected = new Color(255,255,255,64); + Color selected = new Color(226, 115, 42,128); + + var list = Registry.selection.getList(); + var toScan = new ArrayList<>(Registry.getScene().getChildren()); while(!toScan.isEmpty()) { Node node = toScan.remove(0); - - if(node instanceof MeshInstance meshInstance) { - // if they have a mesh, draw it. - Mesh mesh2 = meshInstance.getMesh(); - if(mesh2==null) continue; - - AABB boundingBox = mesh2.getBoundingBox(); - Point3d max = boundingBox.getBoundsTop(); - Point3d min = boundingBox.getBoundsBottom(); - mesh.setVertex(0, min.x, max.y, max.z); - mesh.setVertex(1, max.x, max.y, max.z); - mesh.setVertex(2, max.x, min.y, max.z); - mesh.setVertex(3, min.x, min.y, max.z); - mesh.setVertex(4, min.x, max.y, min.z); - mesh.setVertex(5, max.x, max.y, min.z); - mesh.setVertex(6, max.x, min.y, min.z); - mesh.setVertex(7, min.x, min.y, min.z); - mesh.updateVertexBuffers(gl3); - - // set the model matrix - Matrix4d w = meshInstance.getWorld(); - w.transpose(); - shader.setMatrix4d(gl3,"modelMatrix",w); - // draw it - mesh.render(gl3); - - OpenGLHelper.checkGLError(gl3,logger); - } - toScan.addAll(node.getChildren()); + + if(!(node instanceof MeshInstance meshInstance)) continue; + if(getActiveStatus()==SOMETIMES && !list.contains(meshInstance)) continue; + + // if they have a mesh, draw it. + Mesh mesh2 = meshInstance.getMesh(); + if(mesh2==null) continue; + + AABB boundingBox = mesh2.getBoundingBox(); + Point3d max = boundingBox.getBoundsTop(); + Point3d min = boundingBox.getBoundsBottom(); + mesh.setVertex(0, min.x, max.y, max.z); + mesh.setVertex(1, max.x, max.y, max.z); + mesh.setVertex(2, max.x, min.y, max.z); + mesh.setVertex(3, min.x, min.y, max.z); + mesh.setVertex(4, min.x, max.y, min.z); + mesh.setVertex(5, max.x, max.y, min.z); + mesh.setVertex(6, max.x, min.y, min.z); + mesh.setVertex(7, min.x, min.y, min.z); + mesh.updateVertexBuffers(gl3); + + // set the model matrix + Matrix4d w = meshInstance.getWorld(); + w.transpose(); + shader.setMatrix4d(gl3,"modelMatrix",w); + + // highlight selected items + shader.setColor(gl3,"objectColor", list.contains(meshInstance) ? selected : unselected ); + // draw it + mesh.render(gl3); } + gl3.glEnable(GL3.GL_DEPTH_TEST); } } diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawCameras.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawCameras.java index 1417719f7..1b322d2d4 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawCameras.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawCameras.java @@ -28,6 +28,8 @@ public class DrawCameras extends AbstractRenderPass { private final Mesh rayMesh = new Mesh(); private ShaderProgram shader; private final double cameraConeRatio = 50; + private final double coneScale = cameraConeRatio * 0.0001; + public DrawCameras() { super("Cameras"); @@ -37,8 +39,8 @@ public DrawCameras() { private void setupMeshRay() { rayMesh.setRenderStyle(GL3.GL_LINES); - rayMesh.addColor(1,1,1,1); rayMesh.addVertex(0,0,0); - rayMesh.addColor(1,1,1,1); rayMesh.addVertex(0,0,0); + rayMesh.addVertex(0,0,0); + rayMesh.addVertex(0,0,0); } private void setupMeshCone() { @@ -48,11 +50,11 @@ private void setupMeshCone() { Vector3d b = new Vector3d( 1,-1,-1); Vector3d c = new Vector3d( 1, 1,-1); Vector3d d = new Vector3d(-1, 1,-1); - pyramidMesh.addColor(0,0,0,1); pyramidMesh.addVertex(0,0,0); - pyramidMesh.addColor(0,0,0,1); pyramidMesh.addVertex((float)a.x, (float)a.y, (float)a.z); - pyramidMesh.addColor(0,0,0,1); pyramidMesh.addVertex((float)b.x, (float)b.y, (float)b.z); - pyramidMesh.addColor(0,0,0,1); pyramidMesh.addVertex((float)c.x, (float)c.y, (float)c.z); - pyramidMesh.addColor(0,0,0,1); pyramidMesh.addVertex((float)d.x, (float)d.y, (float)d.z); + pyramidMesh.addVertex(0,0,0); + pyramidMesh.addVertex((float)a.x, (float)a.y, (float)a.z); + pyramidMesh.addVertex((float)b.x, (float)b.y, (float)b.z); + pyramidMesh.addVertex((float)c.x, (float)c.y, (float)c.z); + pyramidMesh.addVertex((float)d.x, (float)d.y, (float)d.z); pyramidMesh.addIndex(0); pyramidMesh.addIndex(1); @@ -105,29 +107,31 @@ public void draw(Viewport viewport) { shader.setVector3d(gl3,"cameraPos",cameraWorldPos); // Camera position in world space shader.setVector3d(gl3,"lightPos",cameraWorldPos); // Light position in world space shader.setColor(gl3,"lightColor", Color.WHITE); - shader.setColor(gl3,"objectColor",Color.GREEN); shader.setColor(gl3,"specularColor",Color.DARK_GRAY); shader.setColor(gl3,"ambientColor",Color.BLACK); - shader.set1i(gl3,"useVertexColor",1); + shader.set1i(gl3,"useVertexColor",0); shader.set1i(gl3,"useLighting",0); shader.set1i(gl3,"diffuseTexture",0); gl3.glDisable(GL3.GL_DEPTH_TEST); gl3.glDisable(GL3.GL_TEXTURE_2D); - double coneScale = cameraConeRatio *0.0001; + var list = Registry.selection.getList(); var normalizedCoordinates = viewport.getCursorAsNormalized(); - // position and draw the ray from the camera. - Matrix4d w = MatrixHelper.createIdentityMatrix4(); - shader.setMatrix4d(gl3, "modelMatrix", w); for(Camera cam : Registry.cameras.getList() ) { + boolean selected = list.contains(cam); + if(getActiveStatus()==SOMETIMES && !selected) continue; + + // position and draw the ray from the camera. + Matrix4d w = MatrixHelper.createIdentityMatrix4(); + shader.setMatrix4d(gl3, "modelMatrix", w); + shader.setColor(gl3,"objectColor",selected ? Color.GREEN : Color.DARK_GRAY); Ray ray = viewport.getRayThroughPoint(cam,normalizedCoordinates.x,normalizedCoordinates.y); changeRayMesh(gl3, ray); rayMesh.render(gl3); - } - // scale and draw the view cones - for(Camera cam : Registry.cameras.getList() ) { + // scale and draw the view cones + shader.setColor(gl3,"objectColor",selected ? Color.WHITE : Color.BLACK); w = cam.getWorld(); Matrix4d scale = MatrixHelper.createIdentityMatrix4(); scale.m00 *= canvasWidth * coneScale; diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawDHParameters.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawDHParameters.java index 95991490e..aae6b1bb3 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawDHParameters.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawDHParameters.java @@ -90,29 +90,34 @@ public void draw(Viewport viewport) { gl3.glDisable(GL3.GL_TEXTURE_2D); gl3.glDisable(GL3.GL_DEPTH_TEST); + var list = Registry.selection.getList(); + List toScan = new ArrayList<>(); toScan.add(Registry.getScene()); while(!toScan.isEmpty()) { Node node = toScan.remove(0); toScan.addAll(node.getChildren()); - if(node instanceof DHParameter parameter) { - double d = parameter.getD(); - mesh.setVertex(1,0,0,d); - mesh.setVertex(2,0,0,d); - double s = Math.sin(Math.toRadians(parameter.getTheta())); - double c = Math.cos(Math.toRadians(parameter.getTheta())); - double r = parameter.getR(); - mesh.setVertex(3,c*r,s*r,d); - mesh.updateVertexBuffers(gl3); - - // set modelView to world - Pose parentPose = parameter.findParent(Pose.class); - Matrix4d w = (parentPose==null) ? MatrixHelper.createIdentityMatrix4() : parentPose.getWorld(); - w.transpose(); - shader.setMatrix4d(gl3,"modelMatrix",w); - mesh.render(gl3); - } + if(!(node instanceof DHParameter parameter)) continue; + if(getActiveStatus()==SOMETIMES && !list.contains(parameter)) continue; + + shader.setColor(gl3,"objectColor",list.contains(parameter) ? Color.WHITE : Color.GRAY); + + double d = parameter.getD(); + mesh.setVertex(1,0,0,d); + mesh.setVertex(2,0,0,d); + double s = Math.sin(Math.toRadians(parameter.getTheta())); + double c = Math.cos(Math.toRadians(parameter.getTheta())); + double r = parameter.getR(); + mesh.setVertex(3,c*r,s*r,d); + mesh.updateVertexBuffers(gl3); + + // set modelView to world + Pose parentPose = parameter.findParent(Pose.class); + Matrix4d w = (parentPose==null) ? MatrixHelper.createIdentityMatrix4() : parentPose.getWorld(); + w.transpose(); + shader.setMatrix4d(gl3,"modelMatrix",w); + mesh.render(gl3); } gl3.glEnable(GL3.GL_DEPTH_TEST); diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java index 15062204b..0f3b651ce 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawHingeJoints.java @@ -80,39 +80,44 @@ public void draw(Viewport viewport) { gl3.glDisable(GL3.GL_TEXTURE_2D); gl3.glDisable(GL3.GL_CULL_FACE); - List toScan = new ArrayList<>(); - toScan.add(Registry.getScene()); + var list = Registry.selection.getList(); + + var toScan = new ArrayList<>(Registry.getScene().getChildren()); while(!toScan.isEmpty()) { Node node = toScan.remove(0); toScan.addAll(node.getChildren()); - if(node instanceof HingeJoint joint) { - - // adjust the position of the mesh based on the joint's minimum angle. - Pose pose = joint.findParent(Pose.class); - Matrix4d w = (pose==null) ? MatrixHelper.createIdentityMatrix4() : pose.getWorld(); - - Matrix4d rZ = new Matrix4d(); - rZ.rotZ(Math.toRadians(joint.getMinAngle())); - w.mul(rZ); - w.mul(w,MatrixHelper.createScaleMatrix4(ringScale)); - w.transpose(); - shader.setColor(gl3,"objectColor",new Color(255,255,0,64)); - shader.setMatrix4d(gl3,"modelMatrix",w); - // draw the range fan - int range = Math.max(0, (int)(joint.getMaxAngle()-joint.getMinAngle()) ); - circleFanMesh.render(gl3,0,1+range); - - // draw the current angle line - w = (pose==null) ? MatrixHelper.createIdentityMatrix4() : pose.getWorld(); - rZ.rotZ(Math.toRadians(joint.getAngle())); - w.mul(rZ); - w.mul(w,MatrixHelper.createScaleMatrix4(ringScale)); - w.transpose(); - shader.setColor(gl3,"objectColor",Color.WHITE); - shader.setMatrix4d(gl3,"modelMatrix",w); - currentAngleMesh.render(gl3); - } + if(!(node instanceof HingeJoint joint)) continue; + if(getActiveStatus()==SOMETIMES && !list.contains(joint)) continue; + + // make bigger if selected + double scale = ringScale; + if(list.contains(joint)) scale*=2; + + // adjust the position of the mesh based on the joint's minimum angle. + Pose pose = joint.findParent(Pose.class); + Matrix4d w = (pose==null) ? MatrixHelper.createIdentityMatrix4() : pose.getWorld(); + + Matrix4d rZ = new Matrix4d(); + rZ.rotZ(Math.toRadians(joint.getMinAngle())); + w.mul(rZ); + w.mul(w,MatrixHelper.createScaleMatrix4(scale)); + w.transpose(); + shader.setColor(gl3,"objectColor",new Color(255,255,0,64)); + shader.setMatrix4d(gl3,"modelMatrix",w); + // draw the range fan + int range = Math.max(0, (int)(joint.getMaxAngle()-joint.getMinAngle()) ); + circleFanMesh.render(gl3,0,1+range); + + // draw the current angle line + w = (pose==null) ? MatrixHelper.createIdentityMatrix4() : pose.getWorld(); + rZ.rotZ(Math.toRadians(joint.getAngle())); + w.mul(rZ); + w.mul(w,MatrixHelper.createScaleMatrix4(scale)); + w.transpose(); + shader.setColor(gl3,"objectColor",Color.WHITE); + shader.setMatrix4d(gl3,"modelMatrix",w); + currentAngleMesh.render(gl3); } gl3.glEnable(GL3.GL_DEPTH_TEST); diff --git a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawPoses.java b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawPoses.java index d013430c7..8772c55d7 100644 --- a/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawPoses.java +++ b/src/main/java/com/marginallyclever/ro3/apps/render/renderpasses/DrawPoses.java @@ -75,8 +75,7 @@ public void draw(Viewport viewport) { gl3.glDisable(GL3.GL_TEXTURE_2D); // collect all poses, separating out the selected ones - var collected = new ArrayList(); - var sel = new ArrayList(); + var list = Registry.selection.getList(); var toScan = new ArrayList(); toScan.add(Registry.getScene()); @@ -84,31 +83,16 @@ public void draw(Viewport viewport) { Node node = toScan.remove(0); toScan.addAll(node.getChildren()); - if (node.getClass().equals(Pose.class)) { - if(Registry.selection.getList().contains(node)) { - sel.add((Pose) node); - } else { - collected.add((Pose) node); - } - } - } + if (!(node instanceof Pose pose)) continue; + boolean selected = list.contains(pose); + if (getActiveStatus() == SOMETIMES && !selected) continue; - // draw unselected - shader.setColor(gl3,"objectColor",Color.GRAY); - for(Pose pose : collected) { - Matrix4d w = pose.getWorld(); - w.transpose(); - shader.setMatrix4d(gl3,"modelMatrix",w); - mesh.render(gl3); - } + shader.setColor(gl3, "objectColor", selected ? Color.WHITE : Color.GRAY); - // draw selected - shader.setColor(gl3,"objectColor",Color.WHITE); - for(Pose pose : sel) { Matrix4d w = pose.getWorld(); - w.mul(w,MatrixHelper.createScaleMatrix4(2)); + w.mul(w, MatrixHelper.createScaleMatrix4(selected ? 2 : 1)); w.transpose(); - shader.setMatrix4d(gl3,"modelMatrix",w); + shader.setMatrix4d(gl3, "modelMatrix", w); mesh.render(gl3); } diff --git a/src/main/java/com/marginallyclever/ro3/node/Node.java b/src/main/java/com/marginallyclever/ro3/node/Node.java index 9da1f11d7..1575ba9cf 100644 --- a/src/main/java/com/marginallyclever/ro3/node/Node.java +++ b/src/main/java/com/marginallyclever/ro3/node/Node.java @@ -187,10 +187,24 @@ public T findParent(Class type) { * @return the child, or null if none found. */ public Node findChild(String name) { + return findChild(name,1); + } + + /** + * Find the first child of this node with the given name. + * @param name the name to match. + * @param maxDepth the maximum depth to search. + * @return the child, or null if none found. + */ + public Node findChild(String name, int maxDepth) { + if(maxDepth==0) return null; + for(Node child : children) { if(child.getName().equals(name)) { return child; } + Node found = child.findChild(name,maxDepth-1); + if(found!=null) return found; } return null; } @@ -203,13 +217,16 @@ public Node findChild(String name) { public Node get(String path) { String[] parts = path.split("/"); Node node = this; + int i=0; if(parts[0].isEmpty()) { node = getRootNode(); + ++i; } - for(String part : parts) { + for(;i { - motor.getAxle().setAngle(dial.getValue()); + motor.getHinge().setAngle(dial.getValue()); }); // TODO subscribe to motor.getAxle().getAngle(), then dial.setValue() without triggering an action event. @@ -313,7 +313,7 @@ private String getMotorsAndFeedrateAsString() { if(motor!=null && motor.hasAxle()) { sb.append(" ") .append(motor.getName()) - .append(StringHelper.formatDouble(motor.getAxle().getAngle())); + .append(StringHelper.formatDouble(motor.getHinge().getAngle())); } } // gripper motor @@ -321,7 +321,7 @@ private String getMotorsAndFeedrateAsString() { if(gripperMotor!=null && gripperMotor.hasAxle()) { sb.append(" ") .append(gripperMotor.getName()) - .append(StringHelper.formatDouble(gripperMotor.getAxle().getAngle())); + .append(StringHelper.formatDouble(gripperMotor.getHinge().getAngle())); } // feedrate sb.append(" F").append(StringHelper.formatDouble(linearVelocity)); @@ -371,7 +371,7 @@ private String parseG0(String gcode) { for (String p : parts) { if (p.startsWith(motor.getName())) { // TODO check new value is in range. - motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + motor.getHinge().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); break; } } @@ -383,7 +383,7 @@ private String parseG0(String gcode) { for (String p : parts) { if (p.startsWith(gripperMotor.getName())) { // TODO check new value is in range. - gripperMotor.getAxle().setAngle(Double.parseDouble(p.substring(gripperMotor.getName().length()))); + gripperMotor.getHinge().setAngle(Double.parseDouble(p.substring(gripperMotor.getName().length()))); break; } } @@ -478,7 +478,16 @@ private double[] getCartesianFromWorld(Matrix4d world) { * @return the number of non-null motors. */ public int getNumJoints() { - return (int)motors.stream().filter(Objects::nonNull).count(); + // count the number of motors such that motors.get(i).getSubject()!=null + int count=0; + for(NodePath paths : motors) { + if(paths.getSubject()!=null) count++; + } + return count; + } + + public Motor getJoint(int i) { + return motors.get(i).getSubject(); } public double[] getAllJointAngles() { @@ -487,7 +496,7 @@ public double[] getAllJointAngles() { for(NodePath paths : motors) { Motor motor = paths.getSubject(); if(motor!=null) { - result[i++] = motor.getAxle().getAngle(); + result[i++] = motor.getHinge().getAngle(); } } return result; @@ -502,7 +511,7 @@ public void setAllJointAngles(double[] values) { for(NodePath paths : motors) { Motor motor = paths.getSubject(); if(motor!=null) { - HingeJoint axle = motor.getAxle(); + HingeJoint axle = motor.getHinge(); if(axle!=null) { axle.setAngle(values[i++]); axle.update(0); @@ -520,7 +529,7 @@ public void setAllJointVelocities(double[] values) { for(NodePath paths : motors) { Motor motor = paths.getSubject(); if(motor!=null) { - HingeJoint axle = motor.getAxle(); + HingeJoint axle = motor.getHinge(); if(axle!=null) { axle.setVelocity(values[i++]); } 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 41e284ddc..f9a630471 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 @@ -1,19 +1,11 @@ package com.marginallyclever.ro3.node.nodes.marlinrobotarm; -import com.marginallyclever.ro3.node.nodes.marlinrobotarm.ApproximateJacobian; -import com.marginallyclever.ro3.node.nodes.marlinrobotarm.ApproximateJacobianScrewTheory; -import com.marginallyclever.robotoverlord.components.ArmEndEffectorComponent; -import com.marginallyclever.robotoverlord.components.DHComponent; -import com.marginallyclever.robotoverlord.components.RobotComponent; -import com.marginallyclever.robotoverlord.entity.Entity; -import com.marginallyclever.robotoverlord.entity.EntityManager; -import com.marginallyclever.robotoverlord.robots.Robot; -import com.marginallyclever.robotoverlord.systems.robot.robotarm.ApproximateJacobianFiniteDifferences; +import com.marginallyclever.ro3.Registry; +import com.marginallyclever.ro3.apps.actions.LoadScene; import org.junit.jupiter.api.*; -import java.util.ArrayList; +import java.io.File; import java.util.Arrays; -import java.util.List; /** * Checking if Approximate jacobians are commutative. This test is ignored because they are not. @@ -21,94 +13,19 @@ * @since 2.6.1 * @author Dan Royer */ -@Disabled public class ApproximateJacobianTest { - private EntityManager entityManager; - @BeforeEach public void setup() { - entityManager = new EntityManager(); - } - - @AfterEach - public void teardown() { - entityManager.clear(); - entityManager = null; + Registry.start(); } - private RobotComponent build5AxisArm() { - Entity base = new Entity("Sixi3-5"); - RobotComponent robot = new RobotComponent(); - base.addComponent(robot); - - // add target - Entity target = new Entity(RobotComponent.TARGET_NAME); - entityManager.addEntityToParent(target, base); - - // position arm - List joints = new ArrayList<>(); - List dh = new ArrayList<>(); - Entity prev = base; - int numJoints=5; - for(int i=0;i joints = new ArrayList<>(); - List dh = new ArrayList<>(); - Entity prev = base; - int numJoints=6; - for(int i=0;i