diff --git a/unity/Runtime/Components/Equality/MjWeld.cs b/unity/Runtime/Components/Equality/MjWeld.cs index 2ea700ee..7c628525 100644 --- a/unity/Runtime/Components/Equality/MjWeld.cs +++ b/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); } } diff --git a/unity/Runtime/Tools/MjEngineTool.cs b/unity/Runtime/Tools/MjEngineTool.cs index 159d09f5..261c3491 100644 --- a/unity/Runtime/Tools/MjEngineTool.cs +++ b/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]; @@ -105,6 +106,11 @@ public static class MjEngineTool { public static unsafe double* MjMatrixAtEntry(double* mjTarget, int offsetEntry) { 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(