From d0a628f464b231dd83ac28cc3f28d0eb2810c0a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?B=C3=A1lint=20Hodossy?= Date: Sat, 17 Sep 2022 17:35:09 +0100 Subject: [PATCH] Fix MjWeld using wrong address in Unity plugin --- unity/Runtime/Components/Equality/MjWeld.cs | 2 +- unity/Runtime/Tools/MjEngineTool.cs | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) 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(