From 14a1f59a04934b005ba02d2b0a0b6191e1183ab5 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Wed, 1 Jul 2020 15:26:49 -0700 Subject: [PATCH 01/12] model+local velocity --- .../Runtime/Sensors/ArticulationBodySensor.cs | 74 +++++++++++++++++++ .../Sensors/ArticulationBodySensor.cs.meta | 11 +++ .../ArticulationBodySensorComponent.cs | 41 ++++++++++ .../ArticulationBodySensorComponent.cs.meta | 11 +++ .../Runtime/Sensors/PhysicsSensorSettings.cs | 32 +++++++- .../Runtime/Sensors/PoseExtractor.cs | 46 +++++++++++- .../Runtime/Sensors/RigidBodyPoseExtractor.cs | 67 ++++++++++++++++- .../Runtime/Sensors/RigidBodySensor.cs | 72 ++++++++++++++++++ .../Runtime/Sensors/RigidBodySensor.cs.meta | 11 +++ .../Sensors/RigidBodySensorComponent.cs | 39 ++++++++++ .../Sensors/RigidBodySensorComponent.cs.meta | 11 +++ 11 files changed, 408 insertions(+), 7 deletions(-) create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs.meta create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs create mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs.meta diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs new file mode 100644 index 0000000000..303cc528be --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs @@ -0,0 +1,74 @@ +#if UNITY_2020_1_OR_NEWER +using UnityEngine; +using Unity.MLAgents.Sensors; + +namespace Unity.MLAgents.Extensions.Sensors +{ + public class ArticulationBodySensor : ISensor + { + int[] m_Shape; + string m_SensorName; + + PoseExtractor m_PoseExtractor; + PhysicsSensorSettings m_Settings; + + public ArticulationBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) + { + m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); + m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody.name}" : sensorName; + m_Settings = settings; + + var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); + m_Shape = new[] { numTransformObservations }; + } + + /// + public int[] GetObservationShape() + { + return m_Shape; + } + + /// + public int Write(ObservationWriter writer) + { + var numWritten = writer.WritePoses(m_Settings, m_PoseExtractor); + return numWritten; + } + + /// + public byte[] GetCompressedObservation() + { + return null; + } + + /// + public void Update() + { + if (m_Settings.UseModelSpace) + { + m_PoseExtractor.UpdateModelSpacePoses(); + } + + if (m_Settings.UseLocalSpace) + { + m_PoseExtractor.UpdateLocalSpacePoses(); + } + } + + /// + public void Reset() {} + + /// + public SensorCompressionType GetCompressionType() + { + return SensorCompressionType.None; + } + + /// + public string GetName() + { + return m_SensorName; + } + } +} +#endif // UNITY_2020_1_OR_NEWER \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta new file mode 100644 index 0000000000..d3e0276850 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a9f0629c5c4cc40e58a1cf20f6b7d26b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs new file mode 100644 index 0000000000..0c8124d484 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs @@ -0,0 +1,41 @@ +#if UNITY_2020_1_OR_NEWER +using UnityEngine; +using Unity.MLAgents.Sensors; + +namespace Unity.MLAgents.Extensions.Sensors +{ + public class ArticulationBodySensorComponent : SensorComponent + { + public ArticulationBody RootBody; + + [SerializeField] + public PhysicsSensorSettings Settings = PhysicsSensorSettings.Default(); + public string sensorName; + + /// + /// Creates a ArticulationBodySensor. + /// + /// + public override ISensor CreateSensor() + { + return new ArticulationBodySensor(RootBody, Settings, sensorName); + } + + /// + public override int[] GetObservationShape() + { + if (RootBody == null) + { + return new[] { 0 }; + } + + // TODO static method in ArticulationBodySensor? + // TODO only update PoseExtractor when body changes? + var poseExtractor = new ArticulationBodyPoseExtractor(RootBody); + var numTransformObservations = Settings.TransformSize(poseExtractor.NumPoses); + return new[] { numTransformObservations }; + } + } + +} +#endif // UNITY_2020_1_OR_NEWER \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs.meta new file mode 100644 index 0000000000..3cdd83ac52 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e57a788acd5e049c6aa9642b450ca318 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs index 3a920eb8cd..ae0ddb3a57 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs @@ -27,6 +27,9 @@ public struct PhysicsSensorSettings /// public bool UseLocalSpaceRotations; + public bool UseModelSpaceLinearVelocity; + public bool UseLocalSpaceLinearVelocity; + /// /// Creates a PhysicsSensorSettings with reasonable default values. /// @@ -45,7 +48,7 @@ public static PhysicsSensorSettings Default() /// public bool UseModelSpace { - get { return UseModelSpaceTranslations || UseModelSpaceRotations; } + get { return UseModelSpaceTranslations || UseModelSpaceRotations || UseModelSpaceLinearVelocity; } } /// @@ -53,7 +56,7 @@ public bool UseModelSpace /// public bool UseLocalSpace { - get { return UseLocalSpaceTranslations || UseLocalSpaceRotations; } + get { return UseLocalSpaceTranslations || UseLocalSpaceRotations || UseLocalSpaceLinearVelocity; } } @@ -70,6 +73,9 @@ public int TransformSize(int numTransforms) obsPerTransform += UseLocalSpaceTranslations ? 3 : 0; obsPerTransform += UseLocalSpaceRotations ? 4 : 0; + obsPerTransform += UseModelSpaceLinearVelocity ? 3 : 0; + obsPerTransform += UseLocalSpaceLinearVelocity ? 3 : 0; + return numTransforms * obsPerTransform; } } @@ -89,8 +95,12 @@ public static int WritePoses(this ObservationWriter writer, PhysicsSensorSetting var offset = baseOffset; if (settings.UseModelSpace) { - foreach (var pose in poseExtractor.ModelSpacePoses) + var poses = poseExtractor.ModelSpacePoses; + var vels = poseExtractor.ModelSpaceVelocities; + + for(var i=0; i /// Read access to the model space transforms. /// @@ -35,7 +39,23 @@ public IList LocalSpacePoses } /// - /// Number of transforms in the hierarchy (read-only). + /// Read access to the model space linear velocities. + /// + public IList ModelSpaceVelocities + { + get { return m_ModelSpaceLinearVelocities; } + } + + /// + /// Read access to the local space linear velocities. + /// + public IList LocalSpaceVelocities + { + get { return m_LocalSpaceLinearVelocities; } + } + + /// + /// Number of poses in the hierarchy (read-only). /// public int NumPoses { @@ -53,6 +73,9 @@ protected void SetParentIndices(int[] parentIndices) var numTransforms = parentIndices.Length; m_ModelSpacePoses = new Pose[numTransforms]; m_LocalSpacePoses = new Pose[numTransforms]; + + m_ModelSpaceLinearVelocities = new Vector3[numTransforms]; + m_LocalSpaceLinearVelocities = new Vector3[numTransforms]; } /// @@ -62,6 +85,12 @@ protected void SetParentIndices(int[] parentIndices) /// protected abstract Pose GetPoseAt(int index); + protected virtual Vector3 GetLinearVelocityAt(int index) + { + return Vector3.zero; + } + + /// /// Update the internal model space transform storage based on the underlying system. /// @@ -74,11 +103,16 @@ public void UpdateModelSpacePoses() var worldTransform = GetPoseAt(0); var worldToModel = worldTransform.Inverse(); + var rootLinearVel = GetLinearVelocityAt(0); for (var i = 0; i < m_ModelSpacePoses.Length; i++) { var currentTransform = GetPoseAt(i); m_ModelSpacePoses[i] = worldToModel.Multiply(currentTransform); + + var currentBodyLinearVel = GetLinearVelocityAt(i); + var relativeVelocity = currentBodyLinearVel - rootLinearVel; + m_ModelSpaceLinearVelocities[i] = worldToModel.Multiply(relativeVelocity); } } @@ -102,10 +136,15 @@ public void UpdateLocalSpacePoses() var invParent = parentTransform.Inverse(); var currentTransform = GetPoseAt(i); m_LocalSpacePoses[i] = invParent.Multiply(currentTransform); + + var parentLinearVel = GetLinearVelocityAt(m_ParentIndices[i]); + var currentLinearVel = GetLinearVelocityAt(i); + m_LocalSpaceLinearVelocities[i] = invParent.Multiply(currentLinearVel - parentLinearVel); } else { m_LocalSpacePoses[i] = Pose.identity; + m_LocalSpaceLinearVelocities[i] = Vector3.zero; } } } @@ -165,6 +204,11 @@ public static Pose Multiply(this Pose pose, Pose rhs) return rhs.GetTransformedBy(pose); } + public static Vector3 Multiply(this Pose pose, Vector3 rhs) + { + return pose.rotation * rhs + pose.position; + } + // TODO optimize inv(A)*B? } } diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs index 176f21b2da..1265f8aa16 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs @@ -54,6 +54,70 @@ public RigidBodyPoseExtractor(Rigidbody rootBody) SetParentIndices(parentIndices); } + + + +// public override void UpdateModelSpacePoses() +// { +// base.UpdateModelSpacePoses(); +// var numBodies = m_Bodies?.Length ?? 0; +// if (numBodies == 0) +// { +// return; +// } +// +// var worldPose = GetPoseAt(0); +// var worldToModel = worldPose.Inverse(); +// var rootBody = m_Bodies[0]; +// +// for (var i = 0; i < numBodies; i++) +// { +// var body = GetBodyAt(i); +// var relativeVelocity = body.velocity - rootBody.velocity; +// m_ModelSpaceLinearVelocities[i] = worldToModel.Multiply(relativeVelocity); +// } +// } +// +// public override void UpdateLocalSpacePoses() +// { +// base.UpdateLocalSpacePoses(); +// +// var numBodies = m_Bodies?.Length ?? 0; +// if (numBodies == 0) +// { +// return; +// } +// +// for (var i = 0; i < numBodies; i++) +// { +// var parentIndex = GetParentIndex(i); +// if (i != -1) +// { +// var parentTransform = GetPoseAt(parentIndex); +// var parentBody = GetBodyAt(parentIndex); +// var currentBody = GetBodyAt(i); +// // This is slightly inefficient, since for a body with multiple children, we'll end up inverting +// // the transform multiple times. Might be able to trade space for perf here. +// var invParent = parentTransform.Inverse(); +// m_LocalSpaceLinearVelocities[i] = invParent.Multiply(currentBody.velocity - parentBody.velocity); +// } +// else +// { +// m_LocalSpaceLinearVelocities[i] = Vector3.zero; +// } +// } +// } + + protected Rigidbody GetBodyAt(int index) + { + return m_Bodies[index]; + } + + protected override Vector3 GetLinearVelocityAt(int index) + { + return GetBodyAt(index).velocity; + } + /// /// Get the pose of the i'th RigidBody. /// @@ -64,7 +128,6 @@ protected override Pose GetPoseAt(int index) var body = m_Bodies[index]; return new Pose { rotation = body.rotation, position = body.position }; } - - } + } diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs new file mode 100644 index 0000000000..de338ec346 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs @@ -0,0 +1,72 @@ +using UnityEngine; +using Unity.MLAgents.Sensors; + +namespace Unity.MLAgents.Extensions.Sensors +{ + public class RigidBodySensor : ISensor + { + int[] m_Shape; + string m_SensorName; + + PoseExtractor m_PoseExtractor; + PhysicsSensorSettings m_Settings; + + public RigidBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, string sensorName=null) + { + m_PoseExtractor = new RigidBodyPoseExtractor(rootBody); + m_SensorName = string.IsNullOrEmpty(sensorName) ? $"RigidBodySensor:{rootBody.name}" : sensorName; + m_Settings = settings; + + var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); + m_Shape = new[] { numTransformObservations }; + } + + /// + public int[] GetObservationShape() + { + return m_Shape; + } + + /// + public int Write(ObservationWriter writer) + { + var numWritten = writer.WritePoses(m_Settings, m_PoseExtractor); + return numWritten; + } + + /// + public byte[] GetCompressedObservation() + { + return null; + } + + /// + public void Update() + { + if (m_Settings.UseModelSpace) + { + m_PoseExtractor.UpdateModelSpacePoses(); + } + + if (m_Settings.UseLocalSpace) + { + m_PoseExtractor.UpdateLocalSpacePoses(); + } + } + + /// + public void Reset() {} + + /// + public SensorCompressionType GetCompressionType() + { + return SensorCompressionType.None; + } + + /// + public string GetName() + { + return m_SensorName; + } + } +} diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta new file mode 100644 index 0000000000..2fce9c0200 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 254640b3578a24bd2838c1fa39f1011a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs new file mode 100644 index 0000000000..99ff360419 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs @@ -0,0 +1,39 @@ +using UnityEngine; +using Unity.MLAgents.Sensors; + +namespace Unity.MLAgents.Extensions.Sensors +{ + public class RigidBodySensorComponent : SensorComponent + { + public Rigidbody RootBody; + + [SerializeField] + public PhysicsSensorSettings Settings = PhysicsSensorSettings.Default(); + public string sensorName; + + /// + /// Creates a RigidBodySensor. + /// + /// + public override ISensor CreateSensor() + { + return new RigidBodySensor(RootBody, Settings, sensorName); + } + + /// + public override int[] GetObservationShape() + { + if (RootBody == null) + { + return new[] { 0 }; + } + + // TODO static method in RigidBodySensor? + // TODO only update PoseExtractor when body changes? + var poseExtractor = new RigidBodyPoseExtractor(RootBody); + var numTransformObservations = Settings.TransformSize(poseExtractor.NumPoses); + return new[] { numTransformObservations }; + } + } + +} diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs.meta new file mode 100644 index 0000000000..59ba148382 --- /dev/null +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: df0f8be9a37d6486498061e2cbc4cd94 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From 000f0e745b3225fc280f64e21249755dae8127fa Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 2 Jul 2020 18:21:45 -0700 Subject: [PATCH 02/12] cleanup, simple unit test --- .../Tests/Editor/Sensors/RigidBodySensorTests.cs | 7 +++++++ .../Tests/Editor/Sensors/RigidBodySensorTests.cs.meta | 11 +++++++++++ 2 files changed, 18 insertions(+) create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs.meta diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs new file mode 100644 index 0000000000..9c54ee9e33 --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs @@ -0,0 +1,7 @@ +namespace Unity.MLAgents.Extensions.Tests.Sensors +{ + public class RigidBodySensorTests + { + + } +} diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs.meta b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs.meta new file mode 100644 index 0000000000..f7f204ce99 --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d8daf5517a7c94bfd9ac7f45f8d1bcd3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From deb5bb5b15835a61d9785ea75af16924250e658f Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 2 Jul 2020 18:24:15 -0700 Subject: [PATCH 03/12] cleanup, simple unit test --- .../Sensors/ArticulationBodyPoseExtractor.cs | 12 ++++- .../Runtime/Sensors/PhysicsSensorSettings.cs | 13 +++-- .../Runtime/Sensors/RigidBodyPoseExtractor.cs | 54 ------------------- .../Runtime/Sensors/RigidBodySensor.cs | 2 +- .../Editor/Sensors/RigidBodySensorTests.cs | 16 +++++- ...ty.ML-Agents.Extensions.EditorTests.asmdef | 4 +- .../Tests/Editor/Sensor/VectorSensorTests.cs | 13 +++-- 7 files changed, 49 insertions(+), 65 deletions(-) diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs index 3f5068ed92..65512838db 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs @@ -46,6 +46,16 @@ public ArticulationBodyPoseExtractor(ArticulationBody rootBody) SetParentIndices(parentIndices); } + protected ArticulationBody GetBodyAt(int index) + { + return m_Bodies[index]; + } + + protected override Vector3 GetLinearVelocityAt(int index) + { + return GetBodyAt(index).velocity; + } + protected override Pose GetPoseAt(int index) { var body = m_Bodies[index]; @@ -53,8 +63,6 @@ protected override Pose GetPoseAt(int index) var t = go.transform; return new Pose { rotation = t.rotation, position = t.position }; } - - } } #endif // UNITY_2020_1_OR_NEWER \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs index ae0ddb3a57..c1239cf1f9 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs @@ -13,7 +13,7 @@ public struct PhysicsSensorSettings public bool UseModelSpaceTranslations; /// - /// Whether to use model space (relative to the root body) rotatoins as observations. + /// Whether to use model space (relative to the root body) rotations as observations. /// public bool UseModelSpaceRotations; @@ -27,7 +27,14 @@ public struct PhysicsSensorSettings /// public bool UseLocalSpaceRotations; + /// + /// Whether to use model space (relative to the root body) linear velocities as observations. + /// public bool UseModelSpaceLinearVelocity; + + /// + /// Whether to use local space (relative to the parent body) linear velocities as observations. + /// public bool UseLocalSpaceLinearVelocity; /// @@ -98,7 +105,7 @@ public static int WritePoses(this ObservationWriter writer, PhysicsSensorSetting var poses = poseExtractor.ModelSpacePoses; var vels = poseExtractor.ModelSpaceVelocities; - for(var i=0; i(); + var sensor = sensorComponent.CreateSensor(); + SensorTestHelper.CompareObservation(sensor, new float[0]); + } } } diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef index 7d0dd20f4f..af86dfbd1f 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef +++ b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef @@ -2,7 +2,9 @@ "name": "Unity.ML-Agents.Extensions.EditorTests", "references": [ "Unity.ML-Agents.Extensions.Editor", - "Unity.ML-Agents.Extensions" + "Unity.ML-Agents.Extensions", + "Unity.ML-Agents", + "Unity.ML-Agents.Editor.Tests" ], "optionalUnityReferences": [ "TestAssemblies" diff --git a/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs b/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs index 7df0638680..a7b85c3d08 100644 --- a/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs +++ b/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs @@ -4,7 +4,7 @@ namespace Unity.MLAgents.Tests { - public class SensorTestHelper + public static class SensorTestHelper { public static void CompareObservation(ISensor sensor, float[] expected) { @@ -15,13 +15,20 @@ public static void CompareObservation(ISensor sensor, float[] expected) { output[i] = fill; } - Assert.AreEqual(fill, output[0]); + + if(numExpected > 0) + { + Assert.AreEqual(fill, output[0]); + } ObservationWriter writer = new ObservationWriter(); writer.SetTarget(output, sensor.GetObservationShape(), 0); // Make sure ObservationWriter didn't touch anything - Assert.AreEqual(fill, output[0]); + if(numExpected > 0) + { + Assert.AreEqual(fill, output[0]); + } sensor.Write(writer); Assert.AreEqual(expected, output); From 28a7e8b97f548ce3931f8e7856eef02fdff2b4e7 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Tue, 7 Jul 2020 14:35:59 -0700 Subject: [PATCH 04/12] UNITY_INCLUDE_TESTS --- .../Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef index af86dfbd1f..3017b1be40 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef +++ b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef @@ -12,5 +12,8 @@ "includePlatforms": [ "Editor" ], - "excludePlatforms": [] + "excludePlatforms": [], + "defineConstraints": [ + "UNITY_INCLUDE_TESTS" + ] } From c4f4f97b29efc3af7b6e7ded81be118da889e6c9 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Tue, 7 Jul 2020 16:56:45 -0700 Subject: [PATCH 05/12] unit tests, move test util to API --- .../Runtime/Sensors/PoseExtractor.cs | 4 +- .../Editor/Sensors/RigidBodySensorTests.cs | 85 ++++++++++++++++++- ...ty.ML-Agents.Extensions.EditorTests.asmdef | 3 +- com.unity.ml-agents/Runtime/SensorHelper.cs | 61 +++++++++++++ .../Tests/Editor/Sensor/VectorSensorTests.cs | 27 +----- 5 files changed, 151 insertions(+), 29 deletions(-) create mode 100644 com.unity.ml-agents/Runtime/SensorHelper.cs diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index 96478ae6c8..6d3d73938c 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -112,7 +112,7 @@ public void UpdateModelSpacePoses() var currentBodyLinearVel = GetLinearVelocityAt(i); var relativeVelocity = currentBodyLinearVel - rootLinearVel; - m_ModelSpaceLinearVelocities[i] = worldToModel.Multiply(relativeVelocity); + m_ModelSpaceLinearVelocities[i] = worldToModel.rotation * relativeVelocity; } } @@ -139,7 +139,7 @@ public void UpdateLocalSpacePoses() var parentLinearVel = GetLinearVelocityAt(m_ParentIndices[i]); var currentLinearVel = GetLinearVelocityAt(i); - m_LocalSpaceLinearVelocities[i] = invParent.Multiply(currentLinearVel - parentLinearVel); + m_LocalSpaceLinearVelocities[i] = invParent.rotation * (currentLinearVel - parentLinearVel); } else { diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs index cb7589fbc0..6c2a189c00 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs @@ -1,11 +1,22 @@ using UnityEngine; using NUnit.Framework; +using Unity.MLAgents.Sensors; using Unity.MLAgents.Extensions.Sensors; -using Unity.MLAgents.Tests; namespace Unity.MLAgents.Extensions.Tests.Sensors { + + public static class SensorTestHelper + { + public static void CompareObservation(ISensor sensor, float[] expected) + { + string errorMessage; + bool isOK = SensorHelper.CompareObservation(sensor, expected, out errorMessage); + Assert.IsTrue(isOK, errorMessage); + } + } + public class RigidBodySensorTests { [Test] @@ -17,5 +28,77 @@ public void TestNullRootBody() var sensor = sensorComponent.CreateSensor(); SensorTestHelper.CompareObservation(sensor, new float[0]); } + + [Test] + public void TestSingleRigidbody() + { + var gameObj = new GameObject(); + var rootRb = gameObj.AddComponent(); + var sensorComponent = gameObj.AddComponent(); + sensorComponent.RootBody = rootRb; + sensorComponent.Settings = new PhysicsSensorSettings + { + UseModelSpaceLinearVelocity = true, + UseLocalSpaceTranslations = true, + UseLocalSpaceRotations = true + }; + + var sensor = sensorComponent.CreateSensor(); + sensor.Update(); + var expected = new[] + { + 0f, 0f, 0f, // ModelSpaceLinearVelocity + 0f, 0f, 0f, // LocalSpaceTranslations + 0f, 0f, 0f, 1f // LocalSpaceRotations + }; + SensorTestHelper.CompareObservation(sensor, expected); + } + + [Test] + public void TestBodiesWithJoint() + { + var rootObj = new GameObject(); + var rootRb = rootObj.AddComponent(); + + var middleGamObj = new GameObject(); + var middleRb = middleGamObj.AddComponent(); + middleGamObj.transform.SetParent(rootObj.transform); + middleGamObj.transform.localPosition = new Vector3(13.37f, 0f, 0f); + var joint = middleGamObj.AddComponent(); + joint.connectedBody = rootRb; + + var leafGameObj = new GameObject(); + var leafRb = leafGameObj.AddComponent(); + leafGameObj.transform.SetParent(middleGamObj.transform); + leafGameObj.transform.localPosition = new Vector3(4.2f, 0f, 0f); + var joint2 = leafGameObj.AddComponent(); + joint2.connectedBody = middleRb; + + + var sensorComponent = rootObj.AddComponent(); + sensorComponent.RootBody = rootRb; + sensorComponent.Settings = new PhysicsSensorSettings + { + UseModelSpaceTranslations = true, + UseLocalSpaceTranslations = true, + }; + + var sensor = sensorComponent.CreateSensor(); + sensor.Update(); + var expected = new[] + { + // Model space + 0f, 0f, 0f, // Root + 13.37f, 0f, 0f, // Middle + leafGameObj.transform.position.x, 0f, 0f, // Leaf + + // Local space + 0f, 0f, 0f, // Root + 13.37f, 0f, 0f, // Attached + 4.2f, 0f, 0f, // Leaf + }; + SensorTestHelper.CompareObservation(sensor, expected); + + } } } diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef index 3017b1be40..2c13fc3ac1 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef +++ b/com.unity.ml-agents.extensions/Tests/Editor/Unity.ML-Agents.Extensions.EditorTests.asmdef @@ -3,8 +3,7 @@ "references": [ "Unity.ML-Agents.Extensions.Editor", "Unity.ML-Agents.Extensions", - "Unity.ML-Agents", - "Unity.ML-Agents.Editor.Tests" + "Unity.ML-Agents" ], "optionalUnityReferences": [ "TestAssemblies" diff --git a/com.unity.ml-agents/Runtime/SensorHelper.cs b/com.unity.ml-agents/Runtime/SensorHelper.cs new file mode 100644 index 0000000000..e52e9c5edf --- /dev/null +++ b/com.unity.ml-agents/Runtime/SensorHelper.cs @@ -0,0 +1,61 @@ +using UnityEngine; + +namespace Unity.MLAgents.Sensors +{ + public static class SensorHelper + { + /// + /// Generates the observations for the provided sensor, and returns true if they equal the + /// expected values. If they are unequal, errorMessage is also set. + /// + /// + /// + /// + /// + public static bool CompareObservation(ISensor sensor, float[] expected, out string errorMessage) + { + var numExpected = expected.Length; + const float fill = -1337f; + var output = new float[numExpected]; + for (var i = 0; i < numExpected; i++) + { + output[i] = fill; + } + + if(numExpected > 0) + { + if (fill != output[0]) + { + errorMessage = "Error setting output buffer."; + return false; + } + } + + ObservationWriter writer = new ObservationWriter(); + writer.SetTarget(output, sensor.GetObservationShape(), 0); + + // Make sure ObservationWriter didn't touch anything + if(numExpected > 0) + { + if (fill != output[0]) + { + errorMessage = "ObservationWriter.SetTarget modified a buffer it shouldn't have."; + return false; + } + } + + sensor.Write(writer); + for (var i = 0; i < output.Length; i++) + { + if (expected[i] != output[i]) + { + errorMessage = $"Expected and actual differed in position {i}. Expected: {expected[i]} Actual: {output[i]} "; + return false; + } + } + + errorMessage = null; + return true; + } + } +} diff --git a/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs b/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs index a7b85c3d08..b24dbddcc0 100644 --- a/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs +++ b/com.unity.ml-agents/Tests/Editor/Sensor/VectorSensorTests.cs @@ -8,30 +8,9 @@ public static class SensorTestHelper { public static void CompareObservation(ISensor sensor, float[] expected) { - var numExpected = expected.Length; - const float fill = -1337f; - var output = new float[numExpected]; - for (var i = 0; i < numExpected; i++) - { - output[i] = fill; - } - - if(numExpected > 0) - { - Assert.AreEqual(fill, output[0]); - } - - ObservationWriter writer = new ObservationWriter(); - writer.SetTarget(output, sensor.GetObservationShape(), 0); - - // Make sure ObservationWriter didn't touch anything - if(numExpected > 0) - { - Assert.AreEqual(fill, output[0]); - } - - sensor.Write(writer); - Assert.AreEqual(expected, output); + string errorMessage; + bool isOK = SensorHelper.CompareObservation(sensor, expected, out errorMessage); + Assert.IsTrue(isOK, errorMessage); } } From 4deda2fa932e8ef6f4ec627cb822795832358cba Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Tue, 7 Jul 2020 18:44:40 -0700 Subject: [PATCH 06/12] tests and bugfixes --- .../Sensors/ArticulationBodyPoseExtractor.cs | 15 ++- .../Runtime/Sensors/ArticulationBodySensor.cs | 2 +- .../Runtime/Sensors/PoseExtractor.cs | 10 ++ .../ArticulationBodyPoseExtractorTests.cs | 63 ++++++++++ ...ArticulationBodyPoseExtractorTests.cs.meta | 11 ++ .../Sensors/ArticulationBodySensorTests.cs | 119 ++++++++++++++++++ .../ArticulationBodySensorTests.cs.meta | 11 ++ .../Sensors/RigidBodyPoseExtractorTests.cs | 1 - .../Editor/Sensors/RigidBodySensorTests.cs | 21 +++- .../Runtime/SensorHelper.cs.meta | 11 ++ 10 files changed, 253 insertions(+), 11 deletions(-) create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs.meta create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs create mode 100644 com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs.meta create mode 100644 com.unity.ml-agents/Runtime/SensorHelper.cs.meta diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs index 65512838db..d9ebd0ba3a 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs @@ -12,6 +12,11 @@ public class ArticulationBodyPoseExtractor : PoseExtractor public ArticulationBodyPoseExtractor(ArticulationBody rootBody) { + if (rootBody == null) + { + return; + } + if (!rootBody.isRoot) { Debug.Log("Must pass ArticulationBody.isRoot"); @@ -38,9 +43,13 @@ public ArticulationBodyPoseExtractor(ArticulationBody rootBody) for (var i = 1; i < numBodies; i++) { - var body = m_Bodies[i]; - var parent = body.GetComponentInParent(); - parentIndices[i] = bodyToIndex[parent]; + var currentArticBody = m_Bodies[i]; + // Component.GetComponentInParent will consider the provided object as well. + // So start looking from the parent. + var currentGameObject = currentArticBody.gameObject; + var parentGameObject = currentGameObject.transform.parent; + var parentArticBody = parentGameObject.GetComponentInParent(); + parentIndices[i] = bodyToIndex[parentArticBody]; } SetParentIndices(parentIndices); diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs index 303cc528be..51609c3330 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs @@ -15,7 +15,7 @@ public class ArticulationBodySensor : ISensor public ArticulationBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) { m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); - m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody.name}" : sensorName; + m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName; m_Settings = settings; var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index 6d3d73938c..a26257d2a2 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -62,6 +62,16 @@ public int NumPoses get { return m_ModelSpacePoses?.Length ?? 0; } } + public int GetParentIndex(int index) + { + if (m_ParentIndices == null) + { + return -1; + } + + return m_ParentIndices[index]; + } + /// /// Initialize with the mapping of parent indices. /// The 0th element is assumed to be -1, indicating that it's the root. diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs new file mode 100644 index 0000000000..b6f640e53e --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs @@ -0,0 +1,63 @@ +#if UNITY_2020_1_OR_NEWER +using UnityEngine; +using NUnit.Framework; +using Unity.MLAgents.Extensions.Sensors; + +namespace Unity.MLAgents.Extensions.Tests.Sensors +{ + public class ArticulationBodyPoseExtractorTests + { + [TearDown] + public void RemoveGameObjects() + { + var objects = GameObject.FindObjectsOfType(); + foreach (var o in objects) + { + UnityEngine.Object.DestroyImmediate(o); + } + } + + [Test] + public void TestNullRoot() + { + var poseExtractor = new ArticulationBodyPoseExtractor(null); + // These should be no-ops + poseExtractor.UpdateLocalSpacePoses(); + poseExtractor.UpdateModelSpacePoses(); + + Assert.AreEqual(0, poseExtractor.NumPoses); + } + + [Test] + public void TestSingleBody() + { + var go = new GameObject(); + var rootArticBody = go.AddComponent(); + var poseExtractor = new ArticulationBodyPoseExtractor(rootArticBody); + Assert.AreEqual(1, poseExtractor.NumPoses); + } + + [Test] + public void TestTwoBodies() + { + // * rootObj + // - rootArticBody + // * leafGameObj + // - leafArticBody + var rootObj = new GameObject(); + var rootArticBody = rootObj.AddComponent(); + + var leafGameObj = new GameObject(); + var leafArticBody = leafGameObj.AddComponent(); + leafGameObj.transform.SetParent(rootObj.transform); + + leafArticBody.jointType = ArticulationJointType.RevoluteJoint; + + var poseExtractor = new ArticulationBodyPoseExtractor(rootArticBody); + Assert.AreEqual(2, poseExtractor.NumPoses); + Assert.AreEqual(-1, poseExtractor.GetParentIndex(0)); + Assert.AreEqual(0, poseExtractor.GetParentIndex(1)); + } + } +} +#endif \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs.meta b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs.meta new file mode 100644 index 0000000000..af76a34446 --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodyPoseExtractorTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 934ea08cde59d4356bc41e040d333c3d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs new file mode 100644 index 0000000000..1b33603b80 --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs @@ -0,0 +1,119 @@ +#if UNITY_2020_1_OR_NEWER +using UnityEngine; +using NUnit.Framework; +using Unity.MLAgents.Extensions.Sensors; + + +namespace Unity.MLAgents.Extensions.Tests.Sensors +{ + + public class ArticulationBodySensorTests + { + [Test] + public void TestNullRootBody() + { + var gameObj = new GameObject(); + + var sensorComponent = gameObj.AddComponent(); + var sensor = sensorComponent.CreateSensor(); + SensorTestHelper.CompareObservation(sensor, new float[0]); + } + + [Test] + public void TestSingleBody() + { + var gameObj = new GameObject(); + var articulationBody = gameObj.AddComponent(); + var sensorComponent = gameObj.AddComponent(); + sensorComponent.RootBody = articulationBody; + sensorComponent.Settings = new PhysicsSensorSettings + { + UseModelSpaceLinearVelocity = true, + UseLocalSpaceTranslations = true, + UseLocalSpaceRotations = true + }; + + var sensor = sensorComponent.CreateSensor(); + sensor.Update(); + var expected = new[] + { + 0f, 0f, 0f, // ModelSpaceLinearVelocity + 0f, 0f, 0f, // LocalSpaceTranslations + 0f, 0f, 0f, 1f // LocalSpaceRotations + }; + SensorTestHelper.CompareObservation(sensor, expected); + } + + [Test] + public void TestBodiesWithJoint() + { + var rootObj = new GameObject(); + var rootArticBody = rootObj.AddComponent(); + // See if there's a way to set velocity directly. + //rootArticBody.velocity = new Vector3(1f, 0f, 0f); + + var middleGamObj = new GameObject(); + var middleArticBody = middleGamObj.AddComponent(); + //middleArticBody.velocity = new Vector3(0f, 1f, 0f); + middleArticBody.AddForce(new Vector3(0f, 1f, 0f)); + middleGamObj.transform.SetParent(rootObj.transform); + middleGamObj.transform.localPosition = new Vector3(13.37f, 0f, 0f); + middleArticBody.jointType = ArticulationJointType.RevoluteJoint; + + var leafGameObj = new GameObject(); + var leafArticBody = leafGameObj.AddComponent(); + //leafArticBody.velocity = new Vector3(0f, 0f, 1f); + leafGameObj.transform.SetParent(middleGamObj.transform); + leafGameObj.transform.localPosition = new Vector3(4.2f, 0f, 0f); + leafArticBody.jointType = ArticulationJointType.RevoluteJoint; + +#if UNITY_2020_2_OR_NEWER + // ArticulationBody.velocity is read-only in 2020.1 + rootArticBody.velocity = new Vector3(1f, 0f, 0f); + middleArticBody.velocity = new Vector3(0f, 1f, 0f); + leafArticBody.velocity = new Vector3(0f, 0f, 1f); +#endif + + var sensorComponent = rootObj.AddComponent(); + sensorComponent.RootBody = rootArticBody; + sensorComponent.Settings = new PhysicsSensorSettings + { + UseModelSpaceTranslations = true, + UseLocalSpaceTranslations = true, +#if UNITY_2020_2_OR_NEWER + UseLocalSpaceLinearVelocity = true +#endif + }; + + + + var sensor = sensorComponent.CreateSensor(); + sensor.Update(); + var expected = new[] + { + // Model space + 0f, 0f, 0f, // Root pos + 13.37f, 0f, 0f, // Middle pos + leafGameObj.transform.position.x, 0f, 0f, // Leaf pos + + // Local space + 0f, 0f, 0f, // Root pos +#if UNITY_2020_2_OR_NEWER + 0f, 0f, 0f, // Root vel +#endif + + 13.37f, 0f, 0f, // Attached pos +#if UNITY_2020_2_OR_NEWER + -1f, 1f, 0f, // Attached vel +#endif + + 4.2f, 0f, 0f, // Leaf pos +#if UNITY_2020_2_OR_NEWER + 0f, -1f, 1f // Leaf vel +#endif + }; + SensorTestHelper.CompareObservation(sensor, expected); + } + } +} +#endif // #if UNITY_2020_1_OR_NEWER \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs.meta b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs.meta new file mode 100644 index 0000000000..97a2d4ba47 --- /dev/null +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0ef757469348342418a68826f51d0783 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodyPoseExtractorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodyPoseExtractorTests.cs index 079e7dac23..a5d8b5bcb5 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodyPoseExtractorTests.cs +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodyPoseExtractorTests.cs @@ -1,4 +1,3 @@ -using System.Collections.Generic; using UnityEngine; using NUnit.Framework; using Unity.MLAgents.Extensions.Sensors; diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs index 6c2a189c00..5fbb74c9cf 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/RigidBodySensorTests.cs @@ -59,9 +59,11 @@ public void TestBodiesWithJoint() { var rootObj = new GameObject(); var rootRb = rootObj.AddComponent(); + rootRb.velocity = new Vector3(1f, 0f, 0f); var middleGamObj = new GameObject(); var middleRb = middleGamObj.AddComponent(); + middleRb.velocity = new Vector3(0f, 1f, 0f); middleGamObj.transform.SetParent(rootObj.transform); middleGamObj.transform.localPosition = new Vector3(13.37f, 0f, 0f); var joint = middleGamObj.AddComponent(); @@ -69,6 +71,7 @@ public void TestBodiesWithJoint() var leafGameObj = new GameObject(); var leafRb = leafGameObj.AddComponent(); + leafRb.velocity = new Vector3(0f, 0f, 1f); leafGameObj.transform.SetParent(middleGamObj.transform); leafGameObj.transform.localPosition = new Vector3(4.2f, 0f, 0f); var joint2 = leafGameObj.AddComponent(); @@ -81,6 +84,7 @@ public void TestBodiesWithJoint() { UseModelSpaceTranslations = true, UseLocalSpaceTranslations = true, + UseLocalSpaceLinearVelocity = true }; var sensor = sensorComponent.CreateSensor(); @@ -88,14 +92,19 @@ public void TestBodiesWithJoint() var expected = new[] { // Model space - 0f, 0f, 0f, // Root - 13.37f, 0f, 0f, // Middle - leafGameObj.transform.position.x, 0f, 0f, // Leaf + 0f, 0f, 0f, // Root pos + 13.37f, 0f, 0f, // Middle pos + leafGameObj.transform.position.x, 0f, 0f, // Leaf pos // Local space - 0f, 0f, 0f, // Root - 13.37f, 0f, 0f, // Attached - 4.2f, 0f, 0f, // Leaf + 0f, 0f, 0f, // Root pos + 0f, 0f, 0f, // Root vel + + 13.37f, 0f, 0f, // Attached pos + -1f, 1f, 0f, // Attached vel + + 4.2f, 0f, 0f, // Leaf pos + 0f, -1f, 1f // Leaf vel }; SensorTestHelper.CompareObservation(sensor, expected); diff --git a/com.unity.ml-agents/Runtime/SensorHelper.cs.meta b/com.unity.ml-agents/Runtime/SensorHelper.cs.meta new file mode 100644 index 0000000000..c331abd0b6 --- /dev/null +++ b/com.unity.ml-agents/Runtime/SensorHelper.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7c1189c0af42c46f7b533350d49ad3e7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: From 6d2dac3b5cd26b55431cece7d763914afa340b9c Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Wed, 8 Jul 2020 15:51:35 -0700 Subject: [PATCH 07/12] docs and package validation fixes --- com.unity.ml-agents.extensions/LICENSE.md | 2 +- .../Sensors/ArticulationBodyPoseExtractor.cs | 13 ++++++------ .../Runtime/Sensors/ArticulationBodySensor.cs | 9 ++++++++ .../Runtime/Sensors/PhysicsSensorSettings.cs | 3 +++ .../Runtime/Sensors/PoseExtractor.cs | 21 ++++++++++++++++++- .../Runtime/Sensors/RigidBodyPoseExtractor.cs | 14 +++---------- .../Runtime/Sensors/RigidBodySensor.cs | 9 ++++++++ .../Sensors/RigidBodySensorComponent.cs | 13 ++++++++++++ com.unity.ml-agents/Runtime/SensorHelper.cs | 9 ++++++-- 9 files changed, 71 insertions(+), 22 deletions(-) diff --git a/com.unity.ml-agents.extensions/LICENSE.md b/com.unity.ml-agents.extensions/LICENSE.md index 1f43a89bfa..3b2574fbe2 100644 --- a/com.unity.ml-agents.extensions/LICENSE.md +++ b/com.unity.ml-agents.extensions/LICENSE.md @@ -1,4 +1,4 @@ -com.unity.ml-agents.extensions copyright © 2020 Unity Technologies +com.unity.ml-agents.extensions copyright © 2020 Unity Technologies ApS Licensed under the Unity Companion License for Unity-dependent projects -- see [Unity Companion License](http://www.unity3d.com/legal/licenses/Unity_Companion_License). diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs index d9ebd0ba3a..512b857345 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodyPoseExtractor.cs @@ -5,7 +5,9 @@ namespace Unity.MLAgents.Extensions.Sensors { - + /// + /// Utility class to track a hierarchy of ArticulationBodies. + /// public class ArticulationBodyPoseExtractor : PoseExtractor { ArticulationBody[] m_Bodies; @@ -55,16 +57,13 @@ public ArticulationBodyPoseExtractor(ArticulationBody rootBody) SetParentIndices(parentIndices); } - protected ArticulationBody GetBodyAt(int index) - { - return m_Bodies[index]; - } - + /// protected override Vector3 GetLinearVelocityAt(int index) { - return GetBodyAt(index).velocity; + return m_Bodies[index].velocity; } + /// protected override Pose GetPoseAt(int index) { var body = m_Bodies[index]; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs index 51609c3330..6621859c70 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs @@ -4,6 +4,9 @@ namespace Unity.MLAgents.Extensions.Sensors { + /// + /// ISensor implementation to generate observations for a group of ArticulationBodies. + /// public class ArticulationBodySensor : ISensor { int[] m_Shape; @@ -12,6 +15,12 @@ public class ArticulationBodySensor : ISensor PoseExtractor m_PoseExtractor; PhysicsSensorSettings m_Settings; + /// + /// Construct a new ArticulationBodySensor. + /// + /// + /// + /// public ArticulationBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) { m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs index c1239cf1f9..31a48e31c9 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsSensorSettings.cs @@ -4,6 +4,9 @@ namespace Unity.MLAgents.Extensions.Sensors { + /// + /// Settings that define the observations generated for physics-based sensors. + /// [Serializable] public struct PhysicsSensorSettings { diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index a26257d2a2..cfc8d3da6f 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -62,6 +62,11 @@ public int NumPoses get { return m_ModelSpacePoses?.Length ?? 0; } } + /// + /// Get the parent index of the body at the specified index. + /// + /// + /// public int GetParentIndex(int index) { if (m_ParentIndices == null) @@ -95,6 +100,11 @@ protected void SetParentIndices(int[] parentIndices) /// protected abstract Pose GetPoseAt(int index); + /// + /// Return the world space linear velocity of the i'th object. + /// + /// + /// protected virtual Vector3 GetLinearVelocityAt(int index) { return Vector3.zero; @@ -160,7 +170,7 @@ public void UpdateLocalSpacePoses() } - public void DrawModelSpace(Vector3 offset) + internal void DrawModelSpace(Vector3 offset) { UpdateLocalSpacePoses(); UpdateModelSpacePoses(); @@ -187,6 +197,9 @@ public void DrawModelSpace(Vector3 offset) } } + /// + /// Extension methods for the Pose struct, in order to improve the readability of some math. + /// public static class PoseExtensions { /// @@ -214,6 +227,12 @@ public static Pose Multiply(this Pose pose, Pose rhs) return rhs.GetTransformedBy(pose); } + /// + /// Transform the vector by the pose. + /// + /// + /// + /// public static Vector3 Multiply(this Pose pose, Vector3 rhs) { return pose.rotation * rhs + pose.position; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs index 0d0b542fb4..0c3766182a 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs @@ -54,21 +54,13 @@ public RigidBodyPoseExtractor(Rigidbody rootBody) SetParentIndices(parentIndices); } - protected Rigidbody GetBodyAt(int index) - { - return m_Bodies[index]; - } - + /// protected override Vector3 GetLinearVelocityAt(int index) { - return GetBodyAt(index).velocity; + return m_Bodies[index].velocity; } - /// - /// Get the pose of the i'th RigidBody. - /// - /// - /// +/// protected override Pose GetPoseAt(int index) { var body = m_Bodies[index]; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs index 89686e694f..d45867d57d 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs @@ -3,6 +3,9 @@ namespace Unity.MLAgents.Extensions.Sensors { + /// + /// ISensor implementation that generates observations for a group of Rigidbodies and Joints. + /// public class RigidBodySensor : ISensor { int[] m_Shape; @@ -11,6 +14,12 @@ public class RigidBodySensor : ISensor PoseExtractor m_PoseExtractor; PhysicsSensorSettings m_Settings; + /// + /// Construct a new RigidBodySensor + /// + /// + /// + /// public RigidBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, string sensorName=null) { m_PoseExtractor = new RigidBodyPoseExtractor(rootBody); diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs index 99ff360419..85c8057234 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs @@ -3,12 +3,25 @@ namespace Unity.MLAgents.Extensions.Sensors { + /// + /// Editor component that creates a RigidBodySensor for the Agent. + /// public class RigidBodySensorComponent : SensorComponent { + /// + /// The root Rigidbody of the system. + /// public Rigidbody RootBody; + /// + /// Settings defining what types of observations will be generated. + /// [SerializeField] public PhysicsSensorSettings Settings = PhysicsSensorSettings.Default(); + + /// + /// Optional sensor name. This must be unique for each Agent. + /// public string sensorName; /// diff --git a/com.unity.ml-agents/Runtime/SensorHelper.cs b/com.unity.ml-agents/Runtime/SensorHelper.cs index e52e9c5edf..471a768b27 100644 --- a/com.unity.ml-agents/Runtime/SensorHelper.cs +++ b/com.unity.ml-agents/Runtime/SensorHelper.cs @@ -2,11 +2,16 @@ namespace Unity.MLAgents.Sensors { + /// + /// Utility methods related to implementations. + /// public static class SensorHelper { /// /// Generates the observations for the provided sensor, and returns true if they equal the /// expected values. If they are unequal, errorMessage is also set. + /// This should not generally be used in production code. It is only intended for + /// simplifying unit tests. /// /// /// @@ -22,7 +27,7 @@ public static bool CompareObservation(ISensor sensor, float[] expected, out stri output[i] = fill; } - if(numExpected > 0) + if (numExpected > 0) { if (fill != output[0]) { @@ -35,7 +40,7 @@ public static bool CompareObservation(ISensor sensor, float[] expected, out stri writer.SetTarget(output, sensor.GetObservationShape(), 0); // Make sure ObservationWriter didn't touch anything - if(numExpected > 0) + if (numExpected > 0) { if (fill != output[0]) { From 7c3736fc931b4236c89415cc4bd484ab6aa505b9 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 9 Jul 2020 13:56:25 -0700 Subject: [PATCH 08/12] combine sensor impls --- .../Runtime/Sensors/ArticulationBodySensor.cs | 83 ------------------- .../Sensors/ArticulationBodySensor.cs.meta | 11 --- .../Runtime/Sensors/RigidBodyPoseExtractor.cs | 2 +- .../Runtime/Sensors/RigidBodySensor.cs | 12 +++ 4 files changed, 13 insertions(+), 95 deletions(-) delete mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs delete mode 100644 com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs deleted file mode 100644 index 6621859c70..0000000000 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs +++ /dev/null @@ -1,83 +0,0 @@ -#if UNITY_2020_1_OR_NEWER -using UnityEngine; -using Unity.MLAgents.Sensors; - -namespace Unity.MLAgents.Extensions.Sensors -{ - /// - /// ISensor implementation to generate observations for a group of ArticulationBodies. - /// - public class ArticulationBodySensor : ISensor - { - int[] m_Shape; - string m_SensorName; - - PoseExtractor m_PoseExtractor; - PhysicsSensorSettings m_Settings; - - /// - /// Construct a new ArticulationBodySensor. - /// - /// - /// - /// - public ArticulationBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) - { - m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); - m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName; - m_Settings = settings; - - var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); - m_Shape = new[] { numTransformObservations }; - } - - /// - public int[] GetObservationShape() - { - return m_Shape; - } - - /// - public int Write(ObservationWriter writer) - { - var numWritten = writer.WritePoses(m_Settings, m_PoseExtractor); - return numWritten; - } - - /// - public byte[] GetCompressedObservation() - { - return null; - } - - /// - public void Update() - { - if (m_Settings.UseModelSpace) - { - m_PoseExtractor.UpdateModelSpacePoses(); - } - - if (m_Settings.UseLocalSpace) - { - m_PoseExtractor.UpdateLocalSpacePoses(); - } - } - - /// - public void Reset() {} - - /// - public SensorCompressionType GetCompressionType() - { - return SensorCompressionType.None; - } - - /// - public string GetName() - { - return m_SensorName; - } - } -} -#endif // UNITY_2020_1_OR_NEWER \ No newline at end of file diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta deleted file mode 100644 index d3e0276850..0000000000 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensor.cs.meta +++ /dev/null @@ -1,11 +0,0 @@ -fileFormatVersion: 2 -guid: a9f0629c5c4cc40e58a1cf20f6b7d26b -MonoImporter: - externalObjects: {} - serializedVersion: 2 - defaultReferences: [] - executionOrder: 0 - icon: {instanceID: 0} - userData: - assetBundleName: - assetBundleVariant: diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs index 0c3766182a..b64c42c7ec 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs @@ -60,7 +60,7 @@ protected override Vector3 GetLinearVelocityAt(int index) return m_Bodies[index].velocity; } -/// + /// protected override Pose GetPoseAt(int index) { var body = m_Bodies[index]; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs index d45867d57d..b9a14986e0 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs @@ -30,6 +30,18 @@ public RigidBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, strin m_Shape = new[] { numTransformObservations }; } +#if UNITY_2020_1_OR_NEWER + public RigidBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) + { + m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); + m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName; + m_Settings = settings; + + var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); + m_Shape = new[] { numTransformObservations }; + } +#endif + /// public int[] GetObservationShape() { From 94a69177c99ed421002cfbf57c3b3b9b62318833 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 9 Jul 2020 14:16:41 -0700 Subject: [PATCH 09/12] rename sensor, doc comments --- .../Sensors/ArticulationBodySensorComponent.cs | 6 +++--- .../{RigidBodySensor.cs => PhysicsBodySensor.cs} | 12 ++++++------ ...dBodySensor.cs.meta => PhysicsBodySensor.cs.meta} | 0 .../Runtime/Sensors/PoseExtractor.cs | 4 +++- .../Runtime/Sensors/RigidBodySensorComponent.cs | 8 ++++---- 5 files changed, 16 insertions(+), 14 deletions(-) rename com.unity.ml-agents.extensions/Runtime/Sensors/{RigidBodySensor.cs => PhysicsBodySensor.cs} (85%) rename com.unity.ml-agents.extensions/Runtime/Sensors/{RigidBodySensor.cs.meta => PhysicsBodySensor.cs.meta} (100%) diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs index 0c8124d484..23682bf2f6 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/ArticulationBodySensorComponent.cs @@ -13,12 +13,12 @@ public class ArticulationBodySensorComponent : SensorComponent public string sensorName; /// - /// Creates a ArticulationBodySensor. + /// Creates a PhysicsBodySensor. /// /// public override ISensor CreateSensor() { - return new ArticulationBodySensor(RootBody, Settings, sensorName); + return new PhysicsBodySensor(RootBody, Settings, sensorName); } /// @@ -29,7 +29,7 @@ public override int[] GetObservationShape() return new[] { 0 }; } - // TODO static method in ArticulationBodySensor? + // TODO static method in PhysicsBodySensor? // TODO only update PoseExtractor when body changes? var poseExtractor = new ArticulationBodyPoseExtractor(RootBody); var numTransformObservations = Settings.TransformSize(poseExtractor.NumPoses); diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs similarity index 85% rename from com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs rename to com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs index b9a14986e0..7cc1d60ad2 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs @@ -4,9 +4,9 @@ namespace Unity.MLAgents.Extensions.Sensors { /// - /// ISensor implementation that generates observations for a group of Rigidbodies and Joints. + /// ISensor implementation that generates observations for a group of Rigidbodies or ArticulationBodies. /// - public class RigidBodySensor : ISensor + public class PhysicsBodySensor : ISensor { int[] m_Shape; string m_SensorName; @@ -15,15 +15,15 @@ public class RigidBodySensor : ISensor PhysicsSensorSettings m_Settings; /// - /// Construct a new RigidBodySensor + /// Construct a new PhysicsBodySensor /// /// /// /// - public RigidBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, string sensorName=null) + public PhysicsBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, string sensorName=null) { m_PoseExtractor = new RigidBodyPoseExtractor(rootBody); - m_SensorName = string.IsNullOrEmpty(sensorName) ? $"RigidBodySensor:{rootBody?.name}" : sensorName; + m_SensorName = string.IsNullOrEmpty(sensorName) ? $"PhysicsBodySensor:{rootBody?.name}" : sensorName; m_Settings = settings; var numTransformObservations = settings.TransformSize(m_PoseExtractor.NumPoses); @@ -31,7 +31,7 @@ public RigidBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, strin } #if UNITY_2020_1_OR_NEWER - public RigidBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) + public PhysicsBodySensor(ArticulationBody rootBody, PhysicsSensorSettings settings, string sensorName=null) { m_PoseExtractor = new ArticulationBodyPoseExtractor(rootBody); m_SensorName = string.IsNullOrEmpty(sensorName) ? $"ArticulationBodySensor:{rootBody?.name}" : sensorName; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs.meta similarity index 100% rename from com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensor.cs.meta rename to com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs.meta diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index cfc8d3da6f..9ce456e44e 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -228,7 +228,9 @@ public static Pose Multiply(this Pose pose, Pose rhs) } /// - /// Transform the vector by the pose. + /// Transform the vector by the pose. Conceptually this is equivalent to treating the Pose + /// as a 4x4 matrix and multiplying the augmented vector. + /// See https://en.wikipedia.org/wiki/Affine_transformation#Augmented_matrix for more details. /// /// /// diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs index 85c8057234..b9bd947f79 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs @@ -4,7 +4,7 @@ namespace Unity.MLAgents.Extensions.Sensors { /// - /// Editor component that creates a RigidBodySensor for the Agent. + /// Editor component that creates a PhysicsBodySensor for the Agent. /// public class RigidBodySensorComponent : SensorComponent { @@ -25,12 +25,12 @@ public class RigidBodySensorComponent : SensorComponent public string sensorName; /// - /// Creates a RigidBodySensor. + /// Creates a PhysicsBodySensor. /// /// public override ISensor CreateSensor() { - return new RigidBodySensor(RootBody, Settings, sensorName); + return new PhysicsBodySensor(RootBody, Settings, sensorName); } /// @@ -41,7 +41,7 @@ public override int[] GetObservationShape() return new[] { 0 }; } - // TODO static method in RigidBodySensor? + // TODO static method in PhysicsBodySensor? // TODO only update PoseExtractor when body changes? var poseExtractor = new RigidBodyPoseExtractor(RootBody); var numTransformObservations = Settings.TransformSize(poseExtractor.NumPoses); From 413e613c0a0144cb4e3dd5a21df4f627175a4a95 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 9 Jul 2020 14:18:15 -0700 Subject: [PATCH 10/12] cleanup --- .../Tests/Editor/Sensors/ArticulationBodySensorTests.cs | 6 ------ 1 file changed, 6 deletions(-) diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs index 1b33603b80..33d23d4697 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/ArticulationBodySensorTests.cs @@ -49,12 +49,9 @@ public void TestBodiesWithJoint() { var rootObj = new GameObject(); var rootArticBody = rootObj.AddComponent(); - // See if there's a way to set velocity directly. - //rootArticBody.velocity = new Vector3(1f, 0f, 0f); var middleGamObj = new GameObject(); var middleArticBody = middleGamObj.AddComponent(); - //middleArticBody.velocity = new Vector3(0f, 1f, 0f); middleArticBody.AddForce(new Vector3(0f, 1f, 0f)); middleGamObj.transform.SetParent(rootObj.transform); middleGamObj.transform.localPosition = new Vector3(13.37f, 0f, 0f); @@ -62,7 +59,6 @@ public void TestBodiesWithJoint() var leafGameObj = new GameObject(); var leafArticBody = leafGameObj.AddComponent(); - //leafArticBody.velocity = new Vector3(0f, 0f, 1f); leafGameObj.transform.SetParent(middleGamObj.transform); leafGameObj.transform.localPosition = new Vector3(4.2f, 0f, 0f); leafArticBody.jointType = ArticulationJointType.RevoluteJoint; @@ -85,8 +81,6 @@ public void TestBodiesWithJoint() #endif }; - - var sensor = sensorComponent.CreateSensor(); sensor.Update(); var expected = new[] From 1ecd2b836bc352b8f33da52c1da2b4be22cd17b9 Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Thu, 9 Jul 2020 16:29:34 -0700 Subject: [PATCH 11/12] GetLinearVelocityAt abstract --- .../Runtime/Sensors/PoseExtractor.cs | 5 +---- .../Tests/Editor/Sensors/PoseExtractorTests.cs | 11 +++++++++++ 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index 9ce456e44e..eb26b06f78 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -105,10 +105,7 @@ protected void SetParentIndices(int[] parentIndices) /// /// /// - protected virtual Vector3 GetLinearVelocityAt(int index) - { - return Vector3.zero; - } + protected abstract Vector3 GetLinearVelocityAt(int index); /// diff --git a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/PoseExtractorTests.cs b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/PoseExtractorTests.cs index b140e1f607..627b4d7b8b 100644 --- a/com.unity.ml-agents.extensions/Tests/Editor/Sensors/PoseExtractorTests.cs +++ b/com.unity.ml-agents.extensions/Tests/Editor/Sensors/PoseExtractorTests.cs @@ -13,6 +13,11 @@ protected override Pose GetPoseAt(int index) return Pose.identity; } + protected override Vector3 GetLinearVelocityAt(int index) + { + return Vector3.zero; + } + public void Init(int[] parentIndices) { SetParentIndices(parentIndices); @@ -68,6 +73,12 @@ protected override Pose GetPoseAt(int index) position = translation }; } + + protected override Vector3 GetLinearVelocityAt(int index) + { + return Vector3.zero; + } + } [Test] From 5435ff9118e0d7202c70f5a076bd3dc76089f6ee Mon Sep 17 00:00:00 2001 From: Chris Elion Date: Fri, 10 Jul 2020 13:43:41 -0700 Subject: [PATCH 12/12] cleanup, handle root GameObj --- .../Runtime/Sensors/PhysicsBodySensor.cs | 4 ++-- .../Runtime/Sensors/PoseExtractor.cs | 9 +++++---- .../Runtime/Sensors/RigidBodyPoseExtractor.cs | 13 +++++++++++-- .../Runtime/Sensors/RigidBodySensorComponent.cs | 4 ++-- 4 files changed, 20 insertions(+), 10 deletions(-) diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs index 7cc1d60ad2..6b0bb2ca0f 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PhysicsBodySensor.cs @@ -20,9 +20,9 @@ public class PhysicsBodySensor : ISensor /// /// /// - public PhysicsBodySensor(Rigidbody rootBody, PhysicsSensorSettings settings, string sensorName=null) + public PhysicsBodySensor(Rigidbody rootBody, GameObject rootGameObject, PhysicsSensorSettings settings, string sensorName=null) { - m_PoseExtractor = new RigidBodyPoseExtractor(rootBody); + m_PoseExtractor = new RigidBodyPoseExtractor(rootBody, rootGameObject); m_SensorName = string.IsNullOrEmpty(sensorName) ? $"PhysicsBodySensor:{rootBody?.name}" : sensorName; m_Settings = settings; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs index eb26b06f78..03902442ec 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/PoseExtractor.cs @@ -118,14 +118,15 @@ public void UpdateModelSpacePoses() return; } - var worldTransform = GetPoseAt(0); - var worldToModel = worldTransform.Inverse(); + var rootWorldTransform = GetPoseAt(0); + var worldToModel = rootWorldTransform.Inverse(); var rootLinearVel = GetLinearVelocityAt(0); for (var i = 0; i < m_ModelSpacePoses.Length; i++) { - var currentTransform = GetPoseAt(i); - m_ModelSpacePoses[i] = worldToModel.Multiply(currentTransform); + var currentWorldSpacePose = GetPoseAt(i); + var currentModelSpacePose = worldToModel.Multiply(currentWorldSpacePose); + m_ModelSpacePoses[i] = currentModelSpacePose; var currentBodyLinearVel = GetLinearVelocityAt(i); var relativeVelocity = currentBodyLinearVel - rootLinearVel; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs index b64c42c7ec..9036cbc4e5 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodyPoseExtractor.cs @@ -16,13 +16,22 @@ public class RigidBodyPoseExtractor : PoseExtractor /// Initialize given a root RigidBody. /// /// - public RigidBodyPoseExtractor(Rigidbody rootBody) + public RigidBodyPoseExtractor(Rigidbody rootBody, GameObject rootGameObject = null) { if (rootBody == null) { return; } - var rbs = rootBody.GetComponentsInChildren (); + + Rigidbody[] rbs; + if (rootGameObject == null) + { + rbs = rootBody.GetComponentsInChildren(); + } + else + { + rbs = rootGameObject.GetComponentsInChildren(); + } var bodyToIndex = new Dictionary(rbs.Length); var parentIndices = new int[rbs.Length]; diff --git a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs index b9bd947f79..88202bbac1 100644 --- a/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs +++ b/com.unity.ml-agents.extensions/Runtime/Sensors/RigidBodySensorComponent.cs @@ -30,7 +30,7 @@ public class RigidBodySensorComponent : SensorComponent /// public override ISensor CreateSensor() { - return new PhysicsBodySensor(RootBody, Settings, sensorName); + return new PhysicsBodySensor(RootBody, gameObject, Settings, sensorName); } /// @@ -43,7 +43,7 @@ public override int[] GetObservationShape() // TODO static method in PhysicsBodySensor? // TODO only update PoseExtractor when body changes? - var poseExtractor = new RigidBodyPoseExtractor(RootBody); + var poseExtractor = new RigidBodyPoseExtractor(RootBody, gameObject); var numTransformObservations = Settings.TransformSize(poseExtractor.NumPoses); return new[] { numTransformObservations }; }