[#1392] Split qpos/qvel into translation/attitude StateData

Author: ReeceHumphreys

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJJoint.cpp

@@ -102,31 +102,33 @@ void MJScalarJoint::writeJointStateMessage(uint64_t CurrentSimNanos)
 {
     checkInitialized();
 
+    auto& scene = body.getSpec().getScene();
+
     ScalarJointStateMsgPayload stateOutMsgPayload;
-    auto state = body.getSpec().getScene().getQposState()->getState();
-    stateOutMsgPayload.state = state(this->qposAdr.value());
-    this->stateOutMsg.write(&stateOutMsgPayload, body.getSpec().getScene().moduleID, 0);
+    stateOutMsgPayload.state = scene.qposGet(this->qposAdr.value());
+    this->stateOutMsg.write(&stateOutMsgPayload, scene.moduleID, 0);
 
     ScalarJointStateMsgPayload stateDotOutMsgPayload;
-    auto stateDot = body.getSpec().getScene().getQvelState()->getState();
-    stateDotOutMsgPayload.state = stateDot(this->qvelAdr.value());
-    this->stateDotOutMsg.write(&stateDotOutMsgPayload, body.getSpec().getScene().moduleID, 0);
+    stateDotOutMsgPayload.state = scene.qvelGet(this->qvelAdr.value());
+    this->stateDotOutMsg.write(&stateDotOutMsgPayload, scene.moduleID, 0);
 }
 
 void MJScalarJoint::setPosition(double value)
 {
     checkInitialized();
 
-    body.getSpec().getScene().getQposState()->state(this->qposAdr.value()) = value;
-    body.getSpec().getScene().markKinematicsAsStale();
+    auto& scene = body.getSpec().getScene();
+    scene.qposRef(this->qposAdr.value()) = value;
+    scene.markKinematicsAsStale();
 }
 
 void MJScalarJoint::setVelocity(double value)
 {
     checkInitialized();
 
-    body.getSpec().getScene().getQvelState()->state(this->qvelAdr.value()) = value;
-    body.getSpec().getScene().markKinematicsAsStale();
+    auto& scene = body.getSpec().getScene();
+    scene.qvelRef(this->qvelAdr.value()) = value;
+    scene.markKinematicsAsStale();
 }
 
 MJSingleJointEquality
@@ -139,45 +141,54 @@ void MJFreeJoint::setPosition(const Eigen::Vector3d& position)
 {
     checkInitialized();
 
-    auto& qposState = body.getSpec().getScene().getQposState()->state;
+    auto& scene = body.getSpec().getScene();
     auto i = this->qposAdr.value();
-    qposState.middleRows(i, 3) = position;
+    scene.qposRef(i + 0) = position[0];
+    scene.qposRef(i + 1) = position[1];
+    scene.qposRef(i + 2) = position[2];
 
-    body.getSpec().getScene().markKinematicsAsStale();
+    scene.markKinematicsAsStale();
 }
 
 void MJFreeJoint::setVelocity(const Eigen::Vector3d& velocity)
 {
     checkInitialized();
 
-    auto& qvelState = body.getSpec().getScene().getQvelState()->state;
+    auto& scene = body.getSpec().getScene();
     auto i = this->qvelAdr.value();
-    qvelState.middleRows(i, 3) = velocity;
+    scene.qvelRef(i + 0) = velocity[0];
+    scene.qvelRef(i + 1) = velocity[1];
+    scene.qvelRef(i + 2) = velocity[2];
 
-    body.getSpec().getScene().markKinematicsAsStale();
+    scene.markKinematicsAsStale();
 }
 
 void MJFreeJoint::setAttitude(const Eigen::MRPd& attitude)
 {
     checkInitialized();
 
-    auto& qposState = body.getSpec().getScene().getQposState()->state;
+    auto& scene = body.getSpec().getScene();
     auto i = this->qposAdr.value();
 
     auto mat = attitude.toRotationMatrix();
     auto quat = Eigen::Quaterniond(mat);
 
-    qposState.middleRows(i + 3, 4) = Eigen::Vector4d{quat.w(), quat.x(), quat.y(), quat.z()};
-    body.getSpec().getScene().markKinematicsAsStale();
+    scene.qposRef(i + 3) = quat.w();
+    scene.qposRef(i + 4) = quat.x();
+    scene.qposRef(i + 5) = quat.y();
+    scene.qposRef(i + 6) = quat.z();
+    scene.markKinematicsAsStale();
 }
 
 void MJFreeJoint::setAttitudeRate(const Eigen::Vector3d& attitudeRate)
 {
     checkInitialized();
 
-    auto& qvelState = body.getSpec().getScene().getQvelState()->state;
+    auto& scene = body.getSpec().getScene();
     auto i = this->qvelAdr.value();
-    qvelState.middleRows(i + 3, 3) = attitudeRate;
+    scene.qvelRef(i + 3) = attitudeRate[0];
+    scene.qvelRef(i + 4) = attitudeRate[1];
+    scene.qvelRef(i + 5) = attitudeRate[2];
 
-    body.getSpec().getScene().markKinematicsAsStale();
+    scene.markKinematicsAsStale();
 }

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJQPosAttitudeStateData.cpp

@@ -0,0 +1,68 @@
+/*
+ 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 "MJQPosAttitudeStateData.h"
+
+#include <mujoco/mujoco.h>
+
+#include <stdexcept>
+
+std::unique_ptr<StateData> MJQPosAttitudeStateData::clone() const
+{
+    return std::unique_ptr<StateData>();
+}
+
+void MJQPosAttitudeStateData::configure(int numQuaternions, int numLinearDofs)
+{
+    this->numQuaternions = numQuaternions;
+    this->numLinearDofs  = numLinearDofs;
+
+    int stateSize = 4 * numQuaternions + numLinearDofs;
+    int derivSize = 3 * numQuaternions + numLinearDofs;
+
+    this->state.resize(stateSize, 1);
+    this->stateDeriv.resize(derivSize, 1);
+}
+
+void MJQPosAttitudeStateData::propagateState(double dt, std::vector<double> pseudoStep)
+{
+    // Quaternion blocks: state has 4 elements per quaternion, stateDeriv
+    // has 3 elements per angular velocity.
+    for (int i = 0; i < numQuaternions; ++i) {
+        mju_quatIntegrate(this->state.data() + 4 * i,
+                          this->stateDeriv.data() + 3 * i,
+                          dt);
+    }
+
+    // Linear DOFs follow the quaternion block in both vectors.
+    int linearStart_state = 4 * numQuaternions;
+    int linearStart_deriv = 3 * numQuaternions;
+    for (int i = 0; i < numLinearDofs; ++i) {
+        this->state(linearStart_state + i) +=
+            this->stateDeriv(linearStart_deriv + i) * dt;
+    }
+
+    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, errorMsg.c_str());
+        throw std::invalid_argument(errorMsg);
+    }
+}

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJQPosAttitudeStateData.h

@@ -0,0 +1,84 @@
+/*
+ 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 MJQPOS_ATTITUDE_STATE_DATA_H
+#define MJQPOS_ATTITUDE_STATE_DATA_H
+
+#include "simulation/dynamics/_GeneralModuleFiles/stateData.h"
+
+/**
+ * @brief Position state data for the attitude/hinges portion of an `MJScene`.
+ *
+ * Holds the rotational and scalar-joint position DOFs of an MJScene's qpos
+ * vector, segregated from the free-joint translational DOFs (which live in a
+ * separate plain `StateData`).  The split lets the adaptive integrator scale
+ * tolerances independently for translation (orbital scales) and attitude
+ * (radian-scale): without it, the orbital state norm would dominate the
+ * tolerance computation and either force absurdly small steps everywhere or
+ * (with looser relTol) allow steps too large for the stiff panel-joint ODE.
+ *
+ * Layout convention (assembled by the owning `MJScene`):
+ *
+ *   * `state`      [4 * numQuaternions + numLinearDofs]
+ *       - First `4 * numQuaternions` entries: 4-element unit quaternions, one
+ *         per free-joint rotation or ball joint.
+ *       - Remaining `numLinearDofs` entries: scalar joint positions (hinges
+ *         and slides), one entry each.
+ *   * `stateDeriv` [3 * numQuaternions + numLinearDofs]
+ *       - First `3 * numQuaternions` entries: 3-element angular velocities
+ *         (one per quaternion block, in the same order).
+ *       - Remaining `numLinearDofs` entries: scalar joint velocities.
+ *
+ * `propagateState` integrates the quaternion blocks with `mju_quatIntegrate`
+ * (proper SO(3) update with renormalization) and the linear DOFs with the
+ * default Euler step `state += stateDeriv * dt`.
+ */
+class MJQPosAttitudeStateData : public StateData
+{
+public:
+    /**
+     * @brief Constructs an `MJQPosAttitudeStateData` with the given name and
+     * initial state matrix (typically empty until `configure` runs).
+     */
+    MJQPosAttitudeStateData(std::string inName, const Eigen::MatrixXd& newState)
+        : StateData(std::move(inName), newState) {}
+
+    /** Clone constructor (returns null to match the existing MuJoCo state convention). */
+    std::unique_ptr<StateData> clone() const override;
+
+    /**
+     * @brief Sizes the `state` and `stateDeriv` vectors and records the layout.
+     *
+     * @param numQuaternions Number of 4-element quaternion blocks (free + ball joints).
+     * @param numLinearDofs  Number of 1-element scalar-joint DOFs (hinges + slides).
+     */
+    void configure(int numQuaternions, int numLinearDofs);
+
+    /**
+     * @brief Propagates the state by `dt`.  Quaternions are updated with
+     * `mju_quatIntegrate` and linear DOFs with `state += stateDeriv * dt`.
+     */
+    void propagateState(double dt, std::vector<double> pseudoStep = {}) override;
+
+protected:
+    int numQuaternions = 0;
+    int numLinearDofs  = 0;
+};
+
+#endif