From e95b0c0eaf043690519af3f8ba67879ed61e211e Mon Sep 17 00:00:00 2001
From: Tom Erez <etom@google.com>
Date: Thu, 15 Dec 2022 07:25:16 -0800
Subject: [PATCH] Add anchor support for connect constraint components.

Fixes #647

PiperOrigin-RevId: 495587232
Change-Id: I528a69dabbc1ac9a12f9ab7c16716e3d69c635e2
---
 unity/Runtime/Components/Equality/MjConnect.cs | 11 ++++++++---
 1 file changed, 8 insertions(+), 3 deletions(-)

diff --git a/unity/Runtime/Components/Equality/MjConnect.cs b/unity/Runtime/Components/Equality/MjConnect.cs
index efdb93a1..23343089 100644
--- a/unity/Runtime/Components/Equality/MjConnect.cs
+++ b/unity/Runtime/Components/Equality/MjConnect.cs
@@ -22,14 +22,17 @@ namespace Mujoco {
   public class MjConnect : MjBaseConstraint {
     public MjBaseBody Body1;
     public MjBaseBody Body2;
+    public Transform Anchor;
     protected override string _constraintName => "connect";
 
     protected override void FromMjcf(XmlElement mjcf) {
       Body1 = mjcf.GetObjectReferenceAttribute<MjBaseBody>("body1");
       Body2 = mjcf.GetObjectReferenceAttribute<MjBaseBody>("body2");
-      if (mjcf.GetStringAttribute("anchor") != null) {
-        Debug.Log($"anchor in connect {name} ignored. Set Transforms in the editor.");
-      }
+      var anchorPos = MjEngineTool.UnityVector3(
+          mjcf.GetVector3Attribute("anchor", defaultValue: Vector3.zero));
+      Anchor = new GameObject("connect_anchor").transform;
+      Anchor.parent = Body1.transform;
+      Anchor.localPosition = anchorPos;
     }
 
     // Generate implementation specific XML element.
@@ -39,6 +42,8 @@ namespace Mujoco {
       }
       mjcf.SetAttribute("body1", Body1.MujocoName);
       mjcf.SetAttribute("body2", Body2.MujocoName);
+      mjcf.SetAttribute("anchor",
+                        MjEngineTool.Vector3ToMjcf(MjEngineTool.MjVector3(Anchor.localPosition)));
     }
 
     public void OnValidate() {