Add user sensor bindings.

Stop force-setting nusersensor, instead allow MuJoCo to infer the size.

PiperOrigin-RevId: 494783883
Change-Id: Ia2a1b1803b2904f2dded7b790d01eb6f1920dbaf

unity/Runtime/Components/MjScene.cs

@@ -401,12 +401,17 @@ public class MjScene : MonoBehaviour {
     MjRoot.AppendChild(GenerateMjcfSection(
         doc, components.Where(component => component is MjBaseConstraint), "equality"));
 
-    MjRoot.AppendChild(GenerateMjcfSection(
-        doc, components.Where(component => component is MjActuator), "actuator"));
-
-    MjRoot.AppendChild(GenerateMjcfSection(
-        doc, components.Where(component => component is MjBaseSensor), "sensor"));
+    MjRoot.AppendChild(
+        GenerateMjcfSection(doc,
+                            components.Where(component => component is MjActuator)
+                                .OrderBy(component => component.transform.GetSiblingIndex()),
+                            "actuator"));
 
+    MjRoot.AppendChild(
+        GenerateMjcfSection(doc,
+                            components.Where(component => component is MjBaseSensor)
+                                .OrderBy(component => component.transform.GetSiblingIndex()),
+                            "sensor"));
     // Generate the Mjcf of the runtime dependencies added to the context.
     _generationContext.GenerateMjcf(MjRoot);
     return doc;

unity/Runtime/Components/Sensors/MjUserSensor.cs

@@ -0,0 +1,46 @@
+// Copyright 2019 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//      http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+using System.Linq;
+using System.Xml;
+using UnityEngine;
+
+namespace Mujoco {
+
+public class MjUserSensor : MjBaseSensor {
+
+  public String Name;
+  public int Dimension = 0;
+  public String UserData;
+
+  protected override XmlElement ToMjcf(XmlDocument doc) {
+    if (Dimension == 0) {
+      throw new MissingFieldException("Dimension should be larger than 0.");
+    }
+    var mjcf = doc.CreateElement("user");
+    mjcf.SetAttribute("name", Name);
+    mjcf.SetAttribute("dim", $"{Dimension}");
+    // TODO: add validation that UserData is a space-separated list of floating numbers?
+    mjcf.SetAttribute("user", UserData);
+    return mjcf;
+  }
+
+  protected override void FromMjcf(XmlElement mjcf) {
+    Name = mjcf.GetAttribute("name");
+    int.TryParse(mjcf.GetAttribute("dim"), out Dimension);
+    UserData = mjcf.GetAttribute("user");
+  }
+}
+}

unity/Runtime/Components/Sensors/MjUserSensor.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a0b10d0a10ca3ad8cb5fa2b6bcd331c1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

unity/Runtime/Importer/MjcfImporter.cs

@@ -376,6 +376,9 @@ public class MjcfImporter {
             break;
         }
         break;
+      case "user":
+        sensorType = typeof(MjUserSensor);
+        break;
       default:
         Debug.Log($"The importer does not yet support sensor <{node.Name}>.");
         break;

unity/Runtime/Tools/MjcfGenerationContext.cs

@@ -72,12 +72,9 @@ public class MjcfGenerationContext {
         "gravity", MjEngineTool.Vector3ToMjcf(MjEngineTool.MjVector3(Physics.gravity)));
     optionMjcf.SetAttribute("timestep", MjEngineTool.MakeLocaleInvariant($"{Time.fixedDeltaTime}"));
 
-    var sizeMjcf = (XmlElement)mjcf.AppendChild(doc.CreateElement("size"));
-    sizeMjcf.SetAttribute("nuser_sensor", $"{_nuserSensor}");
-
     var settings = MjGlobalSettings.Instance;
     if (settings) {
-      settings.OptionSizeToMjcf(optionMjcf, sizeMjcf);
+      settings.OptionSizeToMjcf(optionMjcf, null);
     }
   }
 

unity/Tests/Editor/Components/MjSceneTests.cs

@@ -99,12 +99,25 @@ public class MjSceneGenerationTests {
 
   [Test]
   public void SensorsAreAddedToDedicatedTag() {
-    _sensor.enabled = true;
+    _sensor1.enabled = true;
     var mjcf = _scene.CreateScene(skipCompile:true);
     var mjcfForSensor = mjcf.SelectNodes("/mujoco/sensor/jointpos")[0] as XmlElement;
     Assert.That(mjcfForSensor, Is.Not.Null);
   }
 
+  [Test]
+  public void SensorsAreOrdered() {
+    _sensor0.enabled = true;
+    _sensor1.enabled = true;
+    var mjcf = _scene.CreateScene(skipCompile:true);
+    var sensor0Element =
+        mjcf.SelectNodes("/mujoco/sensor")[0].ChildNodes[_sensor0.transform.GetSiblingIndex()] as XmlElement;
+    Assert.That(sensor0Element.Name, Is.EqualTo("user"));
+    var sensor1Element =
+        mjcf.SelectNodes("/mujoco/sensor")[0].ChildNodes[_sensor1.transform.GetSiblingIndex()] as XmlElement;
+    Assert.That(sensor1Element.Name, Is.EqualTo("jointpos"));
+  }
+
   [Test]
   public unsafe void SceneRecreatedWithAddition() {
     _scene.CreateScene();
@@ -168,7 +181,9 @@ public class MjSceneGenerationTests {
   private MjScene _scene;
   private FakeMjBody _fakeBodyA;
   private FakeMjBody _fakeBodyB;
-  private MjJointScalarSensor _sensor;
+  private Transform _sensorGroup;
+  private MjUserSensor _sensor0;
+  private MjJointScalarSensor _sensor1;
   private MjBody _body;
   private MjInertial _inertia;
   private MjHingeJoint _joint;
@@ -181,7 +196,12 @@ public class MjSceneGenerationTests {
     _scene = MjScene.Instance;
     _fakeBodyA = new GameObject("component").AddComponent<FakeMjBody>();
     _fakeBodyB = new GameObject("component").AddComponent<FakeMjBody>();
-    _sensor = new GameObject("sensor").AddComponent<MjJointScalarSensor>();
+    _sensor0 = new GameObject("sensor").AddComponent<MjUserSensor>();
+    _sensor0.Dimension = 1;
+    _sensor1 = new GameObject("sensor").AddComponent<MjJointScalarSensor>();
+    _sensorGroup = new GameObject("sensors").transform;
+    _sensor0.transform.parent = _sensorGroup;
+    _sensor1.transform.parent = _sensorGroup;
     _actuator = new GameObject("actuator").AddComponent<MjActuator>();
     // body, joint and inertia are always present so that the actuator and sensor are valid
     _body = new GameObject("body").AddComponent<MjBody>();
@@ -189,12 +209,12 @@ public class MjSceneGenerationTests {
     _inertia.transform.parent = _body.transform;
     _joint = new GameObject("joint").AddComponent<MjHingeJoint>();
     _joint.transform.parent = _body.transform;
-    _sensor.Joint = _joint;
-    _sensor.SensorType = MjJointScalarSensor.AvailableSensors.JointPos;
+    _sensor1.Joint = _joint;
+    _sensor1.SensorType = MjJointScalarSensor.AvailableSensors.JointPos;
     _actuator.Joint = _joint;
     _fakeBodyA.enabled = false;
     _fakeBodyB.enabled = false;
-    _sensor.enabled = false;
+    _sensor1.enabled = false;
     _actuator.enabled = false;
   }
 
@@ -202,7 +222,8 @@ public class MjSceneGenerationTests {
   public void TearDown() {
     UnityEngine.Object.DestroyImmediate(_fakeBodyA.gameObject);
     UnityEngine.Object.DestroyImmediate(_fakeBodyB.gameObject);
-    UnityEngine.Object.DestroyImmediate(_sensor.gameObject);
+    UnityEngine.Object.DestroyImmediate(_sensor0.gameObject);
+    UnityEngine.Object.DestroyImmediate(_sensor1.gameObject);
     UnityEngine.Object.DestroyImmediate(_actuator.gameObject);
     UnityEngine.Object.DestroyImmediate(_joint.gameObject);
     UnityEngine.Object.DestroyImmediate(_inertia.gameObject);