Merge pull request #131 from DeepBlueRobotics/add-PoweredHinge2Joint

brettle · web-flow · commit 25537e0e9524 · 2024-07-29T20:40:39.000-07:00
Add PoweredHinge2Joint
diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml
@@ -32,9 +32,11 @@ jobs:
 
     - name: Setup Webots
       id: setupWebots
-      uses: DeepBlueRobotics/setup-webots@v2.0.3
+      uses: DeepBlueRobotics/setup-webots@v2.1
       with:
-        webotsVersion: R2023b
+        webotsVersion: R2024a
+        webotsRepository: DeepBlueRobotics/webots
+        webotsTag: R2024a_DeepBlueSim_2024_07_29
 
     - name: Do the system test
       uses: ./.github/actions/run-system-test
@@ -80,9 +82,11 @@ jobs:
 
     - name: Setup Webots
       id: setupWebots
-      uses: DeepBlueRobotics/setup-webots@v2
+      uses: DeepBlueRobotics/setup-webots@v2.1
       with:
-        webotsVersion: R2023b
+        webotsVersion: R2024a
+        webotsRepository: DeepBlueRobotics/webots
+        webotsTag: R2024a_DeepBlueSim_2024_07_29
 
     - name: Checkout source
       uses: actions/checkout@v4
diff --git a/README.md b/README.md
@@ -22,7 +22,7 @@ advantage of the WPILib's WebSockets server desktop simulation extension.
 
 ### Installation
 
- 1. Install Webots if you don't already have it installed.
+ 1. Install the latest [official nightly build of Webots](https://github.com/cyberbotics/webots/releases).
  1. Add the DeepBlueSim Gradle plugin to your `build.gradle` by adding the following line
  in your `plugins` section:
  ```
diff --git a/example/src/systemTest/java/frc/robot/SystemTestRobot.java b/example/src/systemTest/java/frc/robot/SystemTestRobot.java
@@ -28,7 +28,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception {
                         0.1, "Robot close to target velocity");
                 assertEquals(0.0,
                         Units.radiansToDegrees(
-                                s.angularVelocity("ROBOT").getAngle()),
+                                s.angularVelocity("ROBOT").norm()),
                         1, "Robot close to target angular velocity");
             }).atSec(3.0, s -> {
                 assertEquals(0.0,
@@ -45,7 +45,7 @@ void testDrivesToLocationAndElevatesInAutonomous() throws Exception {
                         0.05, "Robot close to target velocity");
                 assertEquals(0.0,
                         Units.radiansToDegrees(
-                                s.angularVelocity("ROBOT").getAngle()),
+                                s.angularVelocity("ROBOT").norm()),
                         1, "Robot close to target angular velocity");
             }).run();
         }
@@ -78,10 +78,10 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception {
                                 .getDistance(new Translation3d(0, 0, 0)),
                         0.1, "Robot close to target velocity");
                 assertEquals(63.9, Units.radiansToDegrees(
-                                s.angularVelocity("ROBOT").getAngle()),
-                        5.0, "Robot close to target angular velocity");
+                        s.angularVelocity("ROBOT").norm()), 5.0,
+                        "Robot close to target angular velocity");
                 assertEquals(0.0,
-                        new Translation3d(s.angularVelocity("ROBOT").getAxis())
+                        new Translation3d(s.angularVelocity("ROBOT").unit())
                                 .getDistance(new Translation3d(0, 0, 1)),
                         0.1, "Robot close to target angular velocity axis");
             }).atSec(1.5, s -> {
@@ -96,7 +96,7 @@ void testCanBeRotatedInPlaceInTeleop() throws Exception {
                         0.1, "Robot close to target velocity");
                 assertEquals(0.0,
                         Units.radiansToDegrees(
-                                s.angularVelocity("ROBOT").getAngle()),
+                                s.angularVelocity("ROBOT").norm()),
                         1, "Robot close to target angular velocity");
             }).run();
         }
diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHinge2Joint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHinge2Joint.proto
@@ -0,0 +1,48 @@
+#VRML_SIM R2023b utf8
+# license: MIT
+# license url: https://github.com/DeepBlueRobotics/DeepBlueSim/blob/master/LICENSE.md
+# template language: javascript
+# A HingeJoint powered by a DeepBlueSim-compatible motor
+
+EXTERNPROTO "DBSRotationalMotorBase.proto"
+EXTERNPROTO "PreconfiguredDBSRotationalMotor.proto"
+
+EXTERNPROTO "CANCoder.proto"
+EXTERNPROTO "DBSQuadratureEncoder.proto"
+EXTERNPROTO "REVBuiltinEncoder.proto"
+EXTERNPROTO "SparkMaxAbsoluteEncoder.proto"
+EXTERNPROTO "SparkMaxAlternateEncoder.proto"
+EXTERNPROTO "SparkMaxAnalogSensor.proto"
+
+EXTERNPROTO "DBSBrake.proto"
+
+PROTO PoweredHinge2Joint [
+    field MFNode{
+        DBSRotationalMotorBase{}, PreconfiguredDBSRotationalMotor{}
+        CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{},
+        DBSBrake{}
+    } devices [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} DBSBrake{}]
+    field MFNode{
+        DBSRotationalMotorBase{}, PreconfiguredDBSRotationalMotor{}
+        CANCoder{}, DBSQuadratureEncoder{}, REVBuiltinEncoder{}, SparkMaxAbsoluteEncoder{}, SparkMaxAlternateEncoder{}, SparkMaxAnalogSensor{},
+        DBSBrake{}
+    } devices2 [PreconfiguredDBSRotationalMotor{} REVBuiltinEncoder{} DBSBrake{}]
+    field SFNode{Solid{}} endPoint Solid{}
+    field SFNode{HingeJointParameters{}} jointParameters HingeJointParameters{}
+    field SFNode{JointParameters{}} jointParameters2 JointParameters{}
+]
+{
+    %<
+        import {assert} from 'wbutility.js';
+        let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`));
+        dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredHinge2Joint.devices", assert);
+        dbsutility.assertValidPoweredJointDevices(fields.devices2.value, "PoweredHinge2Joint.devices2", assert);
+    >%
+    Hinge2Joint {
+        device IS devices
+        device2 IS devices2
+        endPoint IS endPoint
+        jointParameters IS jointParameters
+        jointParameters2 IS jointParameters2
+    }
+}
diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredHingeJoint.proto
@@ -28,31 +28,9 @@ PROTO PoweredHingeJoint [
 {
     %<
         import {assert} from 'wbutility.js';
-        let devices = fields.devices.value;
-        let hasBrake = false;
-        let motorDevice = null;
-        let encoderDevice = null;
-        for (let i = 0; i < devices.length; i++) {
-            let node_name = devices[i].node_name;
-            if (node_name.endsWith("Brake")) {
-                hasBrake = true;
-            } else if (node_name.endsWith("Motor")) {
-                motorDevice = devices[i];
-            } else if (node_name.endsWith("Encoder")) {
-                encoderDevice = devices[i];
-            }
-        }
-        assert(hasBrake, "PoweredHingeJoint is missing the required DBSBrake device!");
-        assert(motorDevice !== null, "PoweredHingeJoint is missing the required motor device!");
-        let motorType = motorDevice?.node_name;
-        let controllerType = motorDevice?.fields.controllerType.value;
-        if (controllerType?.startsWith("Spark")) {
-            assert(encoderDevice !== null, "PoweredHingeJoint is using a " + controllerType + " motor controller but is missing the required encoder device! "
-                + "Consider adding a REVBuiltinEncoder.");
-        } else if (controllerType === "PWM") {
-            let encoderType = encoderDevice?.node_name;
-            assert(!encoderType?.endsWith("BuiltinEncoder"), "PoweredHingeJoint is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!");
-        }
+        let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`));
+        dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredHingeJoint.devices", assert);
+
     >%
     HingeJoint {
         device IS devices
diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/PoweredSliderJoint.proto
@@ -28,31 +28,8 @@ PROTO PoweredSliderJoint [
 {
     %<
         import {assert} from 'wbutility.js';
-        let devices = fields.devices.value;
-        let hasBrake = false;
-        let motorDevice = null;
-        let encoderDevice = null;
-        for (let i = 0; i < devices.length; i++) {
-            let node_name = devices[i].node_name;
-            if (node_name.endsWith("Brake")) {
-                hasBrake = true;
-            } else if (node_name.endsWith("Motor")) {
-                motorDevice = devices[i];
-            } else if (node_name.endsWith("Encoder")) {
-                encoderDevice = devices[i];
-            }
-        }
-        assert(hasBrake, "PoweredSliderJoint is missing the required DBSBrake device!");
-        assert(motorDevice !== null, "PoweredSliderJoint is missing the required motor device!");
-        let motorType = motorDevice?.node_name;
-        let controllerType = motorDevice?.fields.controllerType.value;
-        if (controllerType?.startsWith("Spark")) {
-            assert(encoderDevice !== null, "PoweredSliderJoint is using a " + controllerType + " motor controller but is missing the required encoder device! "
-                + "Consider adding a REVBuiltinEncoder.");
-        } else if (controllerType === "PWM") {
-            let encoderType = encoderDevice?.node_name;
-            assert(!encoderType?.endsWith("BuiltinEncoder"), "PoweredSliderJoint is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!");
-        }
+        let dbsutility = eval(wbfile.readTextFile(`${context.project_path}protos/deepbluesim/dbsutility.js`));
+        dbsutility.assertValidPoweredJointDevices(fields.devices.value, "PoweredSliderJoint.devices", assert);
     >%
     SliderJoint {
         device IS devices
diff --git a/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsutility.js b/plugin/controller/src/webotsFolder/dist/protos/deepbluesim/dbsutility.js
@@ -0,0 +1,28 @@
+({
+    assertValidPoweredJointDevices: function (devices, fieldDesc, assert) {
+        let hasBrake = false;
+        let motorDevice = null;
+        let encoderDevice = null;
+        for (let i = 0; i < devices.length; i++) {
+            let node_name = devices[i].node_name;
+            if (node_name.endsWith("Brake")) {
+                hasBrake = true;
+            } else if (node_name.endsWith("Motor")) {
+                motorDevice = devices[i];
+            } else if (node_name.endsWith("Encoder")) {
+                encoderDevice = devices[i];
+            }
+        }
+        assert(hasBrake, fieldDesc + " is missing the required DBSBrake device!");
+        assert(motorDevice !== null, fieldDesc + " is missing the required motor device!");
+        let motorType = motorDevice?.node_name;
+        let controllerType = motorDevice?.fields.controllerType.value;
+        if (controllerType?.startsWith("Spark")) {
+            assert(encoderDevice !== null, fieldDesc + " is using a " + controllerType + " motor controller but is missing the required encoder device! "
+                + "Consider adding a REVBuiltinEncoder.");
+        } else if (controllerType === "PWM") {
+            let encoderType = encoderDevice?.node_name;
+            assert(!encoderType?.endsWith("BuiltinEncoder"), fieldDesc + " is using a " + controllerType + " motor controller, so a built-in encoder is not allowed!");
+        }
+    },
+})
diff --git a/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg b/plugin/controller/src/webotsFolder/dist/worlds/.MotorController.jpg
diff --git a/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt b/plugin/controller/src/webotsFolder/dist/worlds/MotorController.wbt
@@ -1,4 +1,4 @@
-#VRML_SIM R2023b utf8
+#VRML_SIM R2024a utf8
 
 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto"
 EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
@@ -7,12 +7,13 @@ EXTERNPROTO "../protos/deepbluesim/PoweredHingeJoint.proto"
 EXTERNPROTO "../protos/deepbluesim/PreconfiguredDBSRotationalMotor.proto"
 EXTERNPROTO "../protos/deepbluesim/DBSBrake.proto"
 EXTERNPROTO "../protos/deepbluesim/REVBuiltinEncoder.proto"
+EXTERNPROTO "../protos/deepbluesim/PoweredHinge2Joint.proto"
 
 WorldInfo {
 }
 Viewpoint {
   orientation -0.24402898166491363 0.33039742727224586 0.9117496345814823 1.5165266004776918
-  position -0.6005906535473244 -4.740227792969629 3.113219051699273
+  position -1.136298742206523 -8.781932059872242 5.731292223532602
 }
 TexturedBackground {
 }
@@ -60,6 +61,78 @@ DEF ROBOT Robot {
         }
       ]
     }
+    DEF HINGE2JOINT Solid {
+      translation 0 -2 0
+      rotation 0 1 0 0
+      children [
+        DEF HINGE2JOINT_BASE Pose {
+          children [
+            Shape {
+              appearance PBRAppearance {
+              }
+              geometry Box {
+                size 1 1 0.1
+              }
+            }
+          ]
+        }
+        PoweredHinge2Joint {
+          devices [
+            PreconfiguredDBSRotationalMotor {
+              motorType "MiniCIM"
+              controllerType "PWM"
+              port 2
+              gearing 97.3333
+            }
+            DBSBrake {
+            }
+          ]
+          devices2 [
+            PreconfiguredDBSRotationalMotor {
+              motorType "MiniCIM"
+              controllerType "PWM"
+              port 3
+              gearing 97.3333
+            }
+            DBSBrake {
+            }
+          ]
+          endPoint DEF HINGE2JOINT_SHAFT Solid {
+            translation 0 0 2
+            children [
+              DEF HINGE2JOINT_SHAFT_GEOM Pose {
+                rotation 0 1 0 1.5701
+                children [
+                  Shape {
+                    appearance PBRAppearance {
+                    }
+                    geometry Cylinder {
+                      height 0.215
+                      radius 0.5
+                      subdivision 8
+                    }
+                  }
+                ]
+              }
+            ]
+            boundingObject USE HINGE2JOINT_SHAFT_GEOM
+            physics Physics {
+            }
+          }
+          jointParameters HingeJointParameters {
+            axis 0 1 0
+            anchor 0 0 2
+          }
+          jointParameters2 JointParameters {
+            axis 1 0 0
+          }
+        }
+      ]
+      name "solid(3)"
+      boundingObject USE HINGE2JOINT_BASE
+      physics Physics {
+      }
+    }
     DEF NEO_BASE Solid {
       translation 0 2 0
       children [
diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/Watcher.java
@@ -9,8 +9,11 @@
 
 import org.carlmontrobotics.libdeepbluesim.internal.NTConstants;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoubleArrayTopic;
 import edu.wpi.first.networktables.NetworkTable;
@@ -40,7 +43,8 @@ class Watcher {
     private volatile CompletableFuture<Void> velocityReady =
             new CompletableFuture<>();
     private volatile Translation3d position = null, velocity = null;
-    private volatile Rotation3d rotation = null, angularVelocity = null;
+    private volatile Vector<N3> angularVelocity = null;
+    private volatile Rotation3d rotation = null;
     private final NetworkTableInstance inst;
     private final String defPath;
     private final NetworkTable table;
@@ -106,8 +110,8 @@ private Watcher(String defPath) {
                                 value.valueData.value.getDoubleArray();
                         velocity = new Translation3d(velAsArray[0],
                                 velAsArray[1], velAsArray[2]);
-                        angularVelocity = new Rotation3d(velAsArray[3],
-                                velAsArray[4], velAsArray[5]);
+                        angularVelocity = VecBuilder.fill(velAsArray[3],
+                                        velAsArray[4], velAsArray[5]);
                         velocityReady.complete(null);
                         break;
                 }
@@ -204,7 +208,7 @@ Rotation3d getRotation() {
      * 
      * @return the node's angular velocity.
      */
-    Rotation3d getAngularVelocity() {
+    Vector<N3> getAngularVelocity() {
         if (!inst.isConnected()) {
             LOG.log(Level.DEBUG,
                     "NetworkTables is not connected, so starting server");
diff --git a/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java b/plugin/libdeepbluesim/src/main/java/org/carlmontrobotics/libdeepbluesim/WebotsSimulator.java
@@ -31,8 +31,10 @@
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.networktables.LogMessage;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEvent.Kind;
@@ -673,11 +675,11 @@ public Rotation3d rotation(String defPath) {
 
         /**
          * Gets the angular velocity of a specific node in the simulated world.
-         * 
+         *
          * @param defPath the DEF path to the Webots node to get the position of.
          * @return the angular velocity of the requested node.
          */
-        public Rotation3d angularVelocity(String defPath) {
+        public Vector<N3> angularVelocity(String defPath) {
             // ntTransientLogLevel = LogMessage.kDebug4;
             try {
                 var watcher = Watcher.getByDefPath(defPath);
diff --git a/tests/src/main/java/frc/robot/MotorControllerRobot.java b/tests/src/main/java/frc/robot/MotorControllerRobot.java
diff --git a/tests/src/systemTest/java/frc/robot/DBSExampleTest.java b/tests/src/systemTest/java/frc/robot/DBSExampleTest.java
diff --git a/tests/src/systemTest/java/frc/robot/MotorControllerTest.java b/tests/src/systemTest/java/frc/robot/MotorControllerTest.java
