Split qpos/qvel into translation/attitude StateData for better integrator tolerances

src/simulation/dynamics/_GeneralModuleFiles/quaternionStateData.cpp

@@ -0,0 +1,97 @@
+/*
+ ISC License
+
+ Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#include "quaternionStateData.h"
+
+#include <cmath>
+#include <stdexcept>
+
+QuaternionStateData::QuaternionStateData(std::string inName,
+                                         const Eigen::MatrixXd& newState)
+    : StateData(std::move(inName), newState)
+{
+    // state holds (w,x,y,z); deriv is the body angular velocity (3x1).
+    this->state.resize(4, 1);
+    this->state.setZero();
+    this->state(0) = 1.0;  // identity quaternion
+    this->stateDeriv.resize(3, 1);
+    this->stateDeriv.setZero();
+}
+
+std::unique_ptr<StateData> QuaternionStateData::clone() const
+{
+    return std::unique_ptr<StateData>();
+}
+
+void QuaternionStateData::propagateState(double dt, std::vector<double> pseudoStep)
+{
+    // Equivalent to mju_quatIntegrate: q <- q * exp(0.5 * dt * omega), normalize.
+    double wx = this->stateDeriv(0) * dt;
+    double wy = this->stateDeriv(1) * dt;
+    double wz = this->stateDeriv(2) * dt;
+    double angle = std::sqrt(wx * wx + wy * wy + wz * wz);
+
+    double dq_w, dq_x, dq_y, dq_z;
+    if (angle < 1e-12) {
+        // Small-angle limit avoids division by zero; sin(a/2)/a -> 1/2.
+        dq_w = 1.0;
+        dq_x = 0.5 * wx;
+        dq_y = 0.5 * wy;
+        dq_z = 0.5 * wz;
+    } else {
+        double half  = 0.5 * angle;
+        double scale = std::sin(half) / angle;
+        dq_w = std::cos(half);
+        dq_x = scale * wx;
+        dq_y = scale * wy;
+        dq_z = scale * wz;
+    }
+
+    double qw = this->state(0);
+    double qx = this->state(1);
+    double qy = this->state(2);
+    double qz = this->state(3);
+
+    // Hamilton product q' = q ⊗ dq (body-frame angular velocity convention).
+    double nw = qw * dq_w - qx * dq_x - qy * dq_y - qz * dq_z;
+    double nx = qw * dq_x + qx * dq_w + qy * dq_z - qz * dq_y;
+    double ny = qw * dq_y - qx * dq_z + qy * dq_w + qz * dq_x;
+    double nz = qw * dq_z + qx * dq_y - qy * dq_x + qz * dq_w;
+
+    double norm = std::sqrt(nw * nw + nx * nx + ny * ny + nz * nz);
+    if (norm > 0.0) {
+        double inv = 1.0 / norm;
+        nw *= inv; nx *= inv; ny *= inv; nz *= inv;
+    } else {
+        nw = 1.0; nx = 0.0; ny = 0.0; nz = 0.0;
+    }
+
+    this->state(0) = nw;
+    this->state(1) = nx;
+    this->state(2) = ny;
+    this->state(3) = nz;
+
+    if (getNumNoiseSources() > 0 && pseudoStep.size() == 0) {
+        auto errorMsg = "State " + this->getName() + " has stochastic dynamics, but "
+            + "the integrator tried to propagate it without pseudoSteps. Are you sure "
+            + "you are using a stochastic integrator?";
+        bskLogger.bskLog(BSK_ERROR, "%s", errorMsg.c_str());
+        throw std::invalid_argument(errorMsg);
+    }
+}

src/simulation/dynamics/_GeneralModuleFiles/quaternionStateData.h

@@ -0,0 +1,59 @@
+/*
+ ISC License
+
+ Copyright (c) 2026, Autonomous Vehicle Systems Lab, University of Colorado at Boulder
+
+ Permission to use, copy, modify, and/or distribute this software for any
+ purpose with or without fee is hereby granted, provided that the above
+ copyright notice and this permission notice appear in all copies.
+
+ THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+ */
+
+#ifndef QUATERNION_STATE_DATA_H
+#define QUATERNION_STATE_DATA_H
+
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "stateData.h"
+
+/**
+ * @brief A `StateData` whose value is a unit quaternion advanced from a body
+ * angular velocity.
+ *
+ * `state` is a 4x1 unit quaternion in `(w, x, y, z)` order.
+ * `stateDeriv` is a 3x1 body angular velocity in rad/s.
+ *
+ * Sizes intentionally differ — the standard Euler step `state += deriv * dt`
+ * is not valid on SO(3).  `propagateState` instead applies the exact
+ * exponential-map integrator and renormalizes so `|q| = 1` after each step.
+ */
+class QuaternionStateData : public StateData
+{
+public:
+    /** Constructs a 4x1 quaternion state with a 3x1 derivative. */
+    QuaternionStateData(std::string inName, const Eigen::MatrixXd& newState);
+
+    /** Quaternion states are owned and seeded by their joint; cloning returns null. */
+    std::unique_ptr<StateData> clone() const override;
+
+    /**
+     * @brief Integrates `state` over `dt` using the body angular velocity in
+     * `stateDeriv`.  Equivalent to MuJoCo's `mju_quatIntegrate`: composes the
+     * current quaternion with `exp(0.5 * dt * omega)` and renormalizes.
+     */
+    void propagateState(double dt, std::vector<double> pseudoStep = {}) override;
+};
+
+#endif

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJBody.cpp

@@ -252,6 +252,40 @@ void MJBody::writeStateDependentOutputMessages(uint64_t CurrentSimNanos)
 void MJBody::registerStates(DynParamRegisterer paramManager)
 {
     this->massState = paramManager.registerState(1, 1, "mass");
+
+    // Each joint owns its own qpos/qvel state(s).  Registering here puts
+    // them all under this body's prefix so the manager-wide names stay
+    // unique even when bodies have joints with the same XML name.
+    for (auto&& joint : this->scalarJoints) {
+        joint.registerStates(paramManager);
+    }
+    if (this->ballJoint.has_value()) {
+        this->ballJoint->registerStates(paramManager);
+    }
+    if (this->freeJoint.has_value()) {
+        this->freeJoint->registerStates(paramManager);
+    }
+}
+
+void MJBody::syncJointStatesToMujoco(mjData* d) const
+{
+    for (auto&& joint : this->scalarJoints) joint.writeStateToMujoco(d);
+    if (this->ballJoint.has_value()) this->ballJoint->writeStateToMujoco(d);
+    if (this->freeJoint.has_value()) this->freeJoint->writeStateToMujoco(d);
+}
+
+void MJBody::seedJointStatesFromMujoco(const mjData* d)
+{
+    for (auto&& joint : this->scalarJoints) joint.readStateFromMujoco(d);
+    if (this->ballJoint.has_value()) this->ballJoint->readStateFromMujoco(d);
+    if (this->freeJoint.has_value()) this->freeJoint->readStateFromMujoco(d);
+}
+
+void MJBody::setJointDerivativesFromMujoco(const mjData* d)
+{
+    for (auto&& joint : this->scalarJoints) joint.setDerivativesFromMujoco(d);
+    if (this->ballJoint.has_value()) this->ballJoint->setDerivativesFromMujoco(d);
+    if (this->freeJoint.has_value()) this->freeJoint->setDerivativesFromMujoco(d);
 }
 
 void MJBody::updateMujocoModelFromMassProps()

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJBody.h

@@ -253,14 +253,28 @@ class MJBody : public MJObject<mjsBody>
     void writeStateDependentOutputMessages(uint64_t CurrentSimNanos);
 
     /**
-     * @brief Registers the body's states with a dynamic parameter registerer.
-     *
-     * Currently, only the mass of the body is considered a parameter.
-     *
-     * @param paramManager The dynamic parameter registerer.
+     * @brief Registers the body's mass state and the qpos/qvel states of all
+     * joints attached to this body.  Each joint owns its own `StateData` so
+     * the adaptive integrator can scale tolerances independently per joint.
      */
     void registerStates(DynParamRegisterer paramManager);
 
+    /** Pushes the qpos/qvel values from each owned joint's state into `mjData`. */
+    void syncJointStatesToMujoco(mjData* d) const;
+
+    /**
+     * Seeds each owned joint's state from `mjData::qpos`/`mjData::qvel`.
+     * Called once after spec compile so the user-visible state matches the
+     * values declared in the XML before any setPosition/setVelocity calls.
+     */
+    void seedJointStatesFromMujoco(const mjData* d);
+
+    /**
+     * Sets each owned joint's qpos/qvel state derivatives from the current
+     * `mjData::qvel`/`mjData::qacc`.
+     */
+    void setJointDerivativesFromMujoco(const mjData* d);
+
     /**
      * @brief Updates the MuJoCo model from the mass properties of the body.
      *

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJBody.swg

@@ -33,6 +33,9 @@
 %ignore MJBody::updateMujocoModelFromMassProps;
 %ignore MJBody::updateMassPropsDerivative;
 %ignore MJBody::updateConstrainedEqualityJoints;
+%ignore MJBody::syncJointStatesToMujoco;
+%ignore MJBody::seedJointStatesFromMujoco;
+%ignore MJBody::setJointDerivativesFromMujoco;
 
 %extend MJBody {
 MJScene& getScene() { return $self->getSpec().getScene(); }