Merge pull request #491 from Balint-H:weld

PiperOrigin-RevId: 479802394
Change-Id: Ieefec4e1ca29fb137947d458837d8ff3c3ee97f3

unity/Runtime/Components/Equality/MjWeld.cs

@@ -28,7 +28,7 @@ namespace Mujoco {
     protected override unsafe void OnBindToRuntime(MujocoLib.mjModel_* model, MujocoLib.mjData_* data) {
       if (WeldOffset) {
         MjEngineTool.SetMjTransform(
-            MjEngineTool.MjTransformAtEntry(model->eq_data, MujocoId),
+            MjEngineTool.MjEqualityAtEntry(model->eq_data, MujocoId)+3,
             WeldOffset.localPosition, WeldOffset.localRotation);
       }
     }

unity/Runtime/Tools/MjEngineTool.cs

@@ -27,6 +27,7 @@ public static class MjEngineTool {
   private const int _elementsPerPosition = 3;
   private const int _elementsPerRotation = 4;
   private const int _elementsPerTransform = 7;
+  private const int _elementsPerEquality = 11;
   private static double[] _mjQuat = new double[4];
   private static double[] _mjMat = new double[9];
 
@@ -106,6 +107,11 @@ public static class MjEngineTool {
     return mjTarget + offsetEntry * _mjMat.Length;
   }
 
+  // Returns a pointer to an entry in the MuJoCo field that's 11*offsetEntry down the buffer.
+  public static unsafe double* MjEqualityAtEntry(double* mjTarget, int offsetEntry) {
+    return mjTarget + offsetEntry * _elementsPerEquality;
+  }
+
   // Stores a unity transform (position+rotation) in a Mujoco target buffer.
   public static unsafe void SetMjTransform(
       double* mjTarget, Vector3 unityVec, Quaternion unityQuat) {