From 4434774dfdde7228d9c1563c194c87af872f7fbf Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 09:38:55 +0900 Subject: [PATCH 01/26] Modify ModelImporter - change 'collision mesh' to 'renderer mesh' for bound calculation --- Assets/Scripts/UI/ModelImporter.cs | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/Assets/Scripts/UI/ModelImporter.cs b/Assets/Scripts/UI/ModelImporter.cs index 08c3b206..9ddfe5ee 100644 --- a/Assets/Scripts/UI/ModelImporter.cs +++ b/Assets/Scripts/UI/ModelImporter.cs @@ -102,10 +102,10 @@ public void SetModelForDeploy(in Transform targetTransform) } var totalBound = new Bounds(); - foreach (var collider in _targetObject.GetComponentsInChildren()) + foreach (var renderer in _targetObject.GetComponentsInChildren()) { - // Debug.Log(collider.bounds.min + ", " + collider.bounds.max); - totalBound.Encapsulate(collider.bounds); + // Debug.Log(renderer.bounds.min + ", " + renderer.bounds.max); + totalBound.Encapsulate(renderer.bounds); } modelDeployOffset.y = DeployOffsetMargin + ((totalBound.min.y < 0) ? -totalBound.min.y : 0); @@ -143,7 +143,7 @@ private bool GetPointAndNormalOnClick(out Vector3 point, out Vector3 normal) return false; } - private void UpdateInitPose() + private void SetInitPose() { var modelHelper = _targetObject.GetComponent(); if (modelHelper != null) @@ -159,7 +159,7 @@ private void CompleteDeployment() _rootArticulationBody.immovable = false; } - UpdateInitPose(); + SetInitPose(); UnblockSelfRaycast(); From 1ba9721b5695b9488fced9f12841879825164488 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 9 Sep 2024 10:17:16 +0900 Subject: [PATCH 02/26] Upgrade Unity editor version -> 2022.3.45f1 (LTS) --- ProjectSettings/ProjectVersion.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 52515b46..29c4eeec 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.44f1 -m_EditorVersionWithRevision: 2022.3.44f1 (c3ae09b9f03c) +m_EditorVersion: 2022.3.45f1 +m_EditorVersionWithRevision: 2022.3.45f1 (a13dfa44d684) From 9c2768cfefd55130b9c78a54dc8b4af346be504e Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 10 Sep 2024 11:59:39 +0900 Subject: [PATCH 03/26] Modify SDF2Unity.GetModelLinkName() - set split options Remove unused namespace --- Assets/Scripts/Devices/Modules/AsyncWork.cs | 3 --- Assets/Scripts/Tools/SDF/Import/Import.Joint.cs | 2 +- Assets/Scripts/Tools/SDF/Parser/Plugin.cs | 2 -- Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs | 4 ++-- 4 files changed, 3 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/AsyncWork.cs b/Assets/Scripts/Devices/Modules/AsyncWork.cs index b8f132e4..d9c4730a 100644 --- a/Assets/Scripts/Devices/Modules/AsyncWork.cs +++ b/Assets/Scripts/Devices/Modules/AsyncWork.cs @@ -4,10 +4,7 @@ * SPDX-License-Identifier: MIT */ -using System; -using Unity.Collections; using UnityEngine.Rendering; -using UnityEngine; namespace SensorDevices { diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index c86bf388..149ca5e9 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -25,7 +25,7 @@ private static UE.Transform FindTransformByName(string name, UE.Transform target rootTransform = rootTransform.parent; } - (var modelName, var linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name); + var (modelName, linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name); // UE.Debug.Log("GetModelLinkName => " + modelName + ", " + linkName); if (string.IsNullOrEmpty(modelName)) diff --git a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs index 65db32e3..eb34f82c 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs @@ -5,8 +5,6 @@ */ using System.Xml; -using System.Xml.Serialization; -using System.IO; namespace SDF { diff --git a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs b/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs index 330ccf99..90a73810 100644 --- a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs +++ b/Assets/Scripts/Tools/SDF/Util/SDF2Unity.Model.cs @@ -8,14 +8,14 @@ public partial class SDF2Unity { - public static (string, string) GetModelLinkName(in string value, in string defaultModelName = "") + public static (string, string) GetModelLinkName(in string value, in string defaultModelName = "__default__") { var modelName = defaultModelName; var linkName = value; if (value.Contains("::")) { - var splittedName = value.Replace("::", ":").Split(':'); + var splittedName = value.Split("::", System.StringSplitOptions.RemoveEmptyEntries); modelName = splittedName[0]; linkName = splittedName[1]; } From 797478173cef7ffdbc7dbc16cffad007d55d024f Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 10 Sep 2024 12:05:49 +0900 Subject: [PATCH 04/26] Modify DeviceMessage - remove Reset() calling after deserialize message in GetMessage() - set default(T) value when deserialize functio throw the exception --- Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs b/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs index bad39ca7..3adc9bb2 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs @@ -56,11 +56,16 @@ public T GetMessage() lock (this) { Seek(0, SeekOrigin.Begin); - result = Serializer.Deserialize(this); + try + { + result = Serializer.Deserialize(this); + } + catch (Exception) + { + result = default(T); + } } - Reset(); - return result; } From 7a6bd55f70d0332dc5c35dbcb91e34bd5fa6787e Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Tue, 10 Sep 2024 12:08:07 +0900 Subject: [PATCH 05/26] Minor refactoring -> MicomPlugin --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 94 +++++++++++--------- 1 file changed, 54 insertions(+), 40 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 21ec92fa..70e07308 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -6,6 +6,7 @@ using System.Collections.Generic; using System.Text; +using System.Linq; using Any = cloisim.msgs.Any; using UnityEngine; @@ -82,45 +83,9 @@ private void SetupMicom() } _micomSensor.SetUpdateRate(updateRate); - var wheelRadius = GetPluginParameters().GetValue("wheel/radius"); - - if (GetPluginParameters().IsValidNode("wheel/tread")) + if (GetPluginParameters().IsValidNode("wheel")) { - Debug.LogWarning(" will be depreacted!! please use wheel/separation"); - } - - var wheelTread = GetPluginParameters().GetValue("wheel/tread"); // TODO: to be deprecated - var wheelSeparation = GetPluginParameters().GetValue("wheel/separation", wheelTread); - - _motorControl.SetWheelInfo(wheelRadius, wheelSeparation); - - var wheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='left']", string.Empty); - var wheelRightName = GetPluginParameters().GetValue("wheel/location[@type='right']", string.Empty); - var rearWheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='rear_left']", string.Empty); - var rearWheelRightName = GetPluginParameters().GetValue("wheel/location[@type='rear_right']", string.Empty); - - if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty)) - { - SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName); - } - else - { - SetWheel(wheelLeftName, wheelRightName); - } - - if (GetPluginParameters().IsValidNode("wheel/PID")) - { - var P = GetPluginParameters().GetValue("wheel/PID/kp"); - var I = GetPluginParameters().GetValue("wheel/PID/ki"); - var D = GetPluginParameters().GetValue("wheel/PID/kd"); - - var iMin = GetPluginParameters().GetValue("wheel/PID/limit/integral/min", -100); - var iMax = GetPluginParameters().GetValue("wheel/PID/limit/integral/max", 100); - var outputMin = GetPluginParameters().GetValue("wheel/PID/limit/output/min", -1000); - var outputMax = GetPluginParameters().GetValue("wheel/PID/limit/output/max", 1000); - // Debug.Log(iMin + ", " + iMax + ", " + outputMin + ", " + outputMax); - - _motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax); + SetWheel(); } if (GetPluginParameters().IsValidNode("battery")) @@ -156,6 +121,55 @@ private void SetupMicom() } } + private void SetWheel() + { + var wheelRadius = GetPluginParameters().GetValue("wheel/radius"); + + if (GetPluginParameters().IsValidNode("wheel/tread")) + { + Debug.LogWarning(" will be depreacted!! please use wheel/separation"); + } + + var wheelTread = GetPluginParameters().GetValue("wheel/tread"); // TODO: to be deprecated + var wheelSeparation = GetPluginParameters().GetValue("wheel/separation", wheelTread); + + _motorControl.SetWheelInfo(wheelRadius, wheelSeparation); + + var wheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='left']", string.Empty); + var wheelRightName = GetPluginParameters().GetValue("wheel/location[@type='right']", string.Empty); + var rearWheelLeftName = GetPluginParameters().GetValue("wheel/location[@type='rear_left']", string.Empty); + var rearWheelRightName = GetPluginParameters().GetValue("wheel/location[@type='rear_right']", string.Empty); + + if (!rearWheelLeftName.Equals(string.Empty) && !rearWheelRightName.Equals(string.Empty)) + { + SetWheel(wheelLeftName, wheelRightName, rearWheelLeftName, rearWheelRightName); + } + else + { + SetWheel(wheelLeftName, wheelRightName); + } + + if (GetPluginParameters().IsValidNode("wheel/PID")) + { + SetWheelPID(); + } + } + + private void SetWheelPID() + { + var P = GetPluginParameters().GetValue("wheel/PID/kp"); + var I = GetPluginParameters().GetValue("wheel/PID/ki"); + var D = GetPluginParameters().GetValue("wheel/PID/kd"); + + var iMin = GetPluginParameters().GetValue("wheel/PID/limit/integral/min", -100); + var iMax = GetPluginParameters().GetValue("wheel/PID/limit/integral/max", 100); + var outputMin = GetPluginParameters().GetValue("wheel/PID/limit/output/min", -1000); + var outputMax = GetPluginParameters().GetValue("wheel/PID/limit/output/max", 1000); + // Debug.Log(iMin + ", " + iMax + ", " + outputMin + ", " + outputMax); + + _motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax); + } + private void SetBattery() { if (GetPluginParameters().GetValues("battery/voltage", out var batteryList)) @@ -256,7 +270,7 @@ private void LoadStaticTF() { var parentFrameId = GetPluginParameters().GetAttributeInPath("ros2/static_transforms/link[text()='" + link + "']", "parent_frame_id", "base_link"); - (var modelName, var linkName) = SDF2Unity.GetModelLinkName(link); + var (modelName, linkName) = SDF2Unity.GetModelLinkName(link); foreach (var linkHelper in _linkHelperInChildren) { @@ -287,7 +301,7 @@ private void LoadTF() { var parentFrameId = GetPluginParameters().GetAttributeInPath("ros2/transforms/link[text()='" + link + "']", "parent_frame_id", "base_link"); - (var modelName, var linkName) = SDF2Unity.GetModelLinkName(link); + var (modelName, linkName) = SDF2Unity.GetModelLinkName(link); foreach (var linkHelper in _linkHelperInChildren) { From 7eac00d745eeafb7a064bcdfcf58a33e6574b766 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 11 Sep 2024 10:43:09 +0900 Subject: [PATCH 06/26] Modify MowingPlugin - Support MowingBlade Modify MicomCommand - interface for mowing blade control Add new module for MowingPlugin - MowingBlade on/off, adjustable blade height Modify TextureUtil - Add Filling texture options Modify GeometryGrass shader - apply ratio by visibility for blade max height --- Assets/Resources/Shader/GeometryGrass.shader | 4 +- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 26 +++ Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs | 184 +++++++----------- Assets/Scripts/Devices/MicomCommand.cs | 66 +++++-- Assets/Scripts/Devices/Modules/MowingBlade.cs | 138 +++++++++++++ .../Devices/Modules/MowingBlade.cs.meta | 11 ++ Assets/Scripts/Tools/Mesh/TextureUtil.cs | 42 +++- 7 files changed, 340 insertions(+), 131 deletions(-) create mode 100644 Assets/Scripts/Devices/Modules/MowingBlade.cs create mode 100644 Assets/Scripts/Devices/Modules/MowingBlade.cs.meta diff --git a/Assets/Resources/Shader/GeometryGrass.shader b/Assets/Resources/Shader/GeometryGrass.shader index 50defb28..622f9257 100644 --- a/Assets/Resources/Shader/GeometryGrass.shader +++ b/Assets/Resources/Shader/GeometryGrass.shader @@ -358,8 +358,8 @@ Shader "Custom/GeometryGrass" #endif float width = lerp(_BladeWidthMin, _BladeWidthMax, rand(pos.xzy) * falloff); - float height = lerp(_BladeHeightMin, _BladeHeightMax, rand(pos.zyx) * falloff); - float forward = rand(pos.yyz) * _BladeBendDistance; + float height = lerp(_BladeHeightMin, _BladeHeightMax * grassVisibility, rand(pos.zyx) * falloff); + float forward = rand(pos.yyz) * _BladeBendDistance * grassVisibility; #ifdef DRY_GRASS_ON float dryRate = tex2Dlod(_DryGrassMap, float4(-input[0].uv, 0, 0)).r; diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 70e07308..4da9ca35 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -7,6 +7,7 @@ using System.Collections.Generic; using System.Text; using System.Linq; +using System; using Any = cloisim.msgs.Any; using UnityEngine; @@ -88,6 +89,11 @@ private void SetupMicom() SetWheel(); } + if (GetPluginParameters().IsValidNode("mowing")) + { + SetMowing(); + } + if (GetPluginParameters().IsValidNode("battery")) { SetBattery(); @@ -170,6 +176,26 @@ private void SetWheelPID() _motorControl.SetPID(P, I, D, iMin, iMax, outputMin, outputMax); } + private void SetMowing() + { + var targetBladeName = GetPluginParameters().GetAttributeInPath("mowing/blade", "target"); + if (!string.IsNullOrEmpty(targetBladeName)) + { + var linkHelpers = GetComponentsInChildren(); + var targetBlade = linkHelpers.FirstOrDefault(x => x.name == targetBladeName); + + if (targetBlade != null) + { + var mowingBlade = targetBlade.gameObject.AddComponent(); + + mowingBlade.HeightMin = GetPluginParameters().GetValue("mowing/blade/height/min", 0f); + mowingBlade.HeightMax = GetPluginParameters().GetValue("mowing/blade/height/max", 0.1f); + mowingBlade.RevSpeedMax = GetPluginParameters().GetValue("mowing/blade/rev_speed/max", 1000); + mowingBlade.Height = 0; + } + } + } + private void SetBattery() { if (GetPluginParameters().GetValues("battery/voltage", out var batteryList)) diff --git a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs index a8920052..7e29a091 100644 --- a/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MowingPlugin.cs @@ -13,36 +13,6 @@ [DefaultExecutionOrder(800)] public class MowingPlugin : CLOiSimPlugin { - public struct Target - { - public string modelName; - public string linkName; - - public Target(in string model, in string link) - { - modelName = model; - linkName = link; - } - } - - private class Blade - { - public float ratio = 0.1f; - - public Target target; - - public float size = 0; - - public float heightThreshold = 0; - - public void SetBladingRatio(in float bladeMin, in float bladeMax, in float bladingHeight) - { - this.heightThreshold = bladingHeight; - this.ratio = Mathf.Lerp(bladeMin, bladeMax, bladingHeight); - // Debug.Log("_bladingRatio: " + this.ratio); - } - } - private class Grass { public struct Blade @@ -55,13 +25,14 @@ public Blade(in float min, in float max) heightMin = min; heightMax = max; } - } + public string modelName; + public string linkName; + public float mapResolution = 0.05f; // m/pixel public Blade blade = new Blade(-1, -1); - public Target targetPlane; public Material material = null; public Bounds bounds = new Bounds(); public Texture2D texture = null; @@ -193,14 +164,13 @@ public Vector3 GetGrassOffset() } private Grass _grass = null; - private Blade _blade = null; + private MowingBlade _mowingBlade = null; private Color[] _initialTexturePixels = null; private Transform _targetPlane = null; - private Transform _targetBlade = null; private List _punchingMeshFilters = new List(); - private bool _startMowing = true; + protected override void OnAwake() { @@ -211,7 +181,6 @@ protected override void OnAwake() var geomGrassShader = Shader.Find("Custom/GeometryGrass"); _grass = new Grass(geomGrassShader); - _blade = new Blade(); } protected override void OnStart() @@ -224,11 +193,8 @@ private IEnumerator Start() yield return new WaitForEndOfFrame(); var grassTarget = GetPluginParameters().GetValue("grass/target"); - var grassTragetSplit = grassTarget.Split("::"); - - _grass.targetPlane = new Target(grassTragetSplit[0], grassTragetSplit[1]); - if (FindTargetPlane(_grass.targetPlane)) + if (FindTargetPlane(grassTarget)) { PlantGrass(); } @@ -238,11 +204,7 @@ private IEnumerator Start() } var bladeTarget = GetPluginParameters().GetValue("mowing/blade/target"); - var bladeTargetSplit = bladeTarget.Split("::"); - - _blade.target = new Target(bladeTargetSplit[0], bladeTargetSplit[1]); - - if (FindTargetBlade(_blade.target) == false) + if (FindTargetBlade(bladeTarget) == false) { Debug.LogWarning("Target blade not found"); } @@ -258,13 +220,15 @@ protected override void OnReset() } } - private bool FindTargetPlane(Target targetPlane) + private bool FindTargetPlane(in string targetPlane) { + (_grass.modelName, _grass.linkName) = SDF2Unity.GetModelLinkName(targetPlane); + var modelHelpers = GetComponentsInChildren(); - var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetPlane.modelName); + var targetModel = modelHelpers.FirstOrDefault(x => x.name == _grass.modelName); _targetPlane = targetModel?.GetComponentsInChildren() - .FirstOrDefault(x => x.name == targetPlane.linkName)?.transform; + .FirstOrDefault(x => x.name == _grass.linkName)?.transform; var targetPlaneCollision = _targetPlane?.GetComponentsInChildren() @@ -273,54 +237,45 @@ var targetPlaneCollision return targetPlaneCollision != null; } - private bool FindTargetBlade(Target targetBlade) + private bool FindTargetBlade(in string targetBlade) { + var (targetBladeModelName, targetBladeLinkName) = SDF2Unity.GetModelLinkName(targetBlade); + var modelHelpers = GetComponentsInChildren(); - var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetBlade.modelName); + var targetModel = modelHelpers.FirstOrDefault(x => x.name == targetBladeModelName); - _targetBlade = targetModel?.GetComponentsInChildren() - .FirstOrDefault(x => x.name == targetBlade.linkName)?.transform; + var targetBladeLinkHelper = targetModel?.GetComponentsInChildren() + .FirstOrDefault(x => x.name == targetBladeLinkName)?.transform; - if (_targetBlade == null) + if (targetBladeLinkHelper == null) { return false; } - var meshFilters = _targetBlade.GetComponentsInChildren(); + _mowingBlade = targetBladeLinkHelper.GetComponent(); - var bladeBounds = new Bounds(); - foreach (var meshFilter in meshFilters) + if (_mowingBlade == null) { - // Debug.Log(meshFilter.name); - // Debug.Log(meshFilter.sharedMesh.bounds); - var bounds = meshFilter.sharedMesh.bounds; - bladeBounds.Encapsulate(bounds); + _mowingBlade = targetBladeLinkHelper.gameObject.AddComponent(); } - // Debug.Log(bladeBounds); - _blade.size = Mathf.Max(bladeBounds.extents.x, bladeBounds.extents.z); - var bladeMin = _targetPlane.TransformPoint(_targetBlade.position); - // Debug.Log(bladeMin.y); - - _blade.SetBladingRatio(_grass.blade.heightMin, _grass.blade.heightMax, bladeMin.y); - return true; } private void PlantGrass() { - if (_targetPlane == null) - return; - - var targetPlaneMesh = _targetPlane?.GetComponentInChildren(); - _grass.SetMaterial(GetPluginParameters()); - _grass.SetGrassOffset(_targetPlane.position); - _grass.SetBound(targetPlaneMesh.sharedMesh); - _grass.Generate(); + if (_targetPlane != null) + { + var targetPlaneMesh = _targetPlane?.GetComponentInChildren(); + _grass.SetMaterial(GetPluginParameters()); + _grass.SetGrassOffset(_targetPlane.position); + _grass.SetBound(targetPlaneMesh.sharedMesh); + _grass.Generate(); - StartCoroutine(PunchingGrass()); + StartCoroutine(PunchingGrass()); - AssignMaterial(); + AssignMaterial(); + } } private IEnumerator PunchingGrass() @@ -339,7 +294,7 @@ private IEnumerator PunchingGrass() foreach (var meshFilter in _punchingMeshFilters) { - PunchingTexture(_grass.texture, meshFilter); + PunchingTexture(ref _grass.texture, meshFilter); yield return null; } @@ -364,7 +319,6 @@ private void CreateTempColliderInVisuals(ref List tempMeshCollider foreach (var meshFilter in meshFilters) { var meshCollider = meshFilter.transform.gameObject.AddComponent(); - // Debug.Log(helperVisual.name + "," + meshFilter.name + "," + meshCollider.name); meshCollider.convex = true; meshCollider.isTrigger = true; tempMeshColliders.Add(meshCollider); @@ -376,10 +330,10 @@ private void CreateTempColliderInVisuals(ref List tempMeshCollider private IEnumerator FindMeshFiltersToPunching() { - yield return null; - var layerMask = LayerMask.GetMask("Default"); + yield return null; + var hitColliders = Physics.OverlapBox(_grass.bounds.center, _grass.bounds.extents, Quaternion.identity, layerMask); var i = 0; while (i < hitColliders.Length) @@ -390,11 +344,11 @@ private IEnumerator FindMeshFiltersToPunching() if (helperModel != null) { - // punching other object on same target model - if (helperModel.name.Equals(_grass.targetPlane.modelName)) + // punching other object on same target model + if (helperModel.name.Equals(_grass.modelName)) { var helperLink = hitCollider.GetComponentInParent(); - if (helperLink != null && !helperLink.name.Equals(_grass.targetPlane.linkName)) + if (helperLink != null && !helperLink.name.Equals(_grass.linkName)) { var meshFilters = helperLink.GetComponentsInChildren(); _punchingMeshFilters.AddRange(meshFilters); @@ -414,6 +368,7 @@ private IEnumerator FindMeshFiltersToPunching() _punchingMeshFilters.AddRange(meshFilters); } } + yield return null; } } @@ -436,7 +391,7 @@ private void AssignMaterial() targetPlaneMeshRenderer.materials = newMaterials; } - private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) + private void PunchingTexture(ref Texture2D texture, in MeshFilter meshFilter) { var textureCenterX = (int)(texture.width * 0.5f); var textureCenterY = (int)(texture.height * 0.5f); @@ -447,9 +402,6 @@ private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) var vertices = mesh.vertices; var triangles = mesh.triangles; - // Debug.Log(triangles.Length); - // Debug.Log(vertices.Length); - for (var i = 0; i < triangles.Length; i += 3) { var p0 = vertices[triangles[i + 0]]; @@ -459,61 +411,65 @@ private void PunchingTexture(Texture2D texture, MeshFilter meshFilter) var tp1 = meshFilter.transform.TransformPoint(p1) - offset; var tp2 = meshFilter.transform.TransformPoint(p2) - offset; - // Debug.Log("Punch >> " + tp0.y + ", " + tp1.y + ", " + tp2.y); - { - var P1 = new Vector2(tp0.z, tp0.x); - var P2 = new Vector2(tp1.z, tp1.x); - var P3 = new Vector2(tp2.z, tp2.x); + var P1 = new Vector2(tp0.z, tp0.x); + var P2 = new Vector2(tp1.z, tp1.x); + var P3 = new Vector2(tp2.z, tp2.x); - P1 /= _grass.mapResolution; - P2 /= _grass.mapResolution; - P3 /= _grass.mapResolution; + P1 /= _grass.mapResolution; + P2 /= _grass.mapResolution; + P3 /= _grass.mapResolution; - P1 += textureCenter; - P2 += textureCenter; - P3 += textureCenter; + P1 += textureCenter; + P2 += textureCenter; + P3 += textureCenter; - texture.FillTriangle(P1, P2, P3, Color.clear); - } + texture.FillTriangle(P1, P2, P3, Color.clear); } } private IEnumerator StartMowing() { - const float mowingMargin = 0.01f; - yield return null; - var threshold = _blade.heightThreshold + mowingMargin; - var color = new Color(_blade.ratio, 0, 0, 0); + var mowingThreshold = _grass.blade.heightMax; + var mowingRatioInColor = Color.clear; var planeCenterPosition = _targetPlane.position; - var bladeRadiusIntexture = _blade.size /_grass.mapResolution; + var bladeRadiusIntexture = _mowingBlade.Diameter /_grass.mapResolution; - // Debug.Log("blade threshold= " + threshold + ", bladeRadiusIntexture=" + bladeRadiusIntexture); while (true) { - if (_startMowing && _targetBlade != null) + if (_mowingBlade.IsRunning() && _mowingBlade != null) { - var bladePositionInTexture = _targetBlade.position; + var bladeToPlaneDistance = _targetPlane.TransformPoint(_mowingBlade.Position); - // Debug.Log("blade " + bladePositionInTexture.y.ToString("F4") + " threshold= " + threshold); - if (bladePositionInTexture.y <= threshold) + if (bladeToPlaneDistance.y <= mowingThreshold) { + mowingRatioInColor.r = bladeToPlaneDistance.y / mowingThreshold; + // Debug.Log($"{_grass.blade.heightMin}, {_grass.blade.heightMax}, {bladeToPlaneDistance.y} => {mowingRatioInColor.r }"); + + var bladePositionInTexture = _mowingBlade.Position; bladePositionInTexture -= planeCenterPosition; bladePositionInTexture += _grass.bounds.extents; bladePositionInTexture /= _grass.mapResolution; - // Debug.Log(bladePositionInTexture); _grass.texture.FillCircle( bladePositionInTexture.z, bladePositionInTexture.x, bladeRadiusIntexture, - color); + mowingRatioInColor, + TextureUtil.FillOptions.Lesser); yield return null; } + else + { + yield return new WaitForEndOfFrame(); + } + } + else + { + yield return new WaitForEndOfFrame(); } - yield return new WaitForEndOfFrame(); } } } diff --git a/Assets/Scripts/Devices/MicomCommand.cs b/Assets/Scripts/Devices/MicomCommand.cs index ee7b85ee..22cd7ab0 100644 --- a/Assets/Scripts/Devices/MicomCommand.cs +++ b/Assets/Scripts/Devices/MicomCommand.cs @@ -12,6 +12,7 @@ namespace SensorDevices public class MicomCommand : Device { private MotorControl _motorControl = null; + private MowingBlade _mowingBlade = null; protected override void OnAwake() { @@ -21,6 +22,7 @@ protected override void OnAwake() protected override void OnStart() { + _mowingBlade = GetComponentInChildren(); } protected override void OnReset() @@ -35,22 +37,39 @@ public void SetMotorControl(in MotorControl motorControl) protected override void ProcessDevice() { - if (PopDeviceMessage(out var micomWritingData)) + if (PopDeviceMessage(out var receivedMessage)) { - var linear = micomWritingData.Linear; - var angular = micomWritingData.Angular; + var cmdVelocity = receivedMessage.GetMessage(); - var linearVelocity = SDF2Unity.Position(linear.X, linear.Y, linear.Z); - var angularVelocity = SDF2Unity.Position(angular.X, angular.Y, angular.Z); + if (cmdVelocity != null) + { + var linear = cmdVelocity.Linear; + var angular = cmdVelocity.Angular; - DoWheelDrive(linearVelocity, angularVelocity); - } + var linearVelocity = SDF2Unity.Position(linear.X, linear.Y, linear.Z); + var angularVelocity = SDF2Unity.Position(angular.X, angular.Y, angular.Z); + + DoWheelDrive(linearVelocity, angularVelocity); + } + else + { + var cmdMowing = receivedMessage.GetMessage(); + + if (cmdMowing != null) + { + if (!string.IsNullOrEmpty(cmdMowing.Name)) + { + ControlMowing(cmdMowing.Name, cmdMowing.Value); + } + } #if UNITY_EDITOR - else - { - Debug.LogWarning("ERROR: failed to pop device message"); - } + else + { + Debug.LogWarning("ERROR: failed to pop device message"); + } #endif + } + } } /// m/s @@ -65,7 +84,32 @@ private void DoWheelDrive(in Vector3 linearVelocity, in Vector3 angularVelocity) var targetLinearVelocity = linearVelocity.z; var targetAngularVelocity = angularVelocity.y; + _motorControl.SetTwistDrive(targetLinearVelocity, targetAngularVelocity); } + + private void ControlMowing(in string target, in cloisim.msgs.Any value) + { + if (string.Compare(target, "mowing_blade_height") == 0) + { + if (_mowingBlade != null && value.Type == messages.Any.ValueType.Double) + { + _mowingBlade.Height = (float)value.DoubleValue; + // Debug.Log($"mowing_blade_height {value} -> {_mowingBlade.Height}"); + } + } + else if (string.Compare(target, "mowing_blade_rev_speed") == 0) + { + if (_mowingBlade != null && value.Type == messages.Any.ValueType.Int32) + { + _mowingBlade.RevSpeed = System.Convert.ToUInt16(value.IntValue); + // Debug.Log($"mowing_blade_rev_speed {value} -> {_mowingBlade.RevSpeed}"); + } + } + else + { + Debug.LogWarning($"Invalid Control Mowing message received: {target}"); + } + } } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MowingBlade.cs b/Assets/Scripts/Devices/Modules/MowingBlade.cs new file mode 100644 index 00000000..65431530 --- /dev/null +++ b/Assets/Scripts/Devices/Modules/MowingBlade.cs @@ -0,0 +1,138 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using System.Collections.Generic; +using System.Collections; +using System; +using UnityEngine; + +public class MowingBlade : MonoBehaviour +{ + [SerializeField] + private float _height = 0; + + [SerializeField] + private UInt16 _revSpeed = 0; + + [SerializeField] + private float _heightMin = 0; + + [SerializeField] + private float _heightMax = 0; + + [SerializeField] + private UInt16 _revSpeedMax = 0; + + [SerializeField] + private float _heightBaseInTransform = 0; + + private Bounds _bladeBounds = new Bounds(); + + private bool _doAdjust = false; + + public float Diameter + { + get => Mathf.Max(_bladeBounds.extents.x, _bladeBounds.extents.z); + } + + public float HeightMin + { + get => _heightMin; + set => _heightMin = value; + } + + public float HeightMax + { + get => _heightMax; + set => _heightMax = value; + } + + public UInt16 RevSpeedMax + { + get => _revSpeedMax; + set => _revSpeedMax = value; + } + + public float Height + { + get => _height; + set => _height = (value <= _heightMin) ? _heightMin : (value >= _heightMax ? _heightMax : value); + } + + public UInt16 RevSpeed + { + get => _revSpeed; + set => _revSpeed = (value >= _revSpeedMax ? _revSpeedMax : value); + } + + public Vector3 Position + { + get => this.transform.position; + } + + public bool IsRunning() + { + return _revSpeed > float.Epsilon; + } + + void Start() + { + StartCoroutine(CalculateBladeSize()); + } + + void OnDestroy() + { + _doAdjust = false; + } + + private IEnumerator CalculateBladeSize() + { + var meshFilters = GetComponentsInChildren(); + + _bladeBounds.center = Vector3.zero; + _bladeBounds.size = Vector3.zero; + foreach (var meshFilter in meshFilters) + { + var bounds = meshFilter.sharedMesh.bounds; + _bladeBounds.Encapsulate(bounds); + } + + _doAdjust = true; + yield return DoAdjust(); + } + + private IEnumerator DoAdjust() + { + var targets = new List(); + var indices = new List(); + var articulationBody = GetComponent(); + + while (_doAdjust) + { + var dof = articulationBody.GetDriveTargets(targets); + articulationBody.GetDofStartIndices(indices); + if (dof > 0) + { + var bodyIndex = articulationBody.index; + var targetIndex = indices[bodyIndex]; + var currentTarget = targets[targetIndex]; + var targetHeight = articulationBody.yDrive.lowerLimit + _height; + if (targetHeight >= articulationBody.yDrive.upperLimit) + { + targetHeight = articulationBody.yDrive.upperLimit; + } + + if (!Mathf.Approximately(currentTarget, targetHeight)) + { + articulationBody.SetDriveTarget(ArticulationDriveAxis.Y, targetHeight); + } + } + yield return null; + } + + yield return null; + } +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta b/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta new file mode 100644 index 00000000..845b2043 --- /dev/null +++ b/Assets/Scripts/Devices/Modules/MowingBlade.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: de13c3d505bb498458879e613e126aef +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Tools/Mesh/TextureUtil.cs b/Assets/Scripts/Tools/Mesh/TextureUtil.cs index 04e8c495..87d36dad 100644 --- a/Assets/Scripts/Tools/Mesh/TextureUtil.cs +++ b/Assets/Scripts/Tools/Mesh/TextureUtil.cs @@ -11,6 +11,13 @@ public static class TextureUtil { + public enum FillOptions + { + Overwrite, + Lesser, + Greater + }; + public static void Clear(this Texture2D texture) { Clear(texture, Color.clear); @@ -47,12 +54,16 @@ public static void Fill(this Texture2D texture, ref Color[] colors) texture.SetPixels(colors); } - public static void FillCircle(this Texture2D texture, in float x, in float y, in float radius, in Color color) + public static void FillCircle( + this Texture2D texture, in float x, in float y, in float radius, in Color color, + in FillOptions option = FillOptions.Overwrite) { - FillCircle(texture, (int)x, (int)y, (int)radius, color); + FillCircle(texture, (int)x, (int)y, (int)radius, color, option); } - public static void FillCircle(this Texture2D texture, in int x, in int y, in int radius, in Color color) + public static void FillCircle( + this Texture2D texture, in int x, in int y, in int radius, in Color color, + in FillOptions option = FillOptions.Overwrite) { var rSquared = radius * radius; @@ -67,7 +78,30 @@ public static void FillCircle(this Texture2D texture, in int x, in int y, in int { if ((x - u) * (x - u) + (y - v) * (y - v) < rSquared) { - texture.SetPixel(u, v, color); + var pixelColor = texture.GetPixel(u, v); + switch (option) + { + case FillOptions.Lesser: + pixelColor.r = (pixelColor.r < color.r) ? pixelColor.r : color.r; + pixelColor.g = (pixelColor.g < color.g) ? pixelColor.g : color.g; + pixelColor.b = (pixelColor.b < color.b) ? pixelColor.b : color.b; + pixelColor.a = (pixelColor.a < color.a) ? pixelColor.a : color.a; + break; + + case FillOptions.Greater: + pixelColor.r = (pixelColor.r > color.r) ? pixelColor.r : color.r; + pixelColor.g = (pixelColor.g > color.g) ? pixelColor.g : color.g; + pixelColor.b = (pixelColor.b > color.b) ? pixelColor.b : color.b; + pixelColor.a = (pixelColor.a > color.a) ? pixelColor.a : color.a; + break; + + default: + case FillOptions.Overwrite: + pixelColor = color; + break; + } + + texture.SetPixel(u, v, pixelColor); } } } From 44743e0a686a5362664770436f00879d54fa9554 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 09:41:02 +0900 Subject: [PATCH 07/26] remove deprecated field for SDF.Joint --- Assets/Scripts/Tools/SDF/Parser/Joint.cs | 2 -- 1 file changed, 2 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Parser/Joint.cs b/Assets/Scripts/Tools/SDF/Parser/Joint.cs index 7e5b0c2c..63d61352 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Joint.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Joint.cs @@ -43,8 +43,6 @@ public bool HasJoint() } } - public double initial_position = 0.0; - public Vector3 xyz = new Vector3(0, 0, 1); public bool use_parent_model_frame = false; From 6e2391d2ec555bf364f34fbb1aa2d60d5f85ebb1 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 09:50:58 +0900 Subject: [PATCH 08/26] Meaningless behavior removed in SDF.Import.Joint --- Assets/Scripts/Tools/SDF/Import/Import.Joint.cs | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index 149ca5e9..9ddab212 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -166,16 +166,6 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec } linkHelper.SetJointPoseTarget(axisSpringReference, axis2SpringReference); - - // set adjusted position for pose control - var localPosition = linkHelper.transform.localPosition; - var localRotation = linkHelper.transform.localRotation; - linkHelper.SetPose(localPosition, localRotation); - - var modelHelper = linkHelper.Model; - localPosition = modelHelper.transform.localPosition; - localRotation = modelHelper.transform.localRotation; - modelHelper.SetPose(localPosition, localRotation); } } } From f39ca25c20cc6b405690a148bbc1b85184208d1a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 10:30:46 +0900 Subject: [PATCH 09/26] Modify SDF.Implement.Joint - apply extension method --- Assets/Scripts/Tools/Mesh/TextureUtil.cs | 2 +- .../Tools/SDF/Implement/Implement.Joint.cs | 54 ++++++++++--------- .../Scripts/Tools/SDF/Import/Import.Joint.cs | 14 ++--- 3 files changed, 37 insertions(+), 33 deletions(-) diff --git a/Assets/Scripts/Tools/Mesh/TextureUtil.cs b/Assets/Scripts/Tools/Mesh/TextureUtil.cs index 87d36dad..f2705947 100644 --- a/Assets/Scripts/Tools/Mesh/TextureUtil.cs +++ b/Assets/Scripts/Tools/Mesh/TextureUtil.cs @@ -23,7 +23,7 @@ public static void Clear(this Texture2D texture) Clear(texture, Color.clear); } - public static void Clear(this Texture2D texture, Color color) + public static void Clear(this Texture2D texture, in Color color) { Fill(texture, color); } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index a468e1ab..ad8dd4d2 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -10,9 +10,11 @@ namespace SDF { namespace Implement { - public class Joint + public static class Joint { private static float DefaultJointFriction = 0.05f; + private static float DefaultJointLinearDamping = 0.05f; + private static float DefaultJointAngularDamping = 0.05f; public static UE.Pose SetArticulationBodyRelationship(in SDF.Joint joint, UE.Transform linkParent, UE.Transform linkChild) { @@ -66,7 +68,7 @@ public static UE.Pose SetArticulationBodyRelationship(in SDF.Joint joint, UE.Tra return anchorPose; } - public static void SetArticulationBodyAnchor(in UE.ArticulationBody body, in UE.Pose parentAnchor) + public static void SetAnchor(this UE.ArticulationBody body, in UE.Pose parentAnchor) { // UE.Debug.Log(parentAnchor.position); body.anchorPosition = parentAnchor.position; //UE.Vector3.zero; @@ -77,18 +79,18 @@ public static void SetArticulationBodyAnchor(in UE.ArticulationBody body, in UE. // body.parentAnchorRotation = parentAnchor.rotation; // TODO: matchAnchors is set to true } - public static void MakeRevolute(in UE.ArticulationBody body, in SDF.Axis axis) + public static void MakeRevolute(this UE.ArticulationBody body, in SDF.Axis axis) { body.jointType = UE.ArticulationJointType.SphericalJoint; - body.linearDamping = 0.05f; // TODO : value to find - body.angularDamping = 0.05f; // TODO : value to find + body.linearDamping = DefaultJointLinearDamping; + body.angularDamping = DefaultJointAngularDamping; var drive = new UE.ArticulationDrive(); if (axis.limit.HasJoint()) { // UE.Debug.LogWarningFormat("limit uppper{0}, lower{1}", axis.limit.upper, axis.limit.lower); - SetRevoluteArticulationDriveLimit(ref drive, axis.limit); + drive.SetRevoluteDriveLimit(axis.limit); } drive.forceLimit = (double.IsInfinity(axis.limit.effort)) ? float.MaxValue : (float)axis.limit.effort; @@ -112,7 +114,7 @@ public static void MakeRevolute(in UE.ArticulationBody body, in SDF.Axis axis) { if (jointAxis.Equals(UE.Vector3.left)) { - ReverseArticulationBodyAxis(body, UE.Vector3.forward); + body.ReverseAxis(UE.Vector3.forward); } body.xDrive = drive; body.twistLock = (axis.limit.HasJoint()) ? UE.ArticulationDofLock.LimitedMotion : UE.ArticulationDofLock.FreeMotion; @@ -123,7 +125,7 @@ public static void MakeRevolute(in UE.ArticulationBody body, in SDF.Axis axis) { if (jointAxis.Equals(UE.Vector3.down)) { - ReverseArticulationBodyAxis(body, UE.Vector3.right); + body.ReverseAxis(UE.Vector3.right); } body.yDrive = drive; body.twistLock = UE.ArticulationDofLock.LockedMotion; @@ -134,7 +136,7 @@ public static void MakeRevolute(in UE.ArticulationBody body, in SDF.Axis axis) { if (jointAxis.Equals(UE.Vector3.back)) { - ReverseArticulationBodyAxis(body, UE.Vector3.up); + body.ReverseAxis(UE.Vector3.up); } body.zDrive = drive; body.twistLock = UE.ArticulationDofLock.LockedMotion; @@ -147,7 +149,7 @@ public static void MakeRevolute(in UE.ArticulationBody body, in SDF.Axis axis) } } - public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, in SDF.Axis axis2) + public static void MakeRevolute2(this UE.ArticulationBody body, in SDF.Axis axis1, in SDF.Axis axis2) { MakeRevolute(body, axis1); @@ -155,7 +157,7 @@ public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, if (axis2.limit.HasJoint()) { - SetRevoluteArticulationDriveLimit(ref drive, axis2.limit); + drive.SetRevoluteDriveLimit(axis2.limit); } drive.forceLimit = (double.IsInfinity(axis2.limit.effort)) ? float.MaxValue : (float)axis2.limit.effort; @@ -165,7 +167,7 @@ public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, { if (joint2Axis.Equals(UE.Vector3.left)) { - ReverseArticulationBodyAxis(body, UE.Vector3.forward); + body.ReverseAxis(UE.Vector3.forward); } body.xDrive = drive; body.twistLock = (axis2.limit.HasJoint()) ? UE.ArticulationDofLock.LimitedMotion : UE.ArticulationDofLock.FreeMotion; @@ -174,7 +176,7 @@ public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, { if (joint2Axis.Equals(UE.Vector3.down)) { - ReverseArticulationBodyAxis(body, UE.Vector3.right); + body.ReverseAxis(UE.Vector3.right); } body.yDrive = drive; body.swingYLock = (axis2.limit.HasJoint()) ? UE.ArticulationDofLock.LimitedMotion : UE.ArticulationDofLock.FreeMotion; @@ -183,7 +185,7 @@ public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, { if (joint2Axis.Equals(UE.Vector3.back)) { - ReverseArticulationBodyAxis(body, UE.Vector3.up); + body.ReverseAxis(UE.Vector3.up); } body.zDrive = drive; body.swingZLock = (axis2.limit.HasJoint()) ? UE.ArticulationDofLock.LimitedMotion : UE.ArticulationDofLock.FreeMotion; @@ -194,7 +196,7 @@ public static void MakeRevolute2(in UE.ArticulationBody body, in SDF.Axis axis1, } } - public static void MakeFixed(in UE.ArticulationBody body) + public static void MakeFixed(this UE.ArticulationBody body) { body.jointType = UE.ArticulationJointType.FixedJoint; body.linearDamping = 0.00f; @@ -202,25 +204,25 @@ public static void MakeFixed(in UE.ArticulationBody body) body.jointFriction = 0; } - public static void MakeBall(in UE.ArticulationBody body) + public static void MakeBall(this UE.ArticulationBody body) { body.jointType = UE.ArticulationJointType.SphericalJoint; - body.linearDamping = 0.05f; - body.angularDamping = 0.05f; + body.linearDamping = DefaultJointLinearDamping; + body.angularDamping = DefaultJointAngularDamping; body.swingYLock = UE.ArticulationDofLock.FreeMotion; body.swingZLock = UE.ArticulationDofLock.FreeMotion; body.twistLock = UE.ArticulationDofLock.FreeMotion; } - public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, in SDF.Pose pose) + public static void MakePrismatic(this UE.ArticulationBody body, in SDF.Axis axis, in SDF.Pose pose) { body.jointType = UE.ArticulationJointType.PrismaticJoint; body.anchorRotation *= SDF2Unity.Rotation(pose?.Rot); // body.parentAnchorRotation *= SDF2Unity.Rotation(pose?.Rot); // TODO: matchAnchors is set to true - body.linearDamping = 0.05f; - body.angularDamping = 0.05f; + body.linearDamping = DefaultJointLinearDamping; + body.angularDamping = DefaultJointAngularDamping; var drive = new UE.ArticulationDrive(); @@ -251,7 +253,7 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, { if (jointAxis.Equals(UE.Vector3.left)) { - ReverseArticulationBodyAxis(body, UE.Vector3.forward); + body.ReverseAxis(UE.Vector3.forward); } body.xDrive = drive; @@ -263,7 +265,7 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, { if (jointAxis.Equals(UE.Vector3.down)) { - ReverseArticulationBodyAxis(body, UE.Vector3.right); + body.ReverseAxis(UE.Vector3.right); } body.yDrive = drive; @@ -275,7 +277,7 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, { if (jointAxis.Equals(UE.Vector3.back)) { - ReverseArticulationBodyAxis(body, UE.Vector3.up); + body.ReverseAxis(UE.Vector3.up); } body.zDrive = drive; @@ -289,13 +291,13 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, } } - private static void ReverseArticulationBodyAxis(in UE.ArticulationBody body, in UE.Vector3 euler) + private static void ReverseAxis(this UE.ArticulationBody body, in UE.Vector3 euler) { body.anchorRotation *= UE.Quaternion.Euler(euler * 180f); // body.parentAnchorRotation *= UE.Quaternion.Euler(euler * 180); // TODO: matchAnchors is set to true } - private static void SetRevoluteArticulationDriveLimit(ref UE.ArticulationDrive drive, in SDF.Axis.Limit limit) + private static void SetRevoluteDriveLimit(this ref UE.ArticulationDrive drive, in SDF.Axis.Limit limit) { drive.lowerLimit = SDF2Unity.CurveOrientation((float)limit.upper); drive.upperLimit = SDF2Unity.CurveOrientation((float)limit.lower); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index 9ddab212..d184b531 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -10,6 +10,8 @@ namespace SDF { + using Implement; + namespace Import { public partial class Loader : Base @@ -85,29 +87,29 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec var anchorPose = Implement.Joint.SetArticulationBodyRelationship(joint, linkObjectParent, linkObjectChild); - Implement.Joint.SetArticulationBodyAnchor(articulationBodyChild, anchorPose); + articulationBodyChild.SetAnchor(anchorPose); switch (joint.Type) { case "ball": - Implement.Joint.MakeBall(articulationBodyChild); + articulationBodyChild.MakeBall(); break; case "prismatic": - Implement.Joint.MakePrismatic(articulationBodyChild, joint.Axis, joint.Pose); + articulationBodyChild.MakePrismatic(joint.Axis, joint.Pose); break; case "revolute": - Implement.Joint.MakeRevolute(articulationBodyChild, joint.Axis); + articulationBodyChild.MakeRevolute(joint.Axis); break; case "universal": case "revolute2": - Implement.Joint.MakeRevolute2(articulationBodyChild, joint.Axis, joint.Axis2); + articulationBodyChild.MakeRevolute2(joint.Axis, joint.Axis2); break; case "fixed": - Implement.Joint.MakeFixed(articulationBodyChild); + articulationBodyChild.MakeFixed(); break; case "gearbox": From 2e1092a6d62d7c4e12a5136fcafea803430c9405 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 10:39:16 +0900 Subject: [PATCH 10/26] Modify SDF.Implement.Sensor - apply extension method --- .../Tools/SDF/Implement/Implement.Sensor.cs | 63 ++++++++++--------- .../Scripts/Tools/SDF/Import/Import.Sensor.cs | 19 +++--- 2 files changed, 42 insertions(+), 40 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index 11bfa5e0..d659e4b2 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -10,9 +10,9 @@ namespace SDF { namespace Implement { - public class Sensor + public static class Sensor { - private static string GetFrameName(in UE.GameObject currentObject) + private static string GetFrameName(this UE.GameObject currentObject) { var frameName = string.Empty; var nextObject = currentObject.transform.parent; @@ -28,8 +28,8 @@ private static string GetFrameName(in UE.GameObject currentObject) } private static void AttachSensor( + this UE.GameObject targetObject, in UE.GameObject sensorObject, - in UE.GameObject targetObject, Pose sensorPose = null) { try @@ -48,13 +48,13 @@ private static void AttachSensor( } } - public static Device AddLidar(in SDF.Lidar element, in UE.GameObject targetObject) + public static Device AddLidar(this UE.GameObject targetObject, in SDF.Lidar element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var lidar = newSensorObject.AddComponent(); - lidar.DeviceName = GetFrameName(newSensorObject); + lidar.DeviceName = newSensorObject.GetFrameName(); lidar.range = new SensorDevices.LaserData.MinMax(element.range.min, element.range.max); var horizontal = element.horizontal; lidar.horizontal = new SensorDevices.LaserData.Scan(horizontal.samples, horizontal.min_angle, horizontal.max_angle, horizontal.resolution); @@ -73,14 +73,14 @@ public static Device AddLidar(in SDF.Lidar element, in UE.GameObject targetObjec return lidar; } - public static Device AddCamera(in SDF.Camera element, in UE.GameObject targetObject) + public static Device AddCamera(this UE.GameObject targetObject, in SDF.Camera element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject, element.Pose); + targetObject.AttachSensor(newSensorObject, element.Pose); var camera = newSensorObject.AddComponent(); camera.tag = "Sensor"; - camera.DeviceName = GetFrameName(newSensorObject); + camera.DeviceName = newSensorObject.GetFrameName(); camera.SetParameter(element); if (element.noise != null) @@ -91,13 +91,13 @@ public static Device AddCamera(in SDF.Camera element, in UE.GameObject targetObj return camera; } - public static Device AddSegmentaionCamera(in SDF.Camera element, in UE.GameObject targetObject) + public static Device AddSegmentaionCamera(this UE.GameObject targetObject, in SDF.Camera element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject, element.Pose); + targetObject.AttachSensor(newSensorObject, element.Pose); var segmentationCamera = newSensorObject.AddComponent(); - segmentationCamera.DeviceName = GetFrameName(newSensorObject); + segmentationCamera.DeviceName = newSensorObject.GetFrameName(); switch (element.image.format) { @@ -129,13 +129,13 @@ public static Device AddSegmentaionCamera(in SDF.Camera element, in UE.GameObjec return segmentationCamera; } - public static Device AddDepthCamera(in SDF.Camera element, in UE.GameObject targetObject) + public static Device AddDepthCamera(this UE.GameObject targetObject, in SDF.Camera element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject, element.Pose); + targetObject.AttachSensor(newSensorObject, element.Pose); var depthCamera = newSensorObject.AddComponent(); - depthCamera.DeviceName = GetFrameName(newSensorObject); + depthCamera.DeviceName = newSensorObject.GetFrameName(); switch (element.image.format) { @@ -171,16 +171,16 @@ public static Device AddDepthCamera(in SDF.Camera element, in UE.GameObject targ return depthCamera; } - public static Device AddMultiCamera(in SDF.Cameras element, in UE.GameObject targetObject) + public static Device AddMultiCamera(this UE.GameObject targetObject, in SDF.Cameras element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var multicamera = newSensorObject.AddComponent(); - multicamera.DeviceName = GetFrameName(newSensorObject); + multicamera.DeviceName = newSensorObject.GetFrameName(); foreach (var camParam in element.cameras) { - var newCam = AddCamera(camParam, newSensorObject); + var newCam = AddCamera(newSensorObject, camParam); newCam.name = camParam.name; newCam.Mode = Device.ModeType.NONE; newCam.DeviceName = multicamera.DeviceName + "::" + element.name + "::" + newCam.name; @@ -190,13 +190,13 @@ public static Device AddMultiCamera(in SDF.Cameras element, in UE.GameObject tar return multicamera; } - public static Device AddSonar(in SDF.Sonar element, in UE.GameObject targetObject) + public static Device AddSonar(this UE.GameObject targetObject, in SDF.Sonar element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var sonar = newSensorObject.AddComponent(); - sonar.DeviceName = GetFrameName(newSensorObject); + sonar.DeviceName = newSensorObject.GetFrameName(); sonar.geometry = element.geometry; sonar.rangeMin = element.min; sonar.rangeMax = element.max; @@ -205,13 +205,13 @@ public static Device AddSonar(in SDF.Sonar element, in UE.GameObject targetObjec return sonar; } - public static Device AddImu(in SDF.IMU element, in UE.GameObject targetObject) + public static Device AddImu(this UE.GameObject targetObject, in SDF.IMU element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var imu = newSensorObject.AddComponent(); - imu.DeviceName = GetFrameName(newSensorObject); + imu.DeviceName = newSensorObject.GetFrameName(); if (element != null) { @@ -249,13 +249,13 @@ public static Device AddImu(in SDF.IMU element, in UE.GameObject targetObject) return imu; } - public static Device AddNavSat(in SDF.NavSat element, in UE.GameObject targetObject) + public static Device AddNavSat(this UE.GameObject targetObject, in SDF.NavSat element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var gps = newSensorObject.AddComponent(); - gps.DeviceName = GetFrameName(newSensorObject); + gps.DeviceName = newSensorObject.GetFrameName(); if (element != null) { @@ -283,13 +283,14 @@ public static Device AddNavSat(in SDF.NavSat element, in UE.GameObject targetObj return gps; } - public static Device AddContact(in SDF.Contact element, in UE.GameObject targetObject) + + public static Device AddContact(this UE.GameObject targetObject, in SDF.Contact element) { var newSensorObject = new UE.GameObject(); - AttachSensor(newSensorObject, targetObject); + targetObject.AttachSensor(newSensorObject); var contact = newSensorObject.AddComponent(); - contact.DeviceName = GetFrameName(newSensorObject); + contact.DeviceName = newSensorObject.GetFrameName(); contact.targetCollision = element.collision; contact.topic = element.topic; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs index b49b2756..7e9745c5 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs @@ -12,6 +12,7 @@ namespace SDF { + using Implement; namespace Import { public partial class Loader : Base @@ -41,41 +42,41 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob case "gpu_lidar": var lidar = sensor.GetSensor() as SDF.Lidar; - device = Implement.Sensor.AddLidar(lidar, targetObject); + device = targetObject.AddLidar(lidar); break; case "depth_camera": case "depth": var depthCamera = sensor.GetSensor() as SDF.Camera; - device = Implement.Sensor.AddDepthCamera(depthCamera, targetObject); + device = targetObject.AddDepthCamera(depthCamera); break; case "camera": var camera = sensor.GetSensor() as SDF.Camera; - device = Implement.Sensor.AddCamera(camera, targetObject); + device = targetObject.AddCamera(camera); break; case "segmentation_camera": case "segmentation": var segmentationCamera = sensor.GetSensor() as SDF.Camera; - device = Implement.Sensor.AddSegmentaionCamera(segmentationCamera, targetObject); + device = targetObject.AddSegmentaionCamera(segmentationCamera); break; case "rgbd_camera": case "rgbd": case "multicamera": var cameras = sensor.GetSensor() as SDF.Cameras; - device = Implement.Sensor.AddMultiCamera(cameras, targetObject); + device = targetObject.AddMultiCamera(cameras); break; case "imu": var imu = sensor.GetSensor() as SDF.IMU; - device = Implement.Sensor.AddImu(imu, targetObject); + device = targetObject.AddImu(imu); break; case "sonar": var sonar = sensor.GetSensor() as SDF.Sonar; - device = Implement.Sensor.AddSonar(sonar, targetObject); + device = targetObject.AddSonar(sonar); break; case "gps": @@ -84,12 +85,12 @@ protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Ob case "navsat": var navsat = sensor.GetSensor() as SDF.NavSat; - device = Implement.Sensor.AddNavSat(navsat, targetObject); + device = targetObject.AddNavSat(navsat); break; case "contact": var contact = sensor.GetSensor() as SDF.Contact; - device = Implement.Sensor.AddContact(contact, targetObject); + device = targetObject.AddContact(contact); break; case "air_pressure": From 363ea1e85ee60649fa2052826b8f71c52808f24e Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 11:44:46 +0900 Subject: [PATCH 11/26] Fix JointTarget when ResetArticulationBody is called in PoseControl - set initial joint position --- .../Scripts/Tools/SDF/Helper/PoseControl.cs | 139 +++++++++--------- 1 file changed, 71 insertions(+), 68 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs index cd858d26..aca03664 100644 --- a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs +++ b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs @@ -14,10 +14,10 @@ namespace Helper { public class PoseControl { - private UE.Transform targetTransform = null; - private UE.ArticulationBody articulationBody = null; + private UE.Transform _targetTransform = null; + private UE.ArticulationBody _articulationBody = null; - private List poseList = new List(); + private List _poseList = new List(); struct JointTarget { @@ -25,13 +25,13 @@ struct JointTarget public float axis2; } - private List jointTargetList = new List(); + private List _jointTargetList = new List(); - public int Count => poseList.Count; + public int Count => _poseList.Count; public PoseControl(in UE.Transform target) { - targetTransform = target; + _targetTransform = target; } public void SetJointTarget(in float targetAxis1, in float targetAxis2, in int targetFrame = 0) @@ -40,15 +40,15 @@ public void SetJointTarget(in float targetAxis1, in float targetAxis2, in int ta jointTarget.axis1 = targetAxis1; jointTarget.axis2 = targetAxis2; - if (targetFrame < jointTargetList.Count) + if (targetFrame < _jointTargetList.Count) { - jointTargetList[targetFrame] = jointTarget; + _jointTargetList[targetFrame] = jointTarget; } else { - lock (jointTargetList) + lock (_jointTargetList) { - jointTargetList.Add(jointTarget); + _jointTargetList.Add(jointTarget); } } } @@ -58,9 +58,9 @@ public void GetJointTarget(out float targetAxis1, out float targetAxis2, in int targetAxis1 = 0; targetAxis2 = 0; - if (targetFrame < jointTargetList.Count) + if (targetFrame < _jointTargetList.Count) { - var jointTarget = jointTargetList[targetFrame]; + var jointTarget = _jointTargetList[targetFrame]; targetAxis1 = jointTarget.axis1; targetAxis2 = jointTarget.axis2; } @@ -68,17 +68,17 @@ public void GetJointTarget(out float targetAxis1, out float targetAxis2, in int public void Add(in UE.Vector3 newPosition, in UE.Quaternion newRotation) { - lock (poseList) + lock (_poseList) { - poseList.Add(new UE.Pose(newPosition, newRotation)); + _poseList.Add(new UE.Pose(newPosition, newRotation)); } } public void Set(in UE.Vector3 newPosition, in UE.Quaternion newRotation, in int targetFrame = 0) { - if (targetFrame < poseList.Count) + if (targetFrame < _poseList.Count) { - poseList[targetFrame] = new UE.Pose(newPosition, newRotation); + _poseList[targetFrame] = new UE.Pose(newPosition, newRotation); } else { @@ -90,11 +90,11 @@ public UE.Pose Get(in int targetFrame = 0) { var getPose = UE.Pose.identity; - lock (poseList) + lock (_poseList) { - if (targetFrame < poseList.Count) + if (targetFrame < _poseList.Count) { - getPose = poseList[targetFrame]; + getPose = _poseList[targetFrame]; } } @@ -103,74 +103,76 @@ public UE.Pose Get(in int targetFrame = 0) public void Clear() { - lock (poseList) + lock (_poseList) { - poseList.Clear(); + _poseList.Clear(); } - lock (jointTargetList) + lock (_jointTargetList) { - jointTargetList.Clear(); + _jointTargetList.Clear(); } } private void ResetArticulationBody(in int targetFrame = 0) { - if (articulationBody != null) + if (_articulationBody != null) { - articulationBody.velocity = UE.Vector3.zero; - articulationBody.angularVelocity = UE.Vector3.zero; + var isPrismatic = _articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); + + _articulationBody.velocity = UE.Vector3.zero; + _articulationBody.angularVelocity = UE.Vector3.zero; var zeroSpace = new UE.ArticulationReducedSpace(); - zeroSpace.dofCount = articulationBody.dofCount; - for (var i = 0; i < articulationBody.dofCount; i++) + zeroSpace.dofCount = _articulationBody.dofCount; + for (var i = 0; i < _articulationBody.dofCount; i++) { zeroSpace[i] = 0; } - articulationBody.jointPosition = zeroSpace; - articulationBody.jointVelocity = zeroSpace; - articulationBody.jointForce = zeroSpace; - var isPrismatic = articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); + _articulationBody.jointPosition = zeroSpace; + _articulationBody.jointVelocity = zeroSpace; + _articulationBody.jointForce = zeroSpace; GetJointTarget(out var targetJoint1, out var targetJoint2, targetFrame); + // Debug.Log($"PoseControl {_articulationBody.name} targetJoint1={targetJoint1.ToString("F6")} targetJoint2={targetJoint2}"); - var xDrive = articulationBody.xDrive; + var xDrive = _articulationBody.xDrive; if (!isPrismatic) { xDrive.targetVelocity = 0; } - if (!articulationBody.linearLockX.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.linearLockX.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - if (!articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - xDrive.target = targetJoint2; + xDrive.target = targetJoint1; } else { - xDrive.target = targetJoint1; + xDrive.target = targetJoint2; } } else { xDrive.target = 0; } - articulationBody.xDrive = xDrive; + _articulationBody.xDrive = xDrive; - var yDrive = articulationBody.yDrive; + var yDrive = _articulationBody.yDrive; if (!isPrismatic) { yDrive.targetVelocity = 0; } - if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - if (!articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - yDrive.target = targetJoint2; + yDrive.target = targetJoint1; } else { @@ -181,75 +183,76 @@ private void ResetArticulationBody(in int targetFrame = 0) { yDrive.target = 0; } - articulationBody.yDrive = yDrive; + _articulationBody.yDrive = yDrive; - var zDrive = articulationBody.zDrive; + var zDrive = _articulationBody.zDrive; if (!isPrismatic) { zDrive.targetVelocity = 0; } - if (!articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - if (!articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || + !_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) { - zDrive.target = targetJoint2; + zDrive.target = targetJoint1; } else { - zDrive.target = targetJoint1; + zDrive.target = targetJoint2; } } else { zDrive.target = 0; } - articulationBody.zDrive = zDrive; + _articulationBody.zDrive = zDrive; } } public void Reset(in int targetFrame = 0) { - lock (poseList) + lock (_poseList) { - if (poseList.Count == 0) + if (_poseList.Count == 0) { // Debug.LogWarning("Nothing to reset, pose List is empty"); return; } - if (targetFrame >= poseList.Count) + if (targetFrame >= _poseList.Count) { - Debug.LogWarningFormat("exceed target frame({0}) in poseList({1})", targetFrame, poseList.Count); + Debug.LogWarningFormat("exceed target frame({0}) in _poseList({1})", targetFrame, _poseList.Count); return; } } - if (targetTransform != null) + if (_targetTransform != null) { var targetPose = Get(targetFrame); - if (articulationBody == null) + if (_articulationBody == null) { - articulationBody = targetTransform.GetComponent(); + _articulationBody = _targetTransform.GetComponent(); } - if (articulationBody != null) + if (_articulationBody != null) { - if (articulationBody.isRoot) + if (_articulationBody.isRoot) { - articulationBody.Sleep(); - articulationBody.TeleportRoot(targetPose.position, targetPose.rotation); + _articulationBody.Sleep(); + _articulationBody.TeleportRoot(targetPose.position, targetPose.rotation); } ResetArticulationBody(targetFrame); } else { - targetTransform.localPosition = targetPose.position; - targetTransform.localRotation = targetPose.rotation; + _targetTransform.localPosition = targetPose.position; + _targetTransform.localRotation = targetPose.rotation; } + // Debug.Log($"Reset: {_targetTransform.name} artbody({_articulationBody}) = {targetPose.position} {targetPose.rotation}"); } } } From 817525e725ba5d6139da1f51e0c84789ef289c62 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 12:10:21 +0900 Subject: [PATCH 12/26] Separate SDF.Import function - new file added SDF.Import.Util - SetChild() - file name changed - Import.Base.common -> Import.Base.Common --- Assets/Scripts/Main.cs | 3 +- .../Scripts/Tools/SDF/Import/Import.Actor.cs | 2 - ...t.Base.common.cs => Import.Base.Common.cs} | 0 ...mon.cs.meta => Import.Base.Common.cs.meta} | 0 .../Tools/SDF/Import/Import.Collision.cs | 2 +- .../Scripts/Tools/SDF/Import/Import.Link.cs | 2 +- .../Scripts/Tools/SDF/Import/Import.Loader.cs | 19 ---------- .../Scripts/Tools/SDF/Import/Import.Model.cs | 2 +- .../Scripts/Tools/SDF/Import/Import.Util.cs | 38 +++++++++++++++++++ .../Tools/SDF/Import/Import.Util.cs.meta | 11 ++++++ .../Scripts/Tools/SDF/Import/Import.Visual.cs | 2 +- 11 files changed, 55 insertions(+), 26 deletions(-) rename Assets/Scripts/Tools/SDF/Import/{Import.Base.common.cs => Import.Base.Common.cs} (100%) rename Assets/Scripts/Tools/SDF/Import/{Import.Base.common.cs.meta => Import.Base.Common.cs.meta} (100%) create mode 100644 Assets/Scripts/Tools/SDF/Import/Import.Util.cs create mode 100644 Assets/Scripts/Tools/SDF/Import/Import.Util.cs.meta diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 501e3a0f..544a5bbc 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -451,8 +451,9 @@ private IEnumerator LoadWorld() if (_sdfRoot.DoParse(out var world, _worldFilename)) { + SDF.Import.Util.RootModels = _worldRoot; + _sdfLoader = new SDF.Import.Loader(); - _sdfLoader.SetRootModels(_worldRoot); _sdfLoader.SetRootLights(_lightsRoot); _sdfLoader.SetRootRoads(_roadsRoot); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs index d660301d..ce4836e5 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs @@ -28,8 +28,6 @@ protected override System.Object ImportActor(in Actor actor) newActorObject.name = actor.Name; newActorObject.tag = "Actor"; - SetParentObject(newActorObject, null); - // Apply attributes var localPosition = SDF2Unity.Position(actor.Pose?.Pos); var localRotation = SDF2Unity.Rotation(actor.Pose?.Rot); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs similarity index 100% rename from Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs rename to Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs.meta b/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs.meta similarity index 100% rename from Assets/Scripts/Tools/SDF/Import/Import.Base.common.cs.meta rename to Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs.meta diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs index 6671c73f..afc7695e 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs @@ -21,7 +21,7 @@ protected override System.Object ImportCollision(in SDF.Collision collision, in var newCollisionObject = new UE.GameObject(collision.Name); newCollisionObject.tag = "Collision"; - SetParentObject(newCollisionObject, targetObject); + targetObject.SetChild(newCollisionObject); var localPosition = SDF2Unity.Position(collision.Pose?.Pos); var localRotation = SDF2Unity.Rotation(collision.Pose?.Rot); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index 33e3b75d..3f4dc819 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs @@ -46,7 +46,7 @@ protected override System.Object ImportLink(in SDF.Link link, in System.Object p var newLinkObject = new UE.GameObject(link.Name); newLinkObject.tag = "Link"; - SetParentObject(newLinkObject, targetObject); + targetObject.SetChild(newLinkObject); var localPosition = SDF2Unity.Position(link.Pose?.Pos); var localRotation = SDF2Unity.Rotation(link.Pose?.Rot); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Loader.cs b/Assets/Scripts/Tools/SDF/Import/Import.Loader.cs index b664295d..7ab47564 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Loader.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Loader.cs @@ -12,15 +12,9 @@ namespace Import { public partial class Loader : Base { - private UE.GameObject _rootModels = null; private UE.GameObject _rootLights = null; private UE.GameObject _rootRoads = null; - public void SetRootModels(in UE.GameObject root) - { - _rootModels = root; - } - public void SetRootLights(in UE.GameObject root) { _rootLights = root; @@ -30,19 +24,6 @@ public void SetRootRoads(in UE.GameObject root) { _rootRoads = root; } - - private void SetParentObject(UE.GameObject childObject, UE.GameObject parentObject) - { - childObject.transform.position = UE.Vector3.zero; - childObject.transform.rotation = UE.Quaternion.identity; - - var targetParentTransform = (parentObject == null) ? _rootModels.transform : parentObject.transform; - childObject.transform.SetParent(targetParentTransform, false); - - childObject.transform.localScale = UE.Vector3.one; - childObject.transform.localPosition = UE.Vector3.zero; - childObject.transform.localRotation = UE.Quaternion.identity; - } } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs index 220008b0..ab522864 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs @@ -79,7 +79,7 @@ protected override System.Object ImportModel(in SDF.Model model, in System.Objec var newModelObject = new UE.GameObject(model.Name); newModelObject.tag = "Model"; - SetParentObject(newModelObject, targetObject); + targetObject.SetChild(newModelObject); // Apply attributes var localPosition = SDF2Unity.Position(model.Pose?.Pos); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs new file mode 100644 index 00000000..7267a006 --- /dev/null +++ b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs @@ -0,0 +1,38 @@ +/* + * Copyright (c) 2024 LG Electronics Inc. + * + * SPDX-License-Identifier: MIT + */ + +using UE = UnityEngine; +using UnityEngine; + +namespace SDF +{ + namespace Import + { + public static class Util + { + private static UE.GameObject _rootModels = null; + + public static UE.GameObject RootModels + { + get => _rootModels; + set => _rootModels = value; + } + + public static void SetChild(this UE.GameObject parent, in UE.GameObject child) + { + child.transform.position = UE.Vector3.zero; + child.transform.rotation = UE.Quaternion.identity; + + var targetParentTransform = (parent == null) ? _rootModels.transform : parent.transform; + child.transform.SetParent(targetParentTransform, false); + + child.transform.localScale = UE.Vector3.one; + child.transform.localPosition = UE.Vector3.zero; + child.transform.localRotation = UE.Quaternion.identity; + } + } + } +} \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs.meta b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs.meta new file mode 100644 index 00000000..c987504c --- /dev/null +++ b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a58e42a64dc6b7ae81590249a1e11d7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs index 780d366b..49222251 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs @@ -23,7 +23,7 @@ protected override System.Object ImportVisual(in SDF.Visual visual, in System.Ob var newVisualObject = new UE.GameObject(visual.Name); newVisualObject.tag = "Visual"; - SetParentObject(newVisualObject, targetObject); + targetObject.SetChild(newVisualObject); var localPosition = SDF2Unity.Position(visual.Pose?.Pos); var localRotation = SDF2Unity.Rotation(visual.Pose?.Rot); From 174b88a22574096b53dabec64091f6901587fd1a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 13:10:09 +0900 Subject: [PATCH 13/26] Modify SDF.Implement.Geometry - apply extension method --- Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs | 2 +- .../Tools/SDF/Implement/Implement.Geometry.cs | 16 ++++++---------- .../Scripts/Tools/SDF/Import/Import.Geometry.cs | 8 +++++--- 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs b/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs index c6f0ffff..e3d18107 100644 --- a/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs +++ b/Assets/Scripts/Tools/Mesh/ProceduralHeightmap.cs @@ -66,7 +66,7 @@ private static byte[] GetBytesFromImage(in string imagePath) return null; } - public static void Generate(in SDF.Heightmap property, in GameObject heightmapObject, in bool isVisualMesh) + public static void GenerateHeightMap(this GameObject heightmapObject, in SDF.Heightmap property, in bool isVisualMesh) { var byteArray = GetBytesFromImage(property.uri); if (byteArray == null) diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Geometry.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Geometry.cs index e484b23a..7cf66486 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Geometry.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Geometry.cs @@ -6,13 +6,12 @@ using UE = UnityEngine; using Debug = UnityEngine.Debug; -using System; namespace SDF { namespace Implement { - public class Geometry + public static class Geometry { private static bool IsVisualObject(in UE.GameObject target) { @@ -20,7 +19,7 @@ private static bool IsVisualObject(in UE.GameObject target) } /// Set mesh from external source - public static void GenerateMeshObject(in SDF.Mesh obj, in UE.GameObject targetParentObject) + public static void GenerateMesh(this UE.GameObject targetParentObject, in SDF.Mesh obj) { var loadedObject = MeshLoader.CreateMeshObject(obj.uri, obj.submesh_name); var isVisualMesh = IsVisualObject(targetParentObject); @@ -32,27 +31,24 @@ public static void GenerateMeshObject(in SDF.Mesh obj, in UE.GameObject targetPa else { loadedObject.transform.localScale = SDF2Unity.Scale(obj.scale); + loadedObject.transform.SetParent(targetParentObject.transform, false); } - - loadedObject.transform.SetParent(targetParentObject.transform, false); } - public static void GenerateMeshObject(in SDF.Heightmap obj, in UE.GameObject targetParentObject) + public static void GenerateMesh(this UE.GameObject targetParentObject, in SDF.Heightmap obj) { var heightmapObject = new UE.GameObject("Heightmap"); var isVisualMesh = IsVisualObject(targetParentObject); heightmapObject.transform.SetParent(targetParentObject.transform, false); - - ProceduralHeightmap.Generate(obj, heightmapObject, isVisualMesh); - + heightmapObject.GenerateHeightMap(obj, isVisualMesh); heightmapObject.transform.localPosition = SDF2Unity.Position(obj.pos); } // // Summary: Set primitive mesh // - public static void GenerateMeshObject(in SDF.ShapeType shape, in UE.GameObject targetParentObject) + public static void GenerateMesh(this UE.GameObject targetParentObject, in SDF.ShapeType shape) { var createdObject = new UE.GameObject("Primitive Mesh"); createdObject.tag = "Geometry"; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Geometry.cs b/Assets/Scripts/Tools/SDF/Import/Import.Geometry.cs index 1e54be7d..540a225d 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Geometry.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Geometry.cs @@ -8,6 +8,8 @@ namespace SDF { + using Implement; + namespace Import { public partial class Loader : Base @@ -26,16 +28,16 @@ protected override void ImportGeometry(in SDF.Geometry geometry, in System.Objec if (t != null && t.Equals(typeof(SDF.Mesh))) { var mesh = shape as SDF.Mesh; - Implement.Geometry.GenerateMeshObject(mesh, targetObject); + targetObject.GenerateMesh(mesh); } else if (t != null && t.Equals(typeof(SDF.Heightmap))) { var heightmap = shape as SDF.Heightmap; - Implement.Geometry.GenerateMeshObject(heightmap, targetObject); + targetObject.GenerateMesh(heightmap); } else if (t != null && typeof(SDF.ShapeType).IsAssignableFrom(t)) { - Implement.Geometry.GenerateMeshObject(shape, targetObject); + targetObject.GenerateMesh(shape); } else { From fb6fe014a4c739120baf693f74fe83674f01a551 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 13:23:49 +0900 Subject: [PATCH 14/26] Modify SDF.Implement.Visual/Collision - apply extension method --- .../SDF/Implement/Implement.Collision.cs | 18 +++++----- .../Tools/SDF/Implement/Implement.Visual.cs | 36 ++++++++++++++++--- .../Tools/SDF/Import/Import.Collision.cs | 6 ++-- .../Scripts/Tools/SDF/Import/Import.Visual.cs | 35 +++--------------- 4 files changed, 49 insertions(+), 46 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Collision.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Collision.cs index 877eec49..5c2558fc 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Collision.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Collision.cs @@ -12,7 +12,7 @@ namespace SDF { namespace Implement { - public class Collision + public static class Collision { public static readonly int PlaneLayerIndex = UE.LayerMask.NameToLayer("Plane"); @@ -28,7 +28,7 @@ public class Collision MCCookingOptions.WeldColocatedVertices | MCCookingOptions.UseFastMidphase; - private static void KeepUnmergedMeshes(in UE.MeshFilter[] meshFilters) + private static void KeepUnmergedMeshes(ref UE.MeshFilter[] meshFilters) { foreach (var meshFilter in meshFilters) { @@ -41,7 +41,7 @@ private static void KeepUnmergedMeshes(in UE.MeshFilter[] meshFilters) } } - private static void MergeCollider(in UE.GameObject targetObject) + private static void MergeCollider(this UE.GameObject targetObject) { var geometryWorldToLocalMatrix = targetObject.transform.worldToLocalMatrix; var meshColliders = targetObject.GetComponentsInChildren(); @@ -65,7 +65,7 @@ private static void MergeCollider(in UE.GameObject targetObject) // mergedMeshCollider.hideFlags |= UE.HideFlags.NotEditable; } - public static void Make(UE.GameObject targetObject) + public static void MakeCollision(this UE.GameObject targetObject) { var modelHelper = targetObject.GetComponentInParent(); // UE.Debug.Log(modelHelper); @@ -83,18 +83,18 @@ public static void Make(UE.GameObject targetObject) { if (targetObject.GetComponent() == null) { - KeepUnmergedMeshes(meshFilters); + KeepUnmergedMeshes(ref meshFilters); #if ENABLE_MERGE_COLLIDER - MergeCollider(targetObject); + targetObject.MergeCollider(); #endif } } - RemoveRenderers(meshFilters); + RemoveRenderers(ref meshFilters); } - private static void RemoveRenderers(UE.MeshFilter[] meshFilters) + private static void RemoveRenderers(ref UE.MeshFilter[] meshFilters) { foreach (var meshFilter in meshFilters) { @@ -108,7 +108,7 @@ private static void RemoveRenderers(UE.MeshFilter[] meshFilters) } } - public static void SetSurfaceFriction(in SDF.Surface surface, in UE.GameObject targetObject) + public static void SetSurfaceFriction(this UE.GameObject targetObject, in SDF.Surface surface) { var material = new UE.PhysicMaterial(); diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Visual.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Visual.cs index 9bd0c652..4345d88e 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Visual.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Visual.cs @@ -14,9 +14,9 @@ namespace SDF { namespace Implement { - public class Visual + public static class Visual { - private static void OptimizeMesh(in UE.Transform target) + private static void OptimizeMesh(this UE.Transform target) { var meshFilters = target.GetComponentsInChildren(); if (meshFilters.Length <= 1) @@ -96,12 +96,38 @@ private static void OptimizeMesh(in UE.Transform target) } } - public static void OptimizeMeshes(in UE.Transform targetTransform) + public static void OptimizeMeshes(this UE.Transform targetTransform) { - for (var i = 0; i< targetTransform.childCount; i++) + for (var i = 0; i < targetTransform.childCount; i++) { var child = targetTransform.GetChild(i); - OptimizeMesh(child); + child.OptimizeMesh(); + } + } + + public static void RemoveColliders(this UE.GameObject targetObject) + { + var colliders = targetObject.GetComponentsInChildren(); + foreach (var collider in colliders) + { + UE.Debug.LogWarning($"{collider.name} Collider should not exit. There was collider"); + UE.GameObject.Destroy(collider); + } + } + + public static void AddRenderes(this UE.GameObject targetObject) + { + var meshFilters = targetObject.GetComponentsInChildren(); + foreach (var meshFilter in meshFilters) + { + var meshRenderer = meshFilter.gameObject.GetComponent(); + if (meshRenderer == null) + { + meshRenderer = meshFilter.gameObject.AddComponent(); + meshRenderer.materials = new UE.Material[] { SDF2Unity.Material.Create(meshFilter.name + "_material") }; + meshRenderer.allowOcclusionWhenDynamic = true; + meshRenderer.receiveShadows = true; + } } } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs index afc7695e..ed31f38c 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs @@ -11,6 +11,8 @@ namespace SDF { + using Implement; + namespace Import { public partial class Loader : Base @@ -41,7 +43,7 @@ protected override void AfterImportCollision(in SDF.Collision collision, in Syst if (collisionObject.CompareTag("Collision")) { var geometryObject = (collisionObject.transform.childCount == 0) ? collisionObject : collisionObject.transform.GetChild(0).gameObject; - Implement.Collision.Make(geometryObject); + geometryObject.MakeCollision(); var shape = collision.GetGeometry().GetShape(); if (shape != null) @@ -78,7 +80,7 @@ protected override void AfterImportCollision(in SDF.Collision collision, in Syst collisionObject.transform.localPosition = SDF2Unity.Position(collision.Pose?.Pos); collisionObject.transform.localRotation = SDF2Unity.Rotation(collision.Pose?.Rot); - Implement.Collision.SetSurfaceFriction(collision.GetSurface(), collisionObject); + collisionObject.SetSurfaceFriction(collision.GetSurface()); } private bool EnhanceCollisionPerformance( diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs index 49222251..1d9bc98b 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs @@ -11,6 +11,8 @@ namespace SDF { + using Implement; + namespace Import { public partial class Loader : Base @@ -52,9 +54,8 @@ protected override void AfterImportVisual(in SDF.Visual visual, in System.Object return; } - RemoveColliders(visualObject); - - AddRenderes(visualObject); + visualObject.RemoveColliders(); + visualObject.AddRenderes(); if (EnableOptimization) { @@ -65,7 +66,7 @@ protected override void AfterImportVisual(in SDF.Visual visual, in System.Object var geometryTransform = visualObject.transform.GetChild(i); if (geometryTransform.CompareTag("Geometry")) { - Implement.Visual.OptimizeMeshes(geometryTransform); + geometryTransform.OptimizeMeshes(); } } } @@ -80,32 +81,6 @@ protected override void AfterImportVisual(in SDF.Visual visual, in System.Object SceneVisibilityManager.instance.DisablePicking(visualObject, true); #endif } - - private void RemoveColliders(UE.GameObject targetObject) - { - var colliders = targetObject.GetComponentsInChildren(); - foreach (var collider in colliders) - { - UE.Debug.LogWarning($"{collider.name} Collider should not exit. There was collider"); - UE.GameObject.Destroy(collider); - } - } - - private void AddRenderes(UE.GameObject targetObject) - { - var meshFilters = targetObject.GetComponentsInChildren(); - foreach (var meshFilter in meshFilters) - { - var meshRenderer = meshFilter.gameObject.GetComponent(); - if (meshRenderer == null) - { - meshRenderer = meshFilter.gameObject.AddComponent(); - meshRenderer.materials = new UE.Material[] { SDF2Unity.Material.Create(meshFilter.name + "_material") }; - meshRenderer.allowOcclusionWhenDynamic = true; - meshRenderer.receiveShadows = true; - } - } - } } } } \ No newline at end of file From a7cbad2c3aad7a9cc573c9f6fe2fc77d258a3de5 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 13:31:37 +0900 Subject: [PATCH 15/26] Modify SDF.Helper.Link - change default value for useGravity, false -> true - Find and Update RootModel --- Assets/Scripts/Tools/SDF/Helper/Link.cs | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Helper/Link.cs b/Assets/Scripts/Tools/SDF/Helper/Link.cs index 3be795a5..7993b648 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Link.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Link.cs @@ -5,7 +5,6 @@ */ using System.Collections.Generic; -using System.Collections; using UE = UnityEngine; namespace SDF @@ -28,7 +27,7 @@ public class Link : Base [UE.Header("SDF Properties")] public bool isSelfCollide = false; - public bool useGravity = false; + public bool useGravity = true; [UE.Header("Joint related")] private string jointName = string.Empty; @@ -85,13 +84,14 @@ public float JointAxis2LimitVelocity base.Awake(); _parentModelHelper = transform.parent?.GetComponent(); _jointAnchorPose = UE.Pose.identity; + + UpdateRootModel(); } // Start is called before the first frame update void Start() { - var modelHelpers = GetComponentsInParent(typeof(Model)); - _rootModel = (Model)modelHelpers[modelHelpers.Length - 1]; + UpdateRootModel(); var parentArtBodies = GetComponentsInParent(); @@ -135,7 +135,14 @@ void LateUpdate() { _jointPose.position -= _parentLink._jointPose.position; } - } + } + + private void UpdateRootModel() + { + var modelHelpers = GetComponentsInParent(typeof(Model)); + _rootModel = (Model)modelHelpers[modelHelpers.Length - 1]; + // UE.Debug.Log($"{name}: LinkHelper _rootModel={_rootModel}"); + } void OnDrawGizmos() { From bb433f68aba042a941a0be3122e0d3aabd5caae4 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 15:21:50 +0900 Subject: [PATCH 16/26] Refactoring SDF.Implement.Joint and SDF.Import.Joint --- .../Tools/SDF/Implement/Implement.Joint.cs | 101 ++++++++++++++++-- .../Scripts/Tools/SDF/Import/Import.Joint.cs | 78 +------------- 2 files changed, 95 insertions(+), 84 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index ad8dd4d2..a519c36b 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -5,6 +5,7 @@ */ using UE = UnityEngine; +using System.Linq; namespace SDF { @@ -79,7 +80,7 @@ public static void SetAnchor(this UE.ArticulationBody body, in UE.Pose parentAnc // body.parentAnchorRotation = parentAnchor.rotation; // TODO: matchAnchors is set to true } - public static void MakeRevolute(this UE.ArticulationBody body, in SDF.Axis axis) + public static void MakeRevoluteJoint(this UE.ArticulationBody body, in SDF.Axis axis) { body.jointType = UE.ArticulationJointType.SphericalJoint; body.linearDamping = DefaultJointLinearDamping; @@ -145,13 +146,13 @@ public static void MakeRevolute(this UE.ArticulationBody body, in SDF.Axis axis) } else { - UE.Debug.LogWarning("MakeRevolute - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + jointAxis); + UE.Debug.LogWarning("MakeRevoluteJoint - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + jointAxis); } } - public static void MakeRevolute2(this UE.ArticulationBody body, in SDF.Axis axis1, in SDF.Axis axis2) + private static void MakeRevoluteJoint2(this UE.ArticulationBody body, in SDF.Axis axis1, in SDF.Axis axis2) { - MakeRevolute(body, axis1); + MakeRevoluteJoint(body, axis1); var drive = new UE.ArticulationDrive(); @@ -192,11 +193,11 @@ public static void MakeRevolute2(this UE.ArticulationBody body, in SDF.Axis axis } else { - UE.Debug.LogWarning("MakeRevolute2 - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + joint2Axis); + UE.Debug.LogWarning("MakeRevoluteJoint2 - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + joint2Axis); } } - public static void MakeFixed(this UE.ArticulationBody body) + private static void MakeFixedJoint(this UE.ArticulationBody body) { body.jointType = UE.ArticulationJointType.FixedJoint; body.linearDamping = 0.00f; @@ -204,7 +205,7 @@ public static void MakeFixed(this UE.ArticulationBody body) body.jointFriction = 0; } - public static void MakeBall(this UE.ArticulationBody body) + private static void MakeBallJoint(this UE.ArticulationBody body) { body.jointType = UE.ArticulationJointType.SphericalJoint; body.linearDamping = DefaultJointLinearDamping; @@ -215,7 +216,7 @@ public static void MakeBall(this UE.ArticulationBody body) body.twistLock = UE.ArticulationDofLock.FreeMotion; } - public static void MakePrismatic(this UE.ArticulationBody body, in SDF.Axis axis, in SDF.Pose pose) + private static void MakePrismaticJoint(this UE.ArticulationBody body, in SDF.Axis axis, in SDF.Pose pose) { body.jointType = UE.ArticulationJointType.PrismaticJoint; body.anchorRotation *= SDF2Unity.Rotation(pose?.Rot); @@ -287,7 +288,50 @@ public static void MakePrismatic(this UE.ArticulationBody body, in SDF.Axis axis } else { - UE.Debug.LogWarning("MakePrismatic - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + jointAxis); + UE.Debug.LogWarning("MakePrismaticJoint - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + jointAxis); + } + } + + public static void MakeJoint(this UE.ArticulationBody body, in SDF.Joint joint) + { + + switch (joint.Type) + { + case "ball": + body.MakeBallJoint(); + break; + + case "prismatic": + body.MakePrismaticJoint(joint.Axis, joint.Pose); + break; + + case "revolute": + body.MakeRevoluteJoint(joint.Axis); + break; + + case "universal": + case "revolute2": + body.MakeRevoluteJoint2(joint.Axis, joint.Axis2); + break; + + case "fixed": + body.MakeFixedJoint(); + break; + + case "gearbox": + // gearbox_ratio = GetValue("gearbox_ratio"); + // gearbox_reference_body = GetValue("gearbox_reference_body"); + UE.Debug.LogWarning("This type[gearbox] is not supported now."); + break; + + case "screw": + // thread_pitch = GetValue("thread_pitch"); + UE.Debug.LogWarning("This type[screw] is not supported now."); + break; + + default: + UE.Debug.LogWarningFormat("Check Joint type[{0}]", joint.Type); + break; } } @@ -302,6 +346,45 @@ private static void SetRevoluteDriveLimit(this ref UE.ArticulationDrive drive, i drive.lowerLimit = SDF2Unity.CurveOrientation((float)limit.upper); drive.upperLimit = SDF2Unity.CurveOrientation((float)limit.lower); } + + public static UE.Transform FindTransformByName(this UE.GameObject targetObject, string name) + { + return targetObject?.transform.FindTransformByName(name); + } + + public static UE.Transform FindTransformByName(this UE.Transform targetTransform, string name) + { + UE.Transform foundLinkObject = null; + + var rootTransform = targetTransform; + + while (!SDF2Unity.IsRootModel(rootTransform)) + { + rootTransform = rootTransform.parent; + } + + (var modelName, var linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name); + // UE.Debug.Log("GetModelLinkName => " + modelName + ", " + linkName); + + if (string.IsNullOrEmpty(modelName)) + { + // UE.Debug.Log(name + ", Find => " + targetTransform.name + ", " + rootTransform.name); + foundLinkObject = targetTransform.GetComponentsInChildren().FirstOrDefault(x => x.name.Equals(name)); + } + else + { + var modelHelper = rootTransform.GetComponentsInChildren().FirstOrDefault(x => x.name.Equals(modelName)); + var modelTransform = modelHelper?.transform; + + if (modelTransform != null) + { + var foundLinkHelper = modelTransform.GetComponentsInChildren().FirstOrDefault(x => x.transform.name.Equals(linkName)); + foundLinkObject = foundLinkHelper?.transform; + } + } + + return foundLinkObject; + } } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index d184b531..71799854 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -6,7 +6,6 @@ using UE = UnityEngine; using Debug = UnityEngine.Debug; -using System.Linq; namespace SDF { @@ -16,47 +15,13 @@ namespace Import { public partial class Loader : Base { - private static UE.Transform FindTransformByName(string name, UE.Transform targetTransform) - { - UE.Transform foundLinkObject = null; - - var rootTransform = targetTransform; - - while (!SDF2Unity.IsRootModel(rootTransform)) - { - rootTransform = rootTransform.parent; - } - - var (modelName, linkName) = SDF2Unity.GetModelLinkName(name, targetTransform.name); - // UE.Debug.Log("GetModelLinkName => " + modelName + ", " + linkName); - - if (string.IsNullOrEmpty(modelName)) - { - // UE.Debug.Log(name + ", Find => " + targetTransform.name + ", " + rootTransform.name); - foundLinkObject = targetTransform.GetComponentsInChildren().FirstOrDefault(x => x.name.Equals(name)); - } - else - { - var modelHelper = rootTransform.GetComponentsInChildren().FirstOrDefault(x => x.name.Equals(modelName)); - var modelTransform = modelHelper?.transform; - - if (modelTransform != null) - { - var foundLinkHelper = modelTransform.GetComponentsInChildren().FirstOrDefault(x => x.transform.name.Equals(linkName)); - foundLinkObject = foundLinkHelper?.transform; - } - } - - return foundLinkObject; - } - protected override void ImportJoint(in Joint joint, in System.Object parentObject) { var targetObject = (parentObject as UE.GameObject); // Debug.LogFormat("[Joint] {0}, {1} <= {2}", joint.Name, joint.ParentLinkName, joint.ChildLinkName); - var linkObjectParent = FindTransformByName(joint.ParentLinkName, targetObject.transform); - var linkObjectChild = FindTransformByName(joint.ChildLinkName, targetObject.transform); + var linkObjectParent = targetObject.FindTransformByName(joint.ParentLinkName); + var linkObjectChild = targetObject.FindTransformByName(joint.ChildLinkName); if (linkObjectParent is null) { @@ -89,44 +54,7 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec articulationBodyChild.SetAnchor(anchorPose); - switch (joint.Type) - { - case "ball": - articulationBodyChild.MakeBall(); - break; - - case "prismatic": - articulationBodyChild.MakePrismatic(joint.Axis, joint.Pose); - break; - - case "revolute": - articulationBodyChild.MakeRevolute(joint.Axis); - break; - - case "universal": - case "revolute2": - articulationBodyChild.MakeRevolute2(joint.Axis, joint.Axis2); - break; - - case "fixed": - articulationBodyChild.MakeFixed(); - break; - - case "gearbox": - // gearbox_ratio = GetValue("gearbox_ratio"); - // gearbox_reference_body = GetValue("gearbox_reference_body"); - Debug.LogWarning("This type[gearbox] is not supported now."); - break; - - case "screw": - // thread_pitch = GetValue("thread_pitch"); - Debug.LogWarning("This type[screw] is not supported now."); - break; - - default: - Debug.LogWarningFormat("Check Joint type[{0}]", joint.Type); - break; - } + articulationBodyChild.MakeJoint(joint); var linkHelper = linkObjectChild.GetComponent(); if (linkHelper != null) From df5d4843ceca61de5b67f04096d44faa2df7b220 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Sep 2024 17:06:36 +0900 Subject: [PATCH 17/26] Modify SDF.Import.Model - refactoring ImportModel() --- .../Tools/SDF/Import/Import.Base.Common.cs | 2 +- .../Scripts/Tools/SDF/Import/Import.Base.cs | 39 +++++++------------ .../Scripts/Tools/SDF/Import/Import.Model.cs | 34 ++++++++++++---- 3 files changed, 42 insertions(+), 33 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs index 98c98b6b..26d79067 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.Common.cs @@ -68,7 +68,7 @@ protected virtual void AfterImportLink(in Link link, in Object targetObject) PrintNotImported(MethodBase.GetCurrentMethod().Name, link.Name); } - protected virtual Object ImportModel(in Model model, in Object parentObject) + protected virtual Object ImportModel(in Model model, in Object parentObject = null) { PrintNotImported(MethodBase.GetCurrentMethod().Name, model.Name); return null; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs index ce9a34ea..af55eba7 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs @@ -5,7 +5,6 @@ */ using System.Collections.Generic; -using System.Threading; using System; namespace SDF @@ -14,7 +13,7 @@ namespace Import { public partial class Base { - private Dictionary jointObjectList = new Dictionary(); + private Dictionary _jointObjectList = new Dictionary(); private void ImportVisuals(IReadOnlyList items, in Object parentObject) { @@ -56,7 +55,7 @@ private void ImportSensors(IReadOnlyList items, in Object parentObject) } } - private void ImportLinks(IReadOnlyList items, in Object parentObject) + protected void ImportLinks(IReadOnlyList items, in Object parentObject) { foreach (var item in items) { @@ -73,7 +72,7 @@ private void ImportLinks(IReadOnlyList items, in Object parentObject) } } - private void ImportPlugins(IReadOnlyList items, Object parentObject) + protected void ImportPlugins(IReadOnlyList items, Object parentObject) { foreach (var item in items) { @@ -81,12 +80,12 @@ private void ImportPlugins(IReadOnlyList items, Object parentObject) } } - private void ImportJoints(IReadOnlyList items, Object parentObject) + protected void ImportJoints(IReadOnlyList items, Object parentObject) { // Joints should be handled after all links of model loaded due to articulation body. foreach (var item in items) { - jointObjectList.Add(item, parentObject); + _jointObjectList.Add(item, parentObject); } } @@ -95,18 +94,7 @@ protected void ImportModels(IReadOnlyList items, in Object parentObject = foreach (var item in items) { // Console.WriteLine("[Model][{0}][{1}]", item.Name, parentObject); - var createdObject = ImportModel(item, parentObject); - - ImportLinks(item.GetLinks(), createdObject); - - // Add nested models - ImportModels(item.GetModels(), createdObject); - - AfterImportModel(item, createdObject); - - ImportJoints(item.GetJoints(), createdObject); - - ImportPlugins(item.GetPlugins(), createdObject); + ImportModel(item, parentObject); } } @@ -131,34 +119,37 @@ protected void ImportLights(IReadOnlyList items) public IEnumerator Start(World world) { // Console.WriteLine("Import Models({0})/Links/Joints", world.GetModels().Count); - jointObjectList.Clear(); + _jointObjectList.Clear(); var worldObject = ImportWorld(world); ImportModels(world.GetModels()); - foreach (var jointObject in jointObjectList) + foreach (var jointObject in _jointObjectList) { ImportJoint(jointObject.Key, jointObject.Value); } ImportPlugins(world.GetPlugins(), worldObject); + yield return null; ImportActors(world.GetActors()); + + yield return null; } public IEnumerator Start(Model model) { - jointObjectList.Clear(); - var models = new List() { model }; + _jointObjectList.Clear(); - ImportModels(models); + var modelObject = ImportModel(model); - foreach (var jointObject in jointObjectList) + foreach (var jointObject in _jointObjectList) { ImportJoint(jointObject.Key, jointObject.Value); } + yield return null; } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs index ab522864..1d3b0936 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs @@ -68,18 +68,12 @@ private static void CreateRootRigidBody(in UE.GameObject targetObject) rigidBody.Sleep(); } - protected override System.Object ImportModel(in SDF.Model model, in System.Object parentObject) + private UE.GameObject CreateModel(in SDF.Model model, in UE.GameObject parentObject) { - if (model == null) - { - return null; - } - - var targetObject = (parentObject as UE.GameObject); var newModelObject = new UE.GameObject(model.Name); newModelObject.tag = "Model"; - targetObject.SetChild(newModelObject); + parentObject.SetChild(newModelObject); // Apply attributes var localPosition = SDF2Unity.Position(model.Pose?.Pos); @@ -103,6 +97,30 @@ protected override System.Object ImportModel(in SDF.Model model, in System.Objec CreateRootArticulationBody(newModelObject); } } + return newModelObject; + } + + + protected override System.Object ImportModel(in SDF.Model model, in System.Object parentObject) + { + if (model == null) + { + return null; + } + + var targetObject = (parentObject as UE.GameObject); + var newModelObject = CreateModel(model, targetObject); + + ImportLinks(model.GetLinks(), newModelObject); + + // Add nested models + ImportModels(model.GetModels(), newModelObject); + + AfterImportModel(model, newModelObject); + + ImportJoints(model.GetJoints(), newModelObject); + + ImportPlugins(model.GetPlugins(), newModelObject); return newModelObject as System.Object; } From 48ef6d0c8ebfe0b2c5c610688b93b99d672fe81a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 5 Sep 2024 17:33:34 +0900 Subject: [PATCH 18/26] Modify SDF.Helper - Moved functionality of finding and Update RootModel from Link To Base --- Assets/Scripts/Tools/SDF/Helper/Actor.cs | 4 +++- Assets/Scripts/Tools/SDF/Helper/Base.cs | 18 ++++++++++++++++++ Assets/Scripts/Tools/SDF/Helper/Collision.cs | 3 ++- Assets/Scripts/Tools/SDF/Helper/Link.cs | 19 ++++--------------- Assets/Scripts/Tools/SDF/Helper/Model.cs | 5 ++++- Assets/Scripts/Tools/SDF/Helper/Visual.cs | 4 +++- 6 files changed, 34 insertions(+), 19 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Helper/Actor.cs b/Assets/Scripts/Tools/SDF/Helper/Actor.cs index 7d458cd7..5eed524e 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Actor.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Actor.cs @@ -55,8 +55,10 @@ private struct WaypointToward RestartWayPointFollowing(); } - void Start() + new protected void Start() { + base.Start(); + capsuleCollider = GetComponentInChildren(); skinMeshRenderer = GetComponentInChildren(); diff --git a/Assets/Scripts/Tools/SDF/Helper/Base.cs b/Assets/Scripts/Tools/SDF/Helper/Base.cs index d14f5e0c..9fe44d92 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Base.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Base.cs @@ -12,10 +12,14 @@ namespace Helper { public class Base : UE.MonoBehaviour { + private Model _rootModelInScopre = null; + private PoseControl _poseControl = null; private bool _isRootModel = false; + public Model RootModel => _rootModelInScopre; + public bool IsFirstChild => _isRootModel; // root model protected void Awake() @@ -23,6 +27,13 @@ protected void Awake() _isRootModel = SDF2Unity.IsRootModel(this.gameObject); _poseControl = new PoseControl(this.transform); Reset(); + + UpdateRootModel(); + } + + protected void Start() + { + UpdateRootModel(); } public void Reset() @@ -30,6 +41,13 @@ public void Reset() ResetPose(); } + private void UpdateRootModel() + { + var modelHelpers = GetComponentsInParent(typeof(Model)); + _rootModelInScopre = (Model)modelHelpers[modelHelpers.Length - 1]; + // UE.Debug.Log($"{name}: BaseHelper _rootModel={_rootModel}"); + } + public void ClearPose() { if (_poseControl != null) diff --git a/Assets/Scripts/Tools/SDF/Helper/Collision.cs b/Assets/Scripts/Tools/SDF/Helper/Collision.cs index d814f14c..850f179d 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Collision.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Collision.cs @@ -14,8 +14,9 @@ public class Collision : Base { // [UE.Header("SDF Properties")] - void Start() + new protected void Start() { + base.Start(); } } } diff --git a/Assets/Scripts/Tools/SDF/Helper/Link.cs b/Assets/Scripts/Tools/SDF/Helper/Link.cs index 7993b648..252dfd0b 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Link.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Link.cs @@ -13,7 +13,6 @@ namespace Helper { public class Link : Base { - private Model _rootModel = null; private Model _parentModelHelper = null; private UE.ArticulationBody _artBody = null; private UE.ArticulationBody _parentArtBody = null; @@ -76,7 +75,6 @@ public float JointAxis2LimitVelocity public UE.Pose LinkJointPose => _jointPose; - public Model RootModel => _rootModel; public Model Model => _parentModelHelper; new protected void Awake() @@ -84,14 +82,12 @@ public float JointAxis2LimitVelocity base.Awake(); _parentModelHelper = transform.parent?.GetComponent(); _jointAnchorPose = UE.Pose.identity; - - UpdateRootModel(); } // Start is called before the first frame update - void Start() + new protected void Start() { - UpdateRootModel(); + base.Start(); var parentArtBodies = GetComponentsInParent(); @@ -137,13 +133,6 @@ void LateUpdate() } } - private void UpdateRootModel() - { - var modelHelpers = GetComponentsInParent(typeof(Model)); - _rootModel = (Model)modelHelpers[modelHelpers.Length - 1]; - // UE.Debug.Log($"{name}: LinkHelper _rootModel={_rootModel}"); - } - void OnDrawGizmos() { if (_artBody && drawInertia) @@ -201,12 +190,12 @@ private UE.Collider[] GetCollidersInChildren() private void IgnoreSelfCollision() { - if (_rootModel == null) + if (RootModel == null) { return; } - var otherLinkPlugins = _rootModel.GetComponentsInChildren(); + var otherLinkPlugins = RootModel.GetComponentsInChildren(); var thisColliders = GetCollidersInChildren(); foreach (var otherLinkPlugin in otherLinkPlugins) diff --git a/Assets/Scripts/Tools/SDF/Helper/Model.cs b/Assets/Scripts/Tools/SDF/Helper/Model.cs index 1133bbc3..e4481fd1 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Model.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Model.cs @@ -23,9 +23,12 @@ public class Model : Base [UE.Header("SDF Properties")] public bool isStatic; + public bool isNested; - void Start() + new protected void Start() { + base.Start(); + if (isStatic) { // if parent model has static option, make it all static in children diff --git a/Assets/Scripts/Tools/SDF/Helper/Visual.cs b/Assets/Scripts/Tools/SDF/Helper/Visual.cs index 89940e6e..4d3d57a7 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Visual.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Visual.cs @@ -17,8 +17,10 @@ public class Visual : Base public int metaLayer = -1; - void Start() + new protected void Start() { + base.Start(); + SetShadowMode(); } From cc4f9f81ca765783a7d8fc9a43775b09e0b600b2 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 5 Sep 2024 18:45:02 +0900 Subject: [PATCH 19/26] Modify SDF.Import.Actor - move code inside null checking --- .../Scripts/Tools/SDF/Import/Import.Actor.cs | 44 +++++++++---------- 1 file changed, 21 insertions(+), 23 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs index ce4836e5..f745b276 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs @@ -20,38 +20,36 @@ protected override System.Object ImportActor(in Actor actor) } var newActorObject = Implement.Actor.CreateSkin(actor.skin); - if (newActorObject == null) + if (newActorObject != null) { - return null; - } - - newActorObject.name = actor.Name; - newActorObject.tag = "Actor"; + newActorObject.name = actor.Name; + newActorObject.tag = "Actor"; - // Apply attributes - var localPosition = SDF2Unity.Position(actor.Pose?.Pos); - var localRotation = SDF2Unity.Rotation(actor.Pose?.Rot); - // Debug.Log(newActorObject.name + "::" + localPosition + ", " + localRotation); + // Apply attributes + var localPosition = SDF2Unity.Position(actor.Pose?.Pos); + var localRotation = SDF2Unity.Rotation(actor.Pose?.Rot); + // Debug.Log(newActorObject.name + "::" + localPosition + ", " + localRotation); - var actorHelper = newActorObject.AddComponent(); - actorHelper.SetPose(localPosition, localRotation); - actorHelper.ResetPose(); + var actorHelper = newActorObject.AddComponent(); + actorHelper.SetPose(localPosition, localRotation); + actorHelper.ResetPose(); - newActorObject.transform.localScale = UE.Vector3.one * (float)actor.skin.scale; + newActorObject.transform.localScale = UE.Vector3.one * (float)actor.skin.scale; - var script = actor.script; - if (actor.animations != null) - { - foreach (var animation in actor.animations) + var script = actor.script; + if (actor.animations != null) { - Implement.Actor.SetAnimation(newActorObject, animation, script.auto_start, script.loop); + foreach (var animation in actor.animations) + { + Implement.Actor.SetAnimation(newActorObject, animation, script.auto_start, script.loop); + } } - } - actorHelper.SetScript(script); + actorHelper.SetScript(script); - var capsuleCollider = newActorObject.AddComponent(); - capsuleCollider.direction = 1; + var capsuleCollider = newActorObject.AddComponent(); + capsuleCollider.direction = 1; + } return newActorObject as System.Object; } From 96e9fb472d8553591eb1c231b1d895f4bc22ef88 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 5 Sep 2024 18:47:09 +0900 Subject: [PATCH 20/26] Store Pose(of sdf element) inside Helper field during Import SDF elements. And specify the all pose considering 'relative_to' attribute of pose element after all SDF is imported. --- Assets/Scripts/Tools/SDF/Helper/Base.cs | 9 ++ .../Scripts/Tools/SDF/Helper/PoseControl.cs | 9 +- .../Scripts/Tools/SDF/Import/Import.Actor.cs | 3 +- .../Scripts/Tools/SDF/Import/Import.Base.cs | 4 + .../Tools/SDF/Import/Import.Collision.cs | 3 +- .../Scripts/Tools/SDF/Import/Import.Link.cs | 6 +- .../Scripts/Tools/SDF/Import/Import.Model.cs | 4 +- .../Scripts/Tools/SDF/Import/Import.Util.cs | 144 +++++++++++++++++- .../Scripts/Tools/SDF/Import/Import.Visual.cs | 3 +- Assets/Scripts/Tools/SDF/Parser/Model.cs | 12 +- Assets/Scripts/Tools/SDF/Parser/Root.cs | 7 +- 11 files changed, 183 insertions(+), 21 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Helper/Base.cs b/Assets/Scripts/Tools/SDF/Helper/Base.cs index 9fe44d92..09818d1a 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Base.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Base.cs @@ -18,6 +18,15 @@ public class Base : UE.MonoBehaviour private bool _isRootModel = false; + [UE.Header("SDF Properties")] + private SDF.Pose _pose = null; // described in SDF file + + public Pose Pose + { + get => _pose; + set => _pose = value; + } + public Model RootModel => _rootModelInScopre; public bool IsFirstChild => _isRootModel; // root model diff --git a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs index aca03664..7085b8f8 100644 --- a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs +++ b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs @@ -247,11 +247,10 @@ public void Reset(in int targetFrame = 0) ResetArticulationBody(targetFrame); } - else - { - _targetTransform.localPosition = targetPose.position; - _targetTransform.localRotation = targetPose.rotation; - } + + _targetTransform.localPosition = targetPose.position; + _targetTransform.localRotation = targetPose.rotation; + // Debug.Log($"Reset: {_targetTransform.name} artbody({_articulationBody}) = {targetPose.position} {targetPose.rotation}"); } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs index f745b276..e32e3d36 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Actor.cs @@ -31,8 +31,7 @@ protected override System.Object ImportActor(in Actor actor) // Debug.Log(newActorObject.name + "::" + localPosition + ", " + localRotation); var actorHelper = newActorObject.AddComponent(); - actorHelper.SetPose(localPosition, localRotation); - actorHelper.ResetPose(); + actorHelper.Pose = actor?.Pose; newActorObject.transform.localScale = UE.Vector3.one * (float)actor.skin.scale; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs index af55eba7..012558d1 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs @@ -132,6 +132,8 @@ public IEnumerator Start(World world) ImportPlugins(world.GetPlugins(), worldObject); + worldObject.SpecifyPose(); + yield return null; ImportActors(world.GetActors()); @@ -150,6 +152,8 @@ public IEnumerator Start(Model model) ImportJoint(jointObject.Key, jointObject.Value); } + modelObject.SpecifyPose(); + yield return null; } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs index ed31f38c..161176da 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Collision.cs @@ -29,8 +29,7 @@ protected override System.Object ImportCollision(in SDF.Collision collision, in var localRotation = SDF2Unity.Rotation(collision.Pose?.Rot); var collisionHelper = newCollisionObject.AddComponent(); - collisionHelper.SetPose(localPosition, localRotation); - collisionHelper.ResetPose(); + collisionHelper.Pose = collision?.Pose; return newCollisionObject as System.Object; } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index 3f4dc819..2847c77a 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs @@ -48,14 +48,10 @@ protected override System.Object ImportLink(in SDF.Link link, in System.Object p targetObject.SetChild(newLinkObject); - var localPosition = SDF2Unity.Position(link.Pose?.Pos); - var localRotation = SDF2Unity.Rotation(link.Pose?.Rot); - var linkHelper = newLinkObject.AddComponent(); linkHelper.isSelfCollide = link.SelfCollide; linkHelper.useGravity = (link.Kinematic) ? false : link.Gravity; - linkHelper.SetPose(localPosition, localRotation); - linkHelper.ResetPose(); + linkHelper.Pose = link?.Pose; if (link.Battery != null) { diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs index 1d3b0936..3157b700 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Model.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Model.cs @@ -83,8 +83,8 @@ private UE.GameObject CreateModel(in SDF.Model model, in UE.GameObject parentObj var modelHelper = newModelObject.AddComponent(); modelHelper.modelNameInPath = model.OriginalName; modelHelper.isStatic = model.IsStatic; - modelHelper.SetPose(localPosition, localRotation); - modelHelper.ResetPose(); + modelHelper.Pose = model?.Pose; + modelHelper.isNested = model.IsNested; if (modelHelper.IsFirstChild) { diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs index 7267a006..635b793b 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs @@ -4,8 +4,9 @@ * SPDX-License-Identifier: MIT */ +using System.Linq; +using System; using UE = UnityEngine; -using UnityEngine; namespace SDF { @@ -33,6 +34,147 @@ public static void SetChild(this UE.GameObject parent, in UE.GameObject child) child.transform.localPosition = UE.Vector3.zero; child.transform.localRotation = UE.Quaternion.identity; } + + private static UE.Transform FindRootParentModel(SDF.Helper.Base targetBaseHelper) + { + var foundRootModelTransform = targetBaseHelper.RootModel.transform; + + if (targetBaseHelper as SDF.Helper.Model) + { + // UE.Debug.Log($"Is Model helper {targetBaseHelper.name}"); + for (var parentTransform = targetBaseHelper.transform.parent; + parentTransform != null; + parentTransform = parentTransform.parent) + { + var modelHelper = parentTransform?.GetComponent(); + if (modelHelper != null) + { + if (!modelHelper.isNested) + { + // UE.Debug.Log($"FindRootModel: modelHelper {modelHelper.name} {modelHelper.isNested} {modelHelper.name}"); + foundRootModelTransform = modelHelper.transform; + break; + } + } + } + } + else if (targetBaseHelper as SDF.Helper.Link) + { + // UE.Debug.Log($"Is not Model helper {targetBaseHelper.name}"); + for (var parentTransform = targetBaseHelper.transform.parent; + parentTransform != null; + parentTransform = parentTransform.parent) + { + var linkHelper = parentTransform?.GetComponent(); + if (linkHelper != null) + { + if (linkHelper.Model.isNested) + { + // UE.Debug.Log($"FindRootModel: {linkHelper.Model.name} {linkHelper.Model.isNested} {linkHelper.name}"); + foundRootModelTransform = linkHelper.transform; + break; + } + } + } + } + else // SDF.Helper.Collision or SDF.Helper.Visual + { + for (var parentTransform = targetBaseHelper.transform.parent; + parentTransform != null; + parentTransform = parentTransform.parent) + { + var linkHelper = parentTransform?.GetComponent(); + if (linkHelper != null) + { + // UE.Debug.Log($"FindRootModel: {linkHelper.Model.name} {linkHelper.Model.isNested} {linkHelper.name}"); + foundRootModelTransform = linkHelper.transform; + break; + } + } + } + + return foundRootModelTransform; + } + + public static void SpecifyPose(this Object targetObject) + { + var rootObject = (targetObject as UE.GameObject); + + var articulationBodies = rootObject.GetComponentsInChildren(); + + // Due to aritucaltion body transformmation + foreach (var body in articulationBodies) + { + body.enabled = false; + } + + var baseHelpers = rootObject.GetComponentsInChildren(); + foreach (var baseHelper in baseHelpers) + { + var pose = baseHelper?.Pose; + if (pose == null) + { + continue; + } + + var localPosition = SDF2Unity.Position(pose?.Pos); + var localRotation = SDF2Unity.Rotation(pose?.Rot); + + var parentObject = baseHelper.transform.parent; + + // UE.Debug.Log($"{baseHelper.name}: baseHelper: {localPosition.ToString("F5")} {localRotation.eulerAngles.ToString("F5")}"); + + if (string.IsNullOrEmpty(pose.relative_to)) + { + // UE.Debug.Log($"{baseHelper.name}: non relative_to baseHelper: {localPosition.ToString("F5")} {localRotation.eulerAngles.ToString("F5")}"); + var rootModelTransform = FindRootParentModel(baseHelper); + localRotation = UE.Quaternion.Inverse(UE.Quaternion.Inverse(rootModelTransform.localRotation) * parentObject.localRotation) * localRotation; + localPosition = (localPosition - (parentObject.position - rootModelTransform.position)); + + if (baseHelper is SDF.Helper.Link) + { + localPosition = localRotation * localPosition; + } + } + else + { + var relativeObjectBaseHelper = + baseHelper.RootModel.GetComponentsInChildren() + .FirstOrDefault(x => x.name.Equals(pose.relative_to)); + + if (relativeObjectBaseHelper != null) + { + var relativeObject = relativeObjectBaseHelper.transform; + + // UE.Debug.Log(linkHelper.name + ": ImportLink: => parent " + parentObject.name + ", " + + // parentObject.localPosition.ToString("F9") + ", " + parentObject.position.ToString("F9")); + // UE.Debug.Log(linkHelper.name + ": ImportLink: => parent " + parentObject.name + ", " + + // parentObject.localRotation.eulerAngles.ToString("F9") + ", " + parentObject.rotation.eulerAngles.ToString("F9")); + + // UE.Debug.Log(linkHelper.name + ": ImportLink: => relative_to " + relativeObject.name + ", " + + // relativeObject.localPosition.ToString("F9") + ", " + relativeObject.position.ToString("F9")); + // UE.Debug.Log(linkHelper.name + ": ImportLink: => relative_to " + relativeObject.name + ", " + + // relativeObject.localRotation.eulerAngles.ToString("F9") + ", " + relativeObject.rotation.eulerAngles.ToString("F9")); + + localPosition = localPosition + (relativeObject.position - parentObject.position); + localRotation = UE.Quaternion.Inverse(UE.Quaternion.Inverse(parentObject.localRotation) * relativeObject.localRotation) * localRotation; + // UE.Debug.LogWarning($"{linkHelper.name}: ImportLink: FINAL relative_to: {pose.relative_to} => {localPosition.ToString("F9")}, {localRotation.eulerAngles.ToString("F9")}"); + } + else + { + UE.Debug.LogWarning($"{baseHelper.name}: AdjustPose: relative_to: {pose.relative_to} NOT FOUND !!!!!!"); + } + } + + baseHelper.SetPose(localPosition, localRotation); + baseHelper.ResetPose(); + } + + foreach (var body in articulationBodies) + { + body.enabled = true; + } + } } } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs index 1d9bc98b..3b64cafa 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Visual.cs @@ -33,8 +33,7 @@ protected override System.Object ImportVisual(in SDF.Visual visual, in System.Ob var visualHelper = newVisualObject.AddComponent(); visualHelper.isCastingShadow = visual.CastShadow; visualHelper.metaLayer = visual.GetMetaLayer(); - visualHelper.SetPose(localPosition, localRotation); - visualHelper.ResetPose(); + visualHelper.Pose = visual?.Pose; return newVisualObject as System.Object; } diff --git a/Assets/Scripts/Tools/SDF/Parser/Model.cs b/Assets/Scripts/Tools/SDF/Parser/Model.cs index e360e9ea..be870b4d 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Model.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Model.cs @@ -47,6 +47,11 @@ public class Model : Entity public string OriginalName => _originalName; #endregion + #region Check nested model + private bool _isNested = false; + public bool IsNested => _isNested; + #endregion + public Model(XmlNode _node) : base(_node) { @@ -71,7 +76,12 @@ protected override void ParseElements() #region SegmentationTag and SaveWorld _originalName = GetAttribute("original_name"); - // Console.Write($"Model::ParseElements > [{_originalName}]"); + // Console.Write($"Model::ParseElements original_name > [{_originalName}]"); + #endregion + + #region Check nested model + _isNested = GetAttribute("is_nested", false); + // Console.Write($"Model{Name}::ParseElements is_nested > [{_isNested}]"); #endregion } diff --git a/Assets/Scripts/Tools/SDF/Parser/Root.cs b/Assets/Scripts/Tools/SDF/Parser/Root.cs index acc44bfd..5567b155 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Root.cs @@ -377,6 +377,11 @@ private void ReplaceAllIncludedModel() { // Console.Write("Node - " + modelNode); var importNode = _doc.ImportNode(modelNode, true); + + var newAttr = _doc.CreateAttribute("is_nested"); + newAttr.Value = "true"; + importNode.Attributes.Append(newAttr); + node.ParentNode.ReplaceChild(importNode, node); } else @@ -512,7 +517,7 @@ public void Save() _originalDoc.Save(saveName); - Console.Write(saveName); + Console.Write($"Worldfile Saved: {saveName}"); } public void Print() From 10f8e0287753e9a9add19bd4d47641e4a542d7eb Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 12 Sep 2024 16:17:53 +0900 Subject: [PATCH 21/26] Upgrade Unity editor version -> 2022.3.46f1 (LTS) --- ProjectSettings/ProjectVersion.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 29c4eeec..1e3d7990 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.45f1 -m_EditorVersionWithRevision: 2022.3.45f1 (a13dfa44d684) +m_EditorVersion: 2022.3.46f1 +m_EditorVersionWithRevision: 2022.3.46f1 (8e9b8558c41a) From 5fda23b271b3c63ddb1844fb03317c563bc19ba5 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 12 Sep 2024 16:18:07 +0900 Subject: [PATCH 22/26] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 5fe0faa8..912618e5 100644 --- a/README.md +++ b/README.md @@ -149,7 +149,7 @@ if `` element of `