From 47f9b5ca91b1de8372dcb3a97001aa642e8d7ac3 Mon Sep 17 00:00:00 2001 From: Dan Royer Date: Thu, 21 Dec 2023 09:09:41 -0800 Subject: [PATCH] apply v to Motors instead of Joints --- pom.xml | 4 +- .../com/marginallyclever/ro3/RO3Frame.java | 2 +- .../ro3/node/nodes/Motor.java | 7 + .../ApproximateJacobianFiniteDifferences.java | 6 +- .../nodes/marlinrobotarm/MarlinRobotArm.java | 205 ++++++++++++------ .../ro3/node/nodetreeview/NodeTreeView.java | 2 +- 6 files changed, 151 insertions(+), 75 deletions(-) diff --git a/pom.xml b/pom.xml index 30bcea9b3..03a7fd03e 100644 --- a/pom.xml +++ b/pom.xml @@ -6,7 +6,7 @@ com.marginallyclever RobotOverlord - 2.11.0 + 2.90.0 Robot Overlord A friendly 3D user interface for controlling robots. http://www.marginallyclever.com/ @@ -132,7 +132,7 @@ splash.png - com.marginallyclever.robotoverlord.RobotOverlord + com.marginallyclever.ro3.RO3 true diff --git a/src/main/java/com/marginallyclever/ro3/RO3Frame.java b/src/main/java/com/marginallyclever/ro3/RO3Frame.java index 3e5e11d58..eb01ccd1b 100644 --- a/src/main/java/com/marginallyclever/ro3/RO3Frame.java +++ b/src/main/java/com/marginallyclever/ro3/RO3Frame.java @@ -37,7 +37,7 @@ public class RO3Frame extends JFrame { private final List windows = new ArrayList<>(); public RO3Frame() { - super("RO3"); + super("Robot Overlord 3"); setLookAndFeel(); 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 c20ca17d6..a1dcd90de 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/Motor.java @@ -10,6 +10,9 @@ import java.awt.*; import java.util.List; +/** + * a {@link Motor} is a {@link Node} that can be attached to a {@link HingeJoint}. + */ public class Motor extends Node { private HingeJoint hinge; @@ -76,4 +79,8 @@ public HingeJoint getAxle() { public void setAxle(HingeJoint hinge) { this.hinge = hinge; } + + public boolean hasAxle() { + return hinge!=null; + } } diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianFiniteDifferences.java b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianFiniteDifferences.java index b6fd86a56..b531110e0 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianFiniteDifferences.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodes/marlinrobotarm/ApproximateJacobianFiniteDifferences.java @@ -20,7 +20,7 @@ protected ApproximateJacobianFiniteDifferences(MarlinRobotArm arm) { Pose endEffector = arm.getEndEffector(); if(endEffector==null) throw new InvalidParameterException("Robot must have an end effector."); - double[] jointAnglesOriginal = arm.getAllJointValues(); + double[] jointAnglesOriginal = arm.getAllJointAngles(); Matrix4d endEffectorPose = endEffector.getWorld(); Matrix4d endEffectorDifference = new Matrix4d(); Matrix3d endEffectorPoseRotation = new Matrix3d(); @@ -31,7 +31,7 @@ protected ApproximateJacobianFiniteDifferences(MarlinRobotArm arm) { // use anglesB to get the hand matrix after a tiny adjustment on one joint. double[] jointAnglesPlusDelta = Arrays.copyOf(jointAnglesOriginal, jointAnglesOriginal.length); jointAnglesPlusDelta[i] += ANGLE_STEP_SIZE_DEGREES; - arm.setAllJointValues(jointAnglesPlusDelta); + arm.setAllJointAngles(jointAnglesPlusDelta); Matrix4d endEffectorPosePlusDelta = endEffector.getWorld(); // use the finite difference in the two matrixes @@ -59,7 +59,7 @@ protected ApproximateJacobianFiniteDifferences(MarlinRobotArm arm) { jacobian[5][i] = skewSymmetric.m01; } } finally { - arm.setAllJointValues(jointAnglesOriginal); + arm.setAllJointAngles(jointAnglesOriginal); arm.update(0); } } 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 044fedf85..e020bb71c 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 @@ -20,11 +20,20 @@ import javax.vecmath.Vector3d; import java.awt.*; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; import java.util.Objects; /** - * {@link MarlinRobotArm} converts the state of a robot arm into GCode and back. + *

{@link MarlinRobotArm} converts the state of a robot arm into GCode and back.

+ *

In order to work it requires references to:

+ * */ public class MarlinRobotArm extends Node { private static final Logger logger = LoggerFactory.getLogger(MarlinRobotArm.class); @@ -100,19 +109,14 @@ public void getComponents(List list) { addLabelAndComponent(pane, "Target", targetSelector); //TODO add a slider to control linear velocity - JSlider slider = new JSlider(0,30,(int)linearVelocity); + JSlider slider = new JSlider(0,20,(int)linearVelocity); slider.addChangeListener(e-> linearVelocity = slider.getValue()); addLabelAndComponent(pane, "Linear Vel", slider); // Add a text field to send a position to the robot arm. JTextField output = new JTextField(); output.setEditable(false); - pane.add(output); - - // Add a button that displays gcode to the output. - JButton getFKButton = new JButton("Get"); - getFKButton.addActionListener(e-> output.setText(getFKAsGCode()) ); - pane.add(getFKButton); + addLabelAndComponent(pane,"Output",output); // Add a text field that will be sent to the robot arm. JTextField input = new JTextField(); @@ -133,10 +137,10 @@ public void getComponents(List list) { public String getFKAsGCode() { StringBuilder sb = new StringBuilder("G0"); for(Motor motor : motors) { - if(motor!=null) { + if(motor!=null && motor.hasAxle()) { sb.append(" ") .append(motor.getName()) - .append(motor.getAxle().getAngle()); + .append(StringHelper.formatDouble(motor.getAxle().getAngle())); } } return sb.toString(); @@ -151,19 +155,12 @@ public String sendGCode(String gcode) { logger.info("heard "+gcode); if(gcode.startsWith("G0")) { // fast non-linear move (FK) - // parse gcode for motor names and angles - String [] parts = gcode.split("\\s+"); - for(Motor motor : motors) { - if(motor!=null) { - for(String p : parts) { - if(p.startsWith(motor.getName())) { - motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); - } - } - } - } - return "Ok"; - } else if(gcode.equals("pos")) { + return parseG0(gcode); + } else if(gcode.equals("fk")) { + String response = getFKAsGCode(); + logger.info(response); + return "Ok: "+response; + } else if(gcode.equals("ik")) { if(endEffector==null) { logger.error("no end effector"); return "Error: no end effector"; @@ -181,37 +178,69 @@ public String sendGCode(String gcode) { return "Ok: "+response; } else if(gcode.equals("aj")) { ApproximateJacobianFiniteDifferences jacobian = new ApproximateJacobianFiniteDifferences(this); - logger.debug(jacobian.toString()); - return "Ok"; - } else if(gcode.startsWith("G1")) { // linear move - // parse gcode for names and values. names are XYZ for linear, UVW for angular. - // They set the new target position for the end effector. - String [] parts = gcode.split("\\s+"); - double [] cartesian = getCartesianFromWorld(endEffector.getWorld()); - logger.debug("before: "+cartesian[0]+","+cartesian[1]+","+cartesian[2]+","+cartesian[3]+","+cartesian[4]+","+cartesian[5]); - for(String p : parts) { - if(p.startsWith("X")) cartesian[0] = Double.parseDouble(p.substring(1)); - else if(p.startsWith("Y")) cartesian[1] = Double.parseDouble(p.substring(1)); - else if(p.startsWith("Z")) cartesian[2] = Double.parseDouble(p.substring(1)); - else if(p.startsWith("U")) cartesian[3] = Double.parseDouble(p.substring(1)); - else if(p.startsWith("V")) cartesian[4] = Double.parseDouble(p.substring(1)); - else if(p.startsWith("W")) cartesian[5] = Double.parseDouble(p.substring(1)); - } - logger.debug("after: "+cartesian[0]+","+cartesian[1]+","+cartesian[2]+","+cartesian[3]+","+cartesian[4]+","+cartesian[5]); - // set the target position relative to the base of the robot arm - if(target==null) { - logger.error("no target"); - return "Error: no target"; - } - target.setLocal(getReverseCartesianFromWorld(cartesian)); + logger.info(jacobian.toString()); return "Ok"; + } else if(gcode.startsWith("G1")) { + return parseG1(gcode); } logger.error("unknown command"); return "Error: unknown command"; } /** - * + *

G0 rapid non-linear move.

+ *

Parse gcode for motor names and angles, then set the associated joint values directly.

+ * @param gcode GCode command + * @return response from robot arm + */ + private String parseG0(String gcode) { + String [] parts = gcode.split("\\s+"); + for(Motor motor : motors) { + if(motor!=null && motor.hasAxle()) { + for(String p : parts) { + if(p.startsWith(motor.getName())) { + // TODO check for NaN, Infinity, etc + // TODO check new value is in range. + motor.getAxle().setAngle(Double.parseDouble(p.substring(motor.getName().length()))); + break; + } + } + } + } + return "Ok"; + } + + /** + *

G1 Linear move.

+ *

Parse gcode for names and values, then set the new target position.Names are XYZ for linear, UVW for angular. + * Angular values should be in degrees.

+ *

Movement will occur on {@link #update(double)} provided the {@link #linearVelocity} and the update time are + * greater than zero.

+ * @param gcode GCode command + * @return response from robot arm + */ + private String parseG1(String gcode) { + if(target==null) { + logger.error("no target"); + return "Error: no target"; + } + + String [] parts = gcode.split("\\s+"); + double [] cartesian = getCartesianFromWorld(endEffector.getWorld()); + for(String p : parts) { + if(p.startsWith("X")) cartesian[0] = Double.parseDouble(p.substring(1)); + else if(p.startsWith("Y")) cartesian[1] = Double.parseDouble(p.substring(1)); + else if(p.startsWith("Z")) cartesian[2] = Double.parseDouble(p.substring(1)); + else if(p.startsWith("U")) cartesian[3] = Double.parseDouble(p.substring(1)); + else if(p.startsWith("V")) cartesian[4] = Double.parseDouble(p.substring(1)); + else if(p.startsWith("W")) cartesian[5] = Double.parseDouble(p.substring(1)); + } + // set the target position relative to the base of the robot arm + target.setLocal(getReverseCartesianFromWorld(cartesian)); + return "Ok"; + } + + /** * @param cartesian XYZ translation and UVW rotation of the end effector. UVW is in degrees. * @return the matrix that represents the given cartesian position. */ @@ -243,7 +272,7 @@ public int getNumJoints() { return (int)motors.stream().filter(Objects::nonNull).count(); } - public double[] getAllJointValues() { + public double[] getAllJointAngles() { double[] result = new double[getNumJoints()]; int i=0; for(Motor motor : motors) { @@ -254,7 +283,7 @@ public double[] getAllJointValues() { return result; } - public void setAllJointValues(double[] values) { + public void setAllJointAngles(double[] values) { if(values.length!=getNumJoints()) { logger.error("setAllJointValues: one value for every motor"); return; @@ -271,6 +300,22 @@ public void setAllJointValues(double[] values) { } } + public void setAllJointVelocities(double[] values) { + if(values.length!=getNumJoints()) { + logger.error("setAllJointValues: one value for every motor"); + return; + } + int i=0; + for(Motor motor : motors) { + if(motor!=null) { + HingeJoint axle = motor.getAxle(); + if(axle!=null) { + axle.setVelocity(values[i++]); + } + } + } + } + public Pose getEndEffector() { return endEffector; } @@ -286,11 +331,14 @@ public void setTarget(Pose target) { @Override public void update(double dt) { super.update(dt); - if(endEffector==null || target==null || linearVelocity<0.0001) return; + moveTowardsTarget(dt); + } + private void moveTowardsTarget(double dt) { + if(endEffector==null || target==null || linearVelocity<0.0001) return; double[] cartesianVelocity = MatrixHelper.getCartesianBetweenTwoMatrices(endEffector.getWorld(), target.getWorld()); capVectorToMagnitude(cartesianVelocity,linearVelocity*dt); - applyCartesianForceToEndEffector(cartesianVelocity); + moveEndEffectorInCartesianDirection(cartesianVelocity); } /** @@ -316,55 +364,76 @@ public static void capVectorToMagnitude(double[] vector, double maxLen) { } /** - * Applies a cartesian force to the robot, moving it in the direction of the cartesian force. + * Attempts to move the robot arm such that the end effector travels in the direction of the cartesian velocity. * @param cartesianVelocity three linear forces (mm) and three angular forces (degrees). * @throws RuntimeException if the robot cannot be moved in the direction of the cartesian force. */ - public void applyCartesianForceToEndEffector(double[] cartesianVelocity) { + public void moveEndEffectorInCartesianDirection(double[] cartesianVelocity) { double sum = sumCartesianVelocityComponents(cartesianVelocity); if(sum<0.0001) return; if(sum <= 1) { - applySmallCartesianForceToEndEffector(cartesianVelocity); + moveEndEffectorInCartesianDirectionALittle(cartesianVelocity); return; } // split the big move in to smaller moves. int total = (int) Math.ceil(sum); // allocate a new buffer so that we don't smash the original. - double[] cartesianVelocityUnit = new double[cartesianVelocity.length]; - for (int i = 0; i < cartesianVelocity.length; ++i) { - cartesianVelocityUnit[i] = cartesianVelocity[i] / total; - } + double[] cartesianVelocityUnit = Arrays.stream(cartesianVelocity) + .map(v -> v / total) + .toArray(); + /* + // option 1, set joints directly. for (int i = 0; i < total; ++i) { - applySmallCartesianForceToEndEffector(cartesianVelocityUnit); + moveEndEffectorInCartesianDirectionALittle(cartesianVelocityUnit); } + */ + // option 2, set joint velocities. + setJointVelocitiesFromCartesianVelocity(cartesianVelocityUnit); } /** - * Applies a cartesian force to the robot, moving it in the direction of the cartesian force. + *

Attempts to move the robot arm such that the end effector travels in the direction of the cartesian + * velocity.

+ *

This method will only move the robot arm a small amount. It is intended to be called repeatedly to move the + * arm in an iterative fashion.

* @param cartesianVelocity three linear forces (mm) and three angular forces (degrees). * @throws RuntimeException if the robot cannot be moved in the direction of the cartesian force. */ - private void applySmallCartesianForceToEndEffector(double[] cartesianVelocity) { - ApproximateJacobian aj = new ApproximateJacobianFiniteDifferences(this); - //ApproximateJacobian aj = new ApproximateJacobianScrewTheory(robotComponent); + private void moveEndEffectorInCartesianDirectionALittle(double[] cartesianVelocity) { + ApproximateJacobian aj = getJacobian(); try { double[] jointVelocity = aj.getJointForceFromCartesianForce(cartesianVelocity); // uses inverse jacobian // do not make moves for impossible velocities - if(impossibleVelocity(jointVelocity)) return; + if(impossibleVelocity(jointVelocity)) return; // TODO: throw exception instead? - double[] angles = getAllJointValues(); // # dof long + double[] angles = getAllJointAngles(); // # dof long for (int i = 0; i < angles.length; ++i) { - // TODO: set desired velocity in joint motor component, let motor system handle the rest. - // TODO: get next derivative and set acceleration? angles[i] += jointVelocity[i]; } - setAllJointValues(angles); + setAllJointAngles(angles); } catch (Exception e) { throw new RuntimeException(e); } } + private void setJointVelocitiesFromCartesianVelocity(double[] cartesianVelocity) { + ApproximateJacobian aj = getJacobian(); + try { + double[] jointVelocity = aj.getJointForceFromCartesianForce(cartesianVelocity); // uses inverse jacobian + setAllJointVelocities(jointVelocity); + } catch (Exception e) { + throw new RuntimeException(e); + } + } + + private ApproximateJacobian getJacobian() { + // option 1, use finite differences + return new ApproximateJacobianFiniteDifferences(this); + // option 2, use screw theory + //ApproximateJacobian aj = new ApproximateJacobianScrewTheory(robotComponent); + } + /** * @param jointVelocity the joint velocity to check * @return true if the given joint velocity is impossible. diff --git a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java index bf77ca9d9..7bf1c059f 100644 --- a/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java +++ b/src/main/java/com/marginallyclever/ro3/node/nodetreeview/NodeTreeView.java @@ -104,7 +104,7 @@ private void stopListeningTo(Node node) { */ public void scanTree(Node toScan) { if(toScan == null) throw new InvalidParameterException("node is null"); - logger.info("Scanning "+toScan.getAbsolutePath()); + //logger.debug("Scanning "+toScan.getAbsolutePath()); NodeTreeBranch parentBranch = findTreeNode(toScan); if(parentBranch == null) {