Support setting pos and vel of MuJoCo body with 3 slider joints

docs/source/Support/bskReleaseNotes.rst

@@ -89,6 +89,8 @@ Version |release|
   both registered their states under the same name, resulting in overwriting and a ``BSK_ERROR``.
 - Added support for :ref:`hingedRigidBodyStateEffector` to be the parent for Dynamic Effectors.
 - Added SWIG Eigen typemaps for passing Eigen products or returning Eigen products to/from director methods.
+- Added support for setting the position and velocity of :ref:`MJBody` with pure translational motion.
+- Added ``getAxis`` and ``isHinge`` to :ref:`MJJoint`.
 
 
 Version 2.8.0 (August 30, 2025)

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJBody.cpp

@@ -27,6 +27,28 @@
 
 #include <iostream>
 
+namespace
+{
+    /** Returns true if the first three scalar joints are translational
+     * and along the main axis ([1,0,0], [0,1,0], [0,0,1]).
+    */
+    bool areJoints3DTranslation(std::list<MJScalarJoint>& joints)
+    {
+        if (joints.size() < 3) return false;
+
+        size_t idx = 0;
+        for (auto&& joint : joints)
+        {
+            if (idx == 3) break;
+            if (joint.isHinge()) return false;
+            if (std::fabs(joint.getAxis()[idx] - 1) > 1e-10) return false;
+            ++idx;
+        }
+
+        return true;
+    }
+}
+
 MJBody::MJBody(mjsBody* body, MJSpec& spec)
     : MJObject(body), spec(spec)
 {
@@ -152,20 +174,40 @@ MJFreeJoint & MJBody::getFreeJoint()
 
 void MJBody::setPosition(const Eigen::Vector3d& position)
 {
-    if (!this->freeJoint.has_value()) {
-        this->getSpec().getScene().logAndThrow<std::runtime_error>("Tried to set position in non-free body " +
+    if (this->freeJoint.has_value()) {
+        this->freeJoint.value().setPosition(position);
+    } else if (areJoints3DTranslation(scalarJoints))
+    {
+        size_t idx = 0;
+        for (auto&& joint : scalarJoints)
+        {
+            if (idx == 3) break;
+            joint.setPosition(position[idx]);
+            ++idx;
+        }
+    } else {
+        this->getSpec().getScene().logAndThrow<std::runtime_error>("Tried to set position in a body with no 'free' joint or no 3D translational joints " +
                                                     name);
     }
-    this->freeJoint.value().setPosition(position);
 }
 
 void MJBody::setVelocity(const Eigen::Vector3d& velocity)
 {
-    if (!this->freeJoint.has_value()) {
-        this->getSpec().getScene().logAndThrow<std::runtime_error>("Tried to set velocity in non-free body " +
+    if (this->freeJoint.has_value()) {
+        this->freeJoint.value().setVelocity(velocity);
+    } else if (areJoints3DTranslation(scalarJoints))
+    {
+        size_t idx = 0;
+        for (auto&& joint : scalarJoints)
+        {
+            if (idx == 3) break;
+            joint.setVelocity(velocity[idx]);
+            ++idx;
+        }
+    } else {
+        this->getSpec().getScene().logAndThrow<std::runtime_error>("Tried to set velocity in a body with no 'free' joint or no 3D translational joints " +
                                                     name);
     }
-    this->freeJoint.value().setVelocity(velocity);
 }
 
 void MJBody::setAttitude(const Eigen::MRPd& attitude)

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJJoint.cpp

@@ -53,7 +53,7 @@ void MJJoint::configure(const mjModel* m)
     this->qvelAdr = m->jnt_dofadr[this->getId()];
 }
 
-void MJJoint::checkInitialized()
+void MJJoint::checkInitialized() const
 {
     if (!this->qposAdr.has_value()) {
         return body.getSpec().getScene().logAndThrow<std::runtime_error>(
@@ -69,6 +69,20 @@ MJScalarJoint::MJScalarJoint(mjsJoint* joint, MJBody& body)
     )
 {}
 
+Eigen::Vector3d MJScalarJoint::getAxis() const
+{
+    checkInitialized();
+    const auto m = this->body.getSpec().getMujocoModel();
+    return Eigen::Vector3d(m->jnt_axis + (this->qposAdr.value() * 3));
+}
+
+bool MJScalarJoint::isHinge() const
+{
+    checkInitialized();
+    const auto m = this->body.getSpec().getMujocoModel();
+    return m->jnt_type[this->qposAdr.value()] == mjJNT_HINGE;
+}
+
 void MJScalarJoint::configure(const mjModel* m)
 {
     MJJoint::configure(m);

src/simulation/mujocoDynamics/_GeneralModuleFiles/MJJoint.h

@@ -89,7 +89,7 @@ class MJJoint : public MJObject<mjsJoint>
      *
      * Throws an exception if initialization has not been completed.
      */
-    void checkInitialized();
+    void checkInitialized() const;
 
 protected:
     MJBody& body; ///< Reference to the body the joint is attached to.
@@ -122,6 +122,20 @@ class MJScalarJoint : public MJJoint
      */
     MJScalarJoint(mjsJoint* joint, MJBody& body);
 
+    /**
+     * @brief Returns the axis of rotation for hinge joints and the direction
+     * of translation for slide joints.
+     *
+     * Returned vector is normalized.
+     */
+    Eigen::Vector3d getAxis() const;
+
+    /**
+     * @brief Returns true if this is a rotational joint, false if it's a
+     * translational slide joint.
+     */
+    bool isHinge() const;
+
     /**
      * @brief Sets the position of the joint.
      *

src/simulation/mujocoDynamics/_GeneralModuleFiles/_UnitTest/test_ball.xml

@@ -3,7 +3,11 @@
 
 	<worldbody>
 		<body name="ball">
-			<freejoint/>
+			<!-- We can use three slider joints for translational-only scenarios -->
+			<joint name="ball_x" type="slide" axis="1 0 0"/>
+			<joint name="ball_y" type="slide" axis="0 1 0"/>
+			<joint name="ball_z" type="slide" axis="0 0 1"/>
+
 			<site name="center" />
 			<geom size="1" rgba="1 0 0 1"/>
 		</body>

src/simulation/mujocoDynamics/_GeneralModuleFiles/_UnitTest/test_mass_update.py

@@ -94,6 +94,10 @@ def test_continuouslyChangingMass(showPlots: bool = False):
     scSim.AddModelToTask("test", posRec)
 
     scSim.InitializeSimulation()
+
+    # initial position should not matter
+    ballBody.setPosition([0, 1, 0])
+
     scSim.ConfigureStopTime(macros.sec2nano(tf))
     scSim.ExecuteSimulation()