diff --git a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java index 6a8da844..36c29945 100644 --- a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java +++ b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/BezierEditor.java @@ -228,11 +228,11 @@ private TransformNR updateLines(CartesianManipulator m, CartesianManipulator p, double xyRot = Math.toDegrees(Math.atan2(cp1Ydiff, cp1XDiff)) - 90; TransformNR az = new TransformNR(0, 0, 0, new RotationNR(0, -xyRot, 0)); - TransformNR reorented = az.times(vect); - // com.neuronrobotics.sdk.common.Log.error("CP1 "+reorented.getX()+" - // "+reorented.getY()+" - // "+reorented.getZ()); - double xzRot = Math.toDegrees(Math.atan2(reorented.getZ(), reorented.getY())); + TransformNR reoriented = az.times(vect); + // com.neuronrobotics.sdk.common.Log.error("CP1 "+reoriented.getX()+" + // "+reoriented.getY()+" + // "+reoriented.getZ()); + double xzRot = Math.toDegrees(Math.atan2(reoriented.getZ(), reoriented.getY())); TransformNR pose = new TransformNR(0, 0, 0, new RotationNR(xzRot, xyRot, 0)); diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java index 1c936e01..6aa0a327 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MobileBasePhysicsManager.java @@ -70,7 +70,8 @@ public void update(float timeStep) { TransformFactory.nrToBullet(new TransformNR(gravity.x, gravity.y, gravity.z, new RotationNR()), gravTrans); TransformFactory.nrToBullet( - new TransformNR(0, 0, 0, orientation.w, orientation.x, orientation.y, orientation.z), orientTrans); + new TransformNR(0, 0, 0, orientation.w, orientation.x, orientation.y, orientation.z), + orientTrans); orientTrans.inverse(); orientTrans.mul(gravTrans); diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MuJoCoPhysicsManager.java b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MuJoCoPhysicsManager.java index 27f23de2..04708be6 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/physics/MuJoCoPhysicsManager.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/physics/MuJoCoPhysicsManager.java @@ -154,9 +154,9 @@ public void controlStep(MuJoCoModelManager manager) { String s = iterator.next(); if (bodyName.contentEquals(s)) { TransformNR tf = mujocoToTransformNR(manager.getBodyPose(s)); - Double xAcceleration = Math.toDegrees(tf.getRotation().getRotationTilt()); - Double yAcceleration = Math.toDegrees(tf.getRotation().getRotationAzimuth()); - Double zAcceleration = Math.toDegrees(tf.getRotation().getRotationElevation()); + Double xAcceleration = Math.toDegrees(tf.getRotation().getRotationTiltRadians()); + Double yAcceleration = Math.toDegrees(tf.getRotation().getRotationAzimuthRadians()); + Double zAcceleration = Math.toDegrees(tf.getRotation().getRotationElevationRadians()); Double rotxAcceleration = 0.0; Double rotyAcceleration = 0.0; diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/physics/TransformFactory.java b/src/main/java/com/neuronrobotics/bowlerstudio/physics/TransformFactory.java index 87a75753..a3d2e7df 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/physics/TransformFactory.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/physics/TransformFactory.java @@ -182,9 +182,9 @@ public static eu.mihosoft.vrl.v3d.Transform nrToCSG(TransformNR nr) { public static TransformNR scale(TransformNR incoming, double scale) { return new TransformNR(incoming.getX() * scale, incoming.getY() * scale, incoming.getZ() * scale, - new RotationNR(Math.toDegrees(incoming.getRotation().getRotationTilt()) * scale, - Math.toDegrees(incoming.getRotation().getRotationAzimuth()) * scale, - Math.toDegrees(incoming.getRotation().getRotationElevation()) * scale)); + new RotationNR(Math.toDegrees(incoming.getRotation().getRotationTiltRadians()) * scale, + Math.toDegrees(incoming.getRotation().getRotationAzimuthRadians()) * scale, + Math.toDegrees(incoming.getRotation().getRotationElevationRadians()) * scale)); } public static TransformNR csgToNR(eu.mihosoft.vrl.v3d.Transform csg) { diff --git a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/FreecadLoader.java b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/FreecadLoader.java index 550e7b2f..e941454e 100644 --- a/src/main/java/com/neuronrobotics/bowlerstudio/scripting/FreecadLoader.java +++ b/src/main/java/com/neuronrobotics/bowlerstudio/scripting/FreecadLoader.java @@ -153,8 +153,9 @@ public static void addSVGToFreeCAD(File freecadModel, File SVG, Transform pose, args.add(freecadModel.getAbsolutePath()); args.add(SVG.getAbsolutePath()); args.add("\"" + nr.getX() + "," + nr.getY() + "," + nr.getZ() + "\""); - args.add("\"" + Math.toDegrees(r.getRotationAzimuth()) + "," + Math.toDegrees(r.getRotationElevation()) - + "," + Math.toDegrees(r.getRotationTilt()) + "\""); + args.add("\"" + Math.toDegrees(r.getRotationAzimuthRadians()) + "," + + Math.toDegrees(r.getRotationElevationRadians()) + "," + Math.toDegrees(r.getRotationTiltRadians()) + + "\""); args.add(name); args.add(bodyName); legacySystemRun(null, export.getAbsoluteFile().getParentFile(), System.out, args);