From 1eec543ea6006cb27c90a210fea0950ec6a32f55 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 27 Nov 2023 18:28:27 +0900 Subject: [PATCH 01/11] Replace {parts_name} in "request_ros2" message for CLOiSimPlugin.CommonMethod --- .../Modules/Base/CLOiSimPlugin.CommonMethod.cs | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index 3b4bff42..ae0bfc6a 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -155,6 +155,7 @@ private void SetCustomHandleRequestMessage() if (string.IsNullOrEmpty(topic_name)) { topic_name = GetPluginParameters().GetValue("ros2/topic_name", partsName); + topic_name = topic_name.Replace("{parts_name}", partsName); } else { @@ -162,6 +163,12 @@ private void SetCustomHandleRequestMessage() } GetPluginParameters().GetValues("ros2/frame_id", out var frameIdList); + + for (var i = 0; i < frameIdList.Count; i++) + { + frameIdList[i] = frameIdList[i].Replace("{parts_name}", partsName); + } + SetROS2CommonInfoResponse(ref response, topic_name, frameIdList); } break; From d2c0fbc8e95d087033757e0042121c22e9a2bac2 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 29 Nov 2023 13:00:23 +0900 Subject: [PATCH 02/11] Modified naming convention in Main.cs -> private variable: _sdfRoot, _sdfLoader --- Assets/Scripts/Main.cs | 40 +++++++++++++++++++++++----------------- 1 file changed, 23 insertions(+), 17 deletions(-) diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index f11fee63..2642ef94 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -65,8 +65,8 @@ public class Main : MonoBehaviour public static CameraControl CameraControl => cameraControl; #region "SDFParser" - private SDF.Root sdfRoot = null; - private SDF.Import.Loader sdfLoader = null; + private SDF.Root _sdfRoot = null; + private SDF.Import.Loader _sdfLoader = null; #endregion private void CleanAllModels() @@ -282,8 +282,14 @@ void Start() } } - private void UpdateUIModelList(ref SDF.Root root) + private void UpdateUIModelList() { + if (_sdfRoot == null) + { + Debug.LogWarning("_sdfRoot is null"); + return; + } + // Update UI Model list var modelList = uiMainCanvasRoot.transform.Find("ModelList").gameObject; var viewport = modelList.transform.GetChild(0); @@ -295,7 +301,7 @@ private void UpdateUIModelList(ref SDF.Root root) GameObject.Destroy(child.gameObject); } - foreach (var item in root.resourceModelTable) + foreach (var item in _sdfRoot.resourceModelTable) { var itemValue = item.Value; var duplicatedbutton = GameObject.Instantiate(buttonTemplate); @@ -333,12 +339,12 @@ private string GetClonedModelName(in string modelName) private IEnumerator LoadModel(string modelPath, string modelFileName) { - if (sdfRoot.DoParse(out var model, modelPath, modelFileName)) + if (_sdfRoot.DoParse(out var model, modelPath, modelFileName)) { // Debug.Log("Parsed: " + item.Key + ", " + item.Value.Item1 + ", " + item.Value.Item2); model.Name = GetClonedModelName(model.Name); - yield return StartCoroutine(sdfLoader.StartImport(model)); + yield return StartCoroutine(_sdfLoader.StartImport(model)); var targetObject = worldRoot.transform.Find(model.Name); @@ -360,21 +366,21 @@ private IEnumerator LoadWorld() // Debug.Log("Hello CLOiSim World!!!!!"); Debug.Log("Target World: " + worldFileName); - sdfRoot = new SDF.Root(); - sdfRoot.fileDefaultPaths.AddRange(fileRootDirectories); - sdfRoot.modelDefaultPaths.AddRange(modelRootDirectories); - sdfRoot.worldDefaultPaths.AddRange(worldRootDirectories); - sdfRoot.UpdateResourceModelTable(); + _sdfRoot = new SDF.Root(); + _sdfRoot.fileDefaultPaths.AddRange(fileRootDirectories); + _sdfRoot.modelDefaultPaths.AddRange(modelRootDirectories); + _sdfRoot.worldDefaultPaths.AddRange(worldRootDirectories); + _sdfRoot.UpdateResourceModelTable(); - UpdateUIModelList(ref sdfRoot); + UpdateUIModelList(); - if (sdfRoot.DoParse(out var world, worldFileName)) + if (_sdfRoot.DoParse(out var world, worldFileName)) { - sdfLoader = new SDF.Import.Loader(); - sdfLoader.SetRootModels(worldRoot); - sdfLoader.SetRootLights(lightsRoot); + _sdfLoader = new SDF.Import.Loader(); + _sdfLoader.SetRootModels(worldRoot); + _sdfLoader.SetRootLights(lightsRoot); - yield return sdfLoader.StartImport(world); + yield return _sdfLoader.StartImport(world); // for GUI followingList?.UpdateList(); From 341437a53329f0560f3e4aabba71a674ec86962f Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 29 Nov 2023 15:48:33 +0900 Subject: [PATCH 03/11] Fix uncreated navMesh area for actors -> problem with order of loading actors and worlds(model) -> Load actors after all worlds are loaded --- Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs | 1 - Assets/Scripts/Core/WorldNavMeshBuilder.cs | 6 +++--- Assets/Scripts/Tools/SDF/Import/Import.Base.cs | 6 ++---- 3 files changed, 5 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs index e03db48b..ad5ba54b 100644 --- a/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/ActorPlugin.cs @@ -32,7 +32,6 @@ protected override void OnStart() return; } - GetPluginParameters().GetValues("activity_zone/model", out var zoneList); foreach (var zone in zoneList) diff --git a/Assets/Scripts/Core/WorldNavMeshBuilder.cs b/Assets/Scripts/Core/WorldNavMeshBuilder.cs index 08beb18f..acd65847 100644 --- a/Assets/Scripts/Core/WorldNavMeshBuilder.cs +++ b/Assets/Scripts/Core/WorldNavMeshBuilder.cs @@ -111,10 +111,10 @@ public void AddNavMeshTracks(in Transform transform) var bounds = new Bounds(transform.position, Vector3.zero); var renderers = transform.GetComponentsInChildren(); - for (var i = 0; i < renderers.Length; i++) + foreach (var render in renderers) { - // Debug.Log(renderer.bounds.center + ", " + renderer.bounds.size); - bounds.Encapsulate(renderers[i].bounds); + // Debug.Log(render.bounds.center + ", " + render.bounds.size); + bounds.Encapsulate(render.bounds); } // Debug.Log("Final: " + bounds.center + ", " + bounds.size); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs index a4ea7e97..a1f27ca9 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs @@ -135,19 +135,17 @@ public IEnumerator StartImport(World world) var worldObject = ImportWorld(world); - ImportActors(world.GetActors()); - yield return null; - ImportModels(world.GetModels()); foreach (var jointObject in jointObjectList) { ImportJoint(jointObject.Key, jointObject.Value); } - yield return null; ImportPlugins(world.GetPlugins(), worldObject); yield return null; + + ImportActors(world.GetActors()); } public IEnumerator StartImport(Model model) From 10465bc549a81b0fd69027da2705924c05659637 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 29 Nov 2023 18:16:37 +0900 Subject: [PATCH 04/11] Modfiy MicomPlugin -> get value in to enable odometry calculation by imu --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 7 +++++++ Assets/Scripts/Devices/MicomSensor.cs | 15 ++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 3093719f..e3c10bd4 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -114,6 +114,13 @@ private void SetupMicom() { micomSensor.SetBumperSensor(bumperJointNameList); } + + var targetImuName = GetPluginParameters().GetValue("imu"); + if (!string.IsNullOrEmpty(targetImuName)) + { + // Debug.Log("Imu Sensor = " + targetImuName); + micomSensor.SetIMU(targetImuName); + } } private void LoadStaticTF() diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index 9fcf8360..0ee8d684 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -34,7 +34,6 @@ protected override void OnAwake() protected override void OnStart() { - imuSensor = gameObject.GetComponentInChildren(); } protected override IEnumerator OnVisualize() @@ -42,6 +41,20 @@ protected override IEnumerator OnVisualize() yield return null; } + public void SetIMU(in string name) + { + var imuList = gameObject.GetComponentsInChildren(); + foreach (var imu in imuList) + { + if (imu.DeviceName.Contains("::" + name + "::")) + { + // Debug.Log(imu.DeviceName + " attached to Micom"); + imuSensor = imu; + break; + } + } + } + public void SetWheel(in string wheelNameLeft, in string wheelNameRight) { var linkList = GetComponentsInChildren(); From 62b208e4667bbfcdad60eea3dd42c8ae01562ac0 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Thu, 30 Nov 2023 18:18:30 +0900 Subject: [PATCH 05/11] modify Globalclock -> Add fixedSimTime Modify DeviceHelper -> add SetTime --- Assets/Scripts/Devices/Clock.cs | 6 +++++- Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs | 13 +++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) diff --git a/Assets/Scripts/Devices/Clock.cs b/Assets/Scripts/Devices/Clock.cs index 1127bfae..aa30f3f9 100644 --- a/Assets/Scripts/Devices/Clock.cs +++ b/Assets/Scripts/Devices/Clock.cs @@ -19,9 +19,11 @@ public class Clock : Device #endregion private double restartedSimTime = 0; + private double restartedFixedSimTime = 0; private double restartedRealTime = 0; private double currentSimTime = 0; + private double currentFixedSimTime = 0; private double currentRealTime = 0; #region time in hms format @@ -66,7 +68,7 @@ private void SetTimeString(ref string target, in TimeSpan ts) #endregion public double SimTime => currentSimTime; - + public double FixedSimTime => currentFixedSimTime; public double RealTime => currentRealTime; public HMS ToHMS() => hms; @@ -90,6 +92,7 @@ private void UpdateCurrentTime() { currentRealTime = Time.realtimeSinceStartupAsDouble - restartedRealTime; currentSimTime = Time.timeAsDouble - restartedSimTime; + currentFixedSimTime = Time.fixedTimeAsDouble - restartedFixedSimTime; } void FixedUpdate() @@ -152,6 +155,7 @@ protected override void GenerateMessage() public void ResetTime() { restartedSimTime = Time.timeAsDouble; + restartedFixedSimTime = Time.fixedTimeAsDouble; restartedRealTime = Time.realtimeSinceStartupAsDouble; UpdateCurrentTime(); diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs index 5fda92e3..7201901f 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs @@ -26,6 +26,8 @@ public static Clock GetGlobalClock() return globalClock; } + public static Clock GlobalClock => globalClock; + public static void SetGlobalSphericalCoordinates(in SphericalCoordinates sphericalCoordinates) { globalSphericalCoordinates = sphericalCoordinates; @@ -82,6 +84,17 @@ public static string GetPartName(in GameObject targetObject) [DllImport("StdHash")] public static extern ulong GetStringHashCode(string value); + public static void SetTime(messages.Time msgTime, in float time) + { + if (msgTime == null) + { + msgTime = new messages.Time(); + } + + msgTime.Sec = (int)time; + msgTime.Nsec = (int)((time - (double)msgTime.Sec) * 1e+9); + } + public static void SetCurrentTime(messages.Time msgTime, in bool useRealTime = false) { if (msgTime == null) From 94ac12468d58a2fbc6dd16713d4ded616dc72b19 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Thu, 30 Nov 2023 18:19:14 +0900 Subject: [PATCH 06/11] Fix timing for Lidar Sensor -> set captured time before request async GPU readback (texture capturing) --- Assets/Scripts/Devices/Lidar.cs | 142 +++++++++----------- Assets/Scripts/Devices/Modules/LaserData.cs | 25 +++- 2 files changed, 88 insertions(+), 79 deletions(-) diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index 6e6fd184..02d38b24 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -55,17 +55,6 @@ public partial class Lidar : Device private RTHandle _rtHandle = null; private ParallelOptions _parallelOptions = null; - struct AsyncLaserWork - { - public int dataIndex; - public AsyncGPUReadbackRequest request; - - public AsyncLaserWork(in int dataIndex, in AsyncGPUReadbackRequest request) - { - this.dataIndex = dataIndex; - this.request = request; - } - } private AsyncLaserWork[] _asyncWorkList; private DepthData.CamBuffer[] _depthCamBuffers; private LaserData.LaserCamData[] _laserCamData; @@ -325,13 +314,14 @@ private void LaserCameraWorker() if (laserCam.isActiveAndEnabled) { laserCam.Render(); + var capturedTime = (float)DeviceHelper.GetGlobalClock().SimTime; var readbackRequest = AsyncGPUReadback.Request(laserCam.targetTexture, 0, GraphicsFormat.R8G8B8A8_UNorm, OnCompleteAsyncReadback); if (_asyncWorkList.Length == numberOfLaserCamData) - _asyncWorkList[dataIndex] = new AsyncLaserWork(dataIndex, readbackRequest); - } + _asyncWorkList[dataIndex] = new AsyncLaserWork(dataIndex, readbackRequest, capturedTime); - laserCam.enabled = false; + laserCam.enabled = false; + } } _lastTimeLaserCameraWork = Time.time; @@ -349,39 +339,40 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request) for (var i = 0; i < _asyncWorkList.Length; i++) { var asyncWork = _asyncWorkList[i]; - if (asyncWork.request.Equals(request)) - { - var dataIndex = asyncWork.dataIndex; - var depthCamBuffer = _depthCamBuffers[dataIndex]; + if (!asyncWork.request.Equals(request)) + continue; - depthCamBuffer.Allocate(); - depthCamBuffer.raw = request.GetData(); + var dataIndex = asyncWork.dataIndex; + var depthCamBuffer = _depthCamBuffers[dataIndex]; - if (depthCamBuffer.depth.IsCreated) - { - var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize); - jobHandleDepthCamBuffer.Complete(); + depthCamBuffer.Allocate(); + depthCamBuffer.raw = request.GetData(); - var laserCamData = _laserCamData[dataIndex]; - laserCamData.depthBuffer = depthCamBuffer.depth; - laserCamData.Allocate(); + if (depthCamBuffer.depth.IsCreated) + { + var jobHandleDepthCamBuffer = depthCamBuffer.Schedule(depthCamBuffer.Length(), BatchSize); + jobHandleDepthCamBuffer.Complete(); - var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize); - jobHandle.Complete(); + var laserCamData = _laserCamData[dataIndex]; + laserCamData.depthBuffer = depthCamBuffer.depth; + laserCamData.Allocate(); - _laserDataOutput[dataIndex].data = laserCamData.GetLaserData(); + var jobHandle = laserCamData.Schedule(laserCamData.OutputLength(), BatchSize); + jobHandle.Complete(); - if (noise != null) - { - noise.Apply(ref _laserDataOutput[dataIndex].data); - } + _laserDataOutput[dataIndex].data = laserCamData.GetLaserData(); + _laserDataOutput[dataIndex].capturedTime = asyncWork.capturedTime; - laserCamData.Deallocate(); + if (noise != null) + { + noise.Apply(ref _laserDataOutput[dataIndex].data); } - depthCamBuffer.Deallocate(); - break; + laserCamData.Deallocate(); } + + depthCamBuffer.Deallocate(); + break; } } } @@ -408,6 +399,9 @@ protected override void GenerateMessage() var laserEndAngleV = (float)vertical.angle.max; var laserTotalAngleV = (float)vertical.angle.range; + const int TargetCaptureTimeIndex = 1; + var capturedTime = (float)DeviceHelper.GlobalClock.FixedSimTime; + Parallel.For(0, numberOfLaserCamData, _parallelOptions, index => { var laserCamData = _laserCamData[index]; @@ -428,6 +422,11 @@ protected override void GenerateMessage() return; } + if (index == TargetCaptureTimeIndex) + { + capturedTime = _laserDataOutput[index].capturedTime; + } + if (laserStartAngleH < 0 && dataEndAngleH > DEG180) { dataStartAngleH -= DEG360; @@ -435,54 +434,33 @@ protected override void GenerateMessage() } // Debug.LogFormat("index {0}: {1}~{2}, {3}~{4}", dataIndex, laserStartAngleH, laserEndAngleH, dataStartAngleH, dataEndAngleH); - var dataCopyType = -1; - for (var sampleIndexV = 0; sampleIndexV < laserSamplesV; sampleIndexV++, doCopy = true) { - if (dataStartAngleH <= laserStartAngleH) + if (dataStartAngleH <= laserStartAngleH) // start side of laser angle { - dataCopyType = 1; // start side of laser angle + dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH; + copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); + srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; + dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength; } - else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) + else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) // middle of laser angle { - dataCopyType = 2; // middle of laser angle + dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH; + copyLength = srcBufferHorizontalLength; + srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; + dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength; } - else if (dataEndAngleH >= laserEndAngleH) + else if (dataEndAngleH >= laserEndAngleH) // end side of laser angle { - dataCopyType = 3; // end side of laser angle + dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH; + copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); + srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength; + dstBufferOffset = laserSamplesH * sampleIndexV + 1; } else { - dataCopyType = -1; - } - - switch (dataCopyType) - { - case 1: - dataLengthRatio = (laserStartAngleH - dataStartAngleH) * dividedDataTotalAngleH; - copyLength = srcBufferHorizontalLength - Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); - srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; - dstBufferOffset = laserSamplesH * (sampleIndexV + 1) - copyLength; - break; - - case 2: - dataLengthRatio = (dataStartAngleH - laserStartAngleH) * dividedLaserTotalAngleH; - copyLength = srcBufferHorizontalLength; - srcBufferOffset = srcBufferHorizontalLength * sampleIndexV; - dstBufferOffset = Mathf.CeilToInt(laserSamplesH * ((float)(sampleIndexV + 1) - dataLengthRatio)) - copyLength; - break; - - case 3: - dataLengthRatio = (laserEndAngleH - dataStartAngleH) * dividedDataTotalAngleH; - copyLength = Mathf.CeilToInt(srcBufferHorizontalLength * dataLengthRatio); - srcBufferOffset = srcBufferHorizontalLength * (sampleIndexV + 1) - copyLength; - dstBufferOffset = laserSamplesH * sampleIndexV + 1; - break; - - default: - Debug.LogWarning("Something wrong data copy type in Generating Laser Data...."); - doCopy = false; - break; + Debug.LogWarning("Something wrong data copy type in Generating Laser Data...."); + doCopy = false; } if (doCopy && copyLength >= 0 && dstBufferOffset >= 0) @@ -496,7 +474,16 @@ protected override void GenerateMessage() } catch (Exception ex) { - Debug.LogWarning("Error occured with Buffer.BlockCopy: " + ex.Message + ", Type: " + dataCopyType + " Offset: src(" + srcBufferOffset + ") dst(" + dstBufferOffset + ") Len: src(" + srcBuffer.Length + ") dst(" + laserScan.Ranges.Length + ") copy_size(" + copyLength + ")"); + var copyType = -1; + if (dataStartAngleH <= laserStartAngleH) + copyType = 0; + else if (dataStartAngleH > laserStartAngleH && dataEndAngleH < laserEndAngleH) + copyType = 1; + else if (dataEndAngleH >= laserEndAngleH) + copyType = 2; + + Debug.LogWarningFormat("Error occured with Buffer.BlockCopy: {0}, Type: {1} Offset: src({2}) dst({3}) Len: src({4}) dst({5}) copy_size({6})", + ex.Message, copyType, srcBufferOffset, dstBufferOffset, srcBuffer.Length, laserScan.Ranges.Length, copyLength); } } else @@ -511,7 +498,8 @@ protected override void GenerateMessage() laserFilter.DoFilter(ref laserScan); } - DeviceHelper.SetCurrentTime(laserScanStamped.Time); + DeviceHelper.SetTime(laserScanStamped.Time, capturedTime); + PushDeviceMessage(laserScanStamped); } diff --git a/Assets/Scripts/Devices/Modules/LaserData.cs b/Assets/Scripts/Devices/Modules/LaserData.cs index 8909b413..4539171c 100644 --- a/Assets/Scripts/Devices/Modules/LaserData.cs +++ b/Assets/Scripts/Devices/Modules/LaserData.cs @@ -3,7 +3,7 @@ * * SPDX-License-Identifier: MIT */ - +using UnityEngine.Rendering; using UnityEngine; using Unity.Collections; using Unity.Jobs; @@ -78,9 +78,12 @@ public struct LaserDataOutput { public double[] data; + public float capturedTime; + public LaserDataOutput(in int length = 0) { data = (length == 0) ? null : new double[length]; + capturedTime = 0; } } @@ -109,7 +112,10 @@ public struct LaserCamData : IJobParallelFor public float EndAngleH; public float TotalAngleH; - public LaserCamData(in int bufferWidth, in int bufferHeight, in MinMax range, in AngleResolution angleResolution, in float centerAngle, in float halfHFovAngle, in float halfVFovAngle) + public LaserCamData( + in int bufferWidth, in int bufferHeight, + in MinMax range, in AngleResolution angleResolution, + in float centerAngle, in float halfHFovAngle, in float halfVFovAngle) { this.maxHAngleHalf = halfHFovAngle; this.maxHAngleHalfTanInverse = 1 / Mathf.Tan(maxHAngleHalf * Mathf.Deg2Rad); @@ -207,4 +213,19 @@ public double[] GetLaserData() } } } + + struct AsyncLaserWork + { + public int dataIndex; + public AsyncGPUReadbackRequest request; + public float capturedTime; + + public AsyncLaserWork(in int dataIndex, in AsyncGPUReadbackRequest request, in float capturedTime) + { + this.dataIndex = dataIndex; + this.request = request; + this.capturedTime = capturedTime; + } + } + } \ No newline at end of file From 0793981590e0588ee47cc965e798d213f4036280 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 4 Dec 2023 16:57:54 +0900 Subject: [PATCH 07/11] Bug fix in wheel odometry calculationa Modfiy GetCurrentVelocity() - apply rolling mean filtering --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 10 +- Assets/Scripts/Devices/MicomSensor.cs | 14 +-- .../Scripts/Devices/Modules/Articulation.cs | 12 ++- .../Devices/Modules/Base/DeviceHelper.cs | 5 + Assets/Scripts/Devices/Modules/Motor.cs | 69 ++++++++---- .../Scripts/Devices/Modules/MotorControl.cs | 20 ++-- Assets/Scripts/Devices/Modules/Odometry.cs | 101 +++++++++++------- 7 files changed, 154 insertions(+), 77 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index e3c10bd4..e8a7797c 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -67,12 +67,18 @@ private void SetupMicom() micomSensor.SetUpdateRate(updateRate); var wheelRadius = GetPluginParameters().GetValue("wheel/radius"); - var wheelTread = GetPluginParameters().GetValue("wheel/tread"); + var wheelTread = GetPluginParameters().GetValue("wheel/tread"); // deprecated + if (Mathf.Approximately(Mathf.Abs(wheelTread), Quaternion.kEpsilon) == false) + { + Debug.LogWarning(" will be depreacted!!"); + } + + var wheelSeparation = GetPluginParameters().GetValue("wheel/separation", wheelTread); var P = GetPluginParameters().GetValue("wheel/PID/kp"); var I = GetPluginParameters().GetValue("wheel/PID/ki"); var D = GetPluginParameters().GetValue("wheel/PID/kd"); - micomSensor.SetMotorConfiguration(wheelRadius, wheelTread, P, I, D); + micomSensor.SetMotorConfiguration(wheelRadius, wheelSeparation, P, I, D); // TODO: to be utilized, currently not used var motorFriction = GetPluginParameters().GetValue("wheel/friction/motor", 0.1f); diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index 0ee8d684..2beb8427 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -48,7 +48,7 @@ public void SetIMU(in string name) { if (imu.DeviceName.Contains("::" + name + "::")) { - // Debug.Log(imu.DeviceName + " attached to Micom"); + Debug.Log(imu.DeviceName + " attached to Micom"); imuSensor = imu; break; } @@ -115,10 +115,10 @@ public void SetWheel(in string frontWheelLeftName, in string frontWheelRightName } } - public void SetMotorConfiguration(in float wheelRadius, in float wheelTread, in float P, in float I, in float D) + public void SetMotorConfiguration(in float wheelRadius, in float wheelSeparation, in float P, in float I, in float D) { motorControl.SetPID(P, I, D); - motorControl.SetWheelInfo(wheelRadius, wheelTread); + motorControl.SetWheelInfo(wheelRadius, wheelSeparation); } public void SetUSS(in List ussList) @@ -229,7 +229,6 @@ protected override void InitializeMessages() protected override void GenerateMessage() { - DeviceHelper.SetCurrentTime(micomSensorData.Time); PushDeviceMessage(micomSensorData); } @@ -240,14 +239,17 @@ void FixedUpdate() return; } - motorControl.UpdateTime(Time.fixedDeltaTime); + var deltaTime = Time.fixedDeltaTime; + + motorControl.Update(deltaTime); UpdateIMU(); UpdateUss(); UpdateIr(); UpdateBumper(); - motorControl.UpdateOdometry(micomSensorData.Odom, Time.fixedDeltaTime, imuSensor); + motorControl.UpdateOdometry(micomSensorData.Odom, deltaTime, imuSensor); + DeviceHelper.SetTime(micomSensorData.Time, DeviceHelper.GlobalClock.FixedSimTime); } private void UpdateBumper() diff --git a/Assets/Scripts/Devices/Modules/Articulation.cs b/Assets/Scripts/Devices/Modules/Articulation.cs index 7fe07626..9a4d51f3 100644 --- a/Assets/Scripts/Devices/Modules/Articulation.cs +++ b/Assets/Scripts/Devices/Modules/Articulation.cs @@ -70,13 +70,15 @@ public Vector3 GetAnchorRotation() return (_jointBody == null) ? Vector3.zero : _jointBody.anchorRotation.eulerAngles; } - /// in radian for angular and in meters for linear + /// in (rad)ian for angular OR in (m)eters for linear public float GetJointPosition(int index = 0) { + if (_jointBody == null) + return 0; + if (IsRevoluteType()) { - index = GetValidIndex(index); - return (_jointBody == null || index == -1) ? 0 : _jointBody.jointPosition[index]; + return (index == -1) ? 0 : _jointBody.jointPosition[index]; } else { @@ -98,6 +100,10 @@ public float GetJointPosition(int index = 0) return _jointBody.transform.localPosition.y; } } + else + { + Debug.LogWarning("Unsupported articulation Type: " + Type); + } } return 0; diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs index 7201901f..abf312b9 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceHelper.cs @@ -84,6 +84,11 @@ public static string GetPartName(in GameObject targetObject) [DllImport("StdHash")] public static extern ulong GetStringHashCode(string value); + public static void SetTime(messages.Time msgTime, in double time) + { + SetTime(msgTime, (float)time); + } + public static void SetTime(messages.Time msgTime, in float time) { if (msgTime == null) diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index 0bc35c99..5a34752f 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +using System.Collections.Generic; using UnityEngine; public class Motor : Articulation @@ -82,12 +83,7 @@ public float Compensate() { if (IsMotionRotating) { - if (IsTargetReached() == false) - { - _compensateValue += CompensatingVelocityIncrease; - // Debug.Log("_test: it is low speed, " + _currentTwistAngularVelocity + " < " + _targetTwistAngularVelocity); - } - else + if (IsTargetReached()) { _compensateValue -= CompensatingVelocityDecrease; @@ -96,6 +92,11 @@ public float Compensate() _compensateValue = 0; } } + else + { + _compensateValue += CompensatingVelocityIncrease; + // Debug.Log("_test: it is low speed, " + _currentTwistAngularVelocity + " < " + _targetTwistAngularVelocity); + } } else { @@ -116,7 +117,6 @@ public float Compensate() private float _targetAngularVelocity = 0; private float _currentMotorVelocity = 0; // degree per seconds - private float _prevJointPosition = 0; public Motor(in GameObject gameObject) : base(gameObject) @@ -139,9 +139,9 @@ public float GetCurrentVelocity() /// degree per second public void SetVelocityTarget(in float targetAngularVelocity) { - var compensatingVelocityRatio = 0f; + var compensatingVelocityRatio = 1f; - if (Mathf.Abs(targetAngularVelocity) < Quaternion.kEpsilon) + if (Mathf.Abs(targetAngularVelocity) < Quaternion.kEpsilon || float.IsInfinity(targetAngularVelocity)) { _enableMotor = false; } @@ -155,10 +155,10 @@ public void SetVelocityTarget(in float targetAngularVelocity) _rapidControl.SetDirectionSwitched(directionSwitch); } - const float compensateThreshold = 10.0f; - const float compensatingRatio = 1.20f; + const float CompensateThreshold = 9.0f; + const float CompensatingRatio = 1.15f; - compensatingVelocityRatio = ((Mathf.Abs(targetAngularVelocity) < compensateThreshold) ? compensatingRatio : 1.0f); + compensatingVelocityRatio = ((Mathf.Abs(targetAngularVelocity) < CompensateThreshold) ? CompensatingRatio : 1f); } _targetAngularVelocity = targetAngularVelocity * compensatingVelocityRatio; @@ -172,7 +172,7 @@ public void Update(in float duration) return; } - _currentMotorVelocity = GetMotorVelocity(duration); + _currentMotorVelocity = GetAngularVelocity(duration); // do stop motion of motor when motor disabled if (_enableMotor) @@ -188,12 +188,13 @@ public void Update(in float duration) if (_rapidControl.DirectionSwitched()) { Stop(); - compensatedTargetAngularVelocity = 0; + // compensatedTargetAngularVelocity = 0; _rapidControl.Wait(); } else { Drive(compensatedTargetAngularVelocity + adjustValue); + // Drive(_targetAngularVelocity + adjustValue); } } else @@ -213,13 +214,41 @@ public void Stop() Reset(); } - private float GetMotorVelocity(in float duration) + private float _prevJointPosition = 0; // in deg + private const int RollingMeanWindowSize = 10; + private Queue rollingMeanAnguluarVelocity = new Queue(RollingMeanWindowSize); + + /// degree per second + private float GetAngularVelocity(in float duration) { - // calculate velocity using joint position is more accurate than joint velocity - var jointPosition = GetJointPosition() * Mathf.Rad2Deg; - var motorVelocity = Mathf.DeltaAngle(_prevJointPosition, jointPosition) / duration; - _prevJointPosition = jointPosition; + var motorVelocity = 0f; + if (Mathf.Approximately(Mathf.Abs(_targetAngularVelocity), Quaternion.kEpsilon) == false) + { +#if true + // calculate velocity using joint position is more accurate than joint velocity + var jointPosition = GetJointPosition() * Mathf.Rad2Deg; + motorVelocity = Mathf.DeltaAngle(_prevJointPosition, jointPosition) / duration; + _prevJointPosition = jointPosition; + // Debug.LogFormat("prv:{0:F5} cur:{1:F5} vel:{2:F5}", _prevJointPosition, jointPosition, motorVelocity); +#else + var motorVelocity = GetJointVelocity() * Mathf.Rad2Deg; +#endif + } + + // rolling mean filtering + if (rollingMeanAnguluarVelocity.Count == RollingMeanWindowSize) + rollingMeanAnguluarVelocity.Dequeue(); + + rollingMeanAnguluarVelocity.Enqueue(motorVelocity); + + var sumVelocity = 0f; + foreach (var velocity in rollingMeanAnguluarVelocity) + sumVelocity += velocity; + + var filteredVelocity = sumVelocity / (float)rollingMeanAnguluarVelocity.Count; + + // Debug.LogFormat("{0:F5} {1:F5}", motorVelocity, filteredVelocity); - return (Mathf.Approximately(motorVelocity, Quaternion.kEpsilon)) ? 0 : motorVelocity; + return (Mathf.Approximately(filteredVelocity, Quaternion.kEpsilon)) ? 0 : filteredVelocity; } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MotorControl.cs b/Assets/Scripts/Devices/Modules/MotorControl.cs index 7e1a618c..e1a1fd04 100644 --- a/Assets/Scripts/Devices/Modules/MotorControl.cs +++ b/Assets/Scripts/Devices/Modules/MotorControl.cs @@ -13,7 +13,7 @@ public class MotorControl { public enum WheelLocation { NONE, LEFT, RIGHT, REAR_LEFT, REAR_RIGHT }; -#region Motor Related + #region Motor Related private Dictionary wheelList = new Dictionary() { {WheelLocation.LEFT, null}, @@ -25,7 +25,7 @@ public enum WheelLocation { NONE, LEFT, RIGHT, REAR_LEFT, REAR_RIGHT }; private float pidGainP, pidGainI, pidGainD; private Odometry odometry = null; -#endregion + #endregion public void Reset() { @@ -35,9 +35,9 @@ public void Reset() } } - public void SetWheelInfo(in float radius, in float tread) + public void SetWheelInfo(in float radius, in float separation) { - this.odometry = new Odometry(radius, tread); + this.odometry = new Odometry(radius, separation); this.odometry.SetMotorControl(this); } @@ -69,9 +69,9 @@ public void SetDifferentialDrive(in float linearVelocityLeft, in float linearVel public void SetTwistDrive(in float linearVelocity, in float angularVelocity) { // m/s, rad/s - // var linearVelocityLeft = ((2 * linearVelocity) + (angularVelocity * wheelTread)) / (2 * wheelRadius); - // var linearVelocityRight = ((2 * linearVelocity) + (angularVelocity * wheelTread)) / (2 * wheelRadius); - var angularCalculation = (angularVelocity * odometry.WheelTread * 0.5f); + // var linearVelocityLeft = ((2 * linearVelocity) + (angularVelocity * WheelSeparation)) / (2 * wheelRadius); + // var linearVelocityRight = ((2 * linearVelocity) + (angularVelocity * WheelSeparation)) / (2 * wheelRadius); + var angularCalculation = (angularVelocity * odometry.WheelSeparation * 0.5f); var linearVelocityLeft = linearVelocity - angularCalculation; var linearVelocityRight = linearVelocity + angularCalculation; @@ -82,7 +82,7 @@ public void SetTwistDrive(in float linearVelocity, in float angularVelocity) public void UpdateMotorFeedback(in float linearVelocityLeft, in float linearVelocityRight) { var linearVelocity = (linearVelocityLeft + linearVelocityRight) * 0.5f; - var angularVelocity = (linearVelocityRight - linearVelocity) / (odometry.WheelTread * 0.5f); + var angularVelocity = (linearVelocityRight - linearVelocity) / (odometry.WheelSeparation * 0.5f); UpdateTargetMotorFeedback(angularVelocity); } @@ -137,6 +137,8 @@ private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelo } } + /// Get target Motor Velocity + /// radian per second public bool GetCurrentVelocity(in WheelLocation location, out float angularVelocity) { angularVelocity = 0; @@ -149,7 +151,7 @@ public bool GetCurrentVelocity(in WheelLocation location, out float angularVeloc return false; } - public void UpdateTime(in float duration) + public void Update(in float duration) { foreach (var wheel in wheelList) { diff --git a/Assets/Scripts/Devices/Modules/Odometry.cs b/Assets/Scripts/Devices/Modules/Odometry.cs index 27c7e5e5..91e81054 100644 --- a/Assets/Scripts/Devices/Modules/Odometry.cs +++ b/Assets/Scripts/Devices/Modules/Odometry.cs @@ -11,15 +11,17 @@ public class Odometry { public struct WheelInfo { - public float wheelRadius; - public float wheelTread; - public float inverseWheelRadius; // for computational performance + public float wheelRadius; // considering contact offset + public float wheelSeparation; // wheel separation + public float inversedWheelRadius; // for computational performance + public float inversedWheelSeparation; // for computational performance - public WheelInfo(in float radius = 0.1f, in float tread = 0) + public WheelInfo(in float radius = 0.1f, in float separation = 0) { this.wheelRadius = radius; - this.wheelTread = tread; - this.inverseWheelRadius = 1.0f / wheelRadius; + this.wheelSeparation = separation; + this.inversedWheelRadius = 1.0f / wheelRadius; + this.inversedWheelSeparation = 1.0f / wheelSeparation; } } @@ -30,16 +32,17 @@ public WheelInfo(in float radius = 0.1f, in float tread = 0) private WheelInfo wheelInfo; private float lastImuYaw = 0f; + private float lastYaw = 0f; private Vector3 _odomPose = Vector3.zero; private float odomTranslationalVelocity = 0; private float odomRotationalVelocity = 0; - public float WheelTread => this.wheelInfo.wheelTread; - public float InverseWheelRadius => this.wheelInfo.inverseWheelRadius; + public float WheelSeparation => this.wheelInfo.wheelSeparation; + public float InverseWheelRadius => this.wheelInfo.inversedWheelRadius; - public Odometry(in float radius, in float tread) + public Odometry(in float radius, in float separation) { - this.wheelInfo = new WheelInfo(radius, tread); + this.wheelInfo = new WheelInfo(radius, separation); } public void SetMotorControl(in MotorControl motorControl) @@ -57,7 +60,7 @@ public void Reset() /// Calculate odometry on this robot /// rad per second for `theta` - private void CalculateOdometry(in float duration, in float angularVelocityLeftWheel, in float angularVelocityRightWheel, in float deltaTheta) + void CalculateOdometry(in float angularVelocityLeftWheel, in float angularVelocityRightWheel, in float deltaTheta, in float duration) { // circumference of wheel [rad] per step time. var wheelCircumLeft = angularVelocityLeftWheel * duration; @@ -88,6 +91,41 @@ private void CalculateOdometry(in float duration, in float angularVelocityLeftWh odomRotationalVelocity = deltaTheta * divideDuration; // rotational velocity [rad/s] } + void CalculateOdometry(in float angularVelocityLeftWheel, in float angularVelocityRightWheel, in float duration) + { + var linearVelocityLeftWheel = angularVelocityLeftWheel * wheelInfo.wheelRadius; + var linearVelocityRightWheel = angularVelocityRightWheel * wheelInfo.wheelRadius; + + odomTranslationalVelocity = (linearVelocityLeftWheel + linearVelocityRightWheel) * 0.5f; + odomRotationalVelocity = (linearVelocityRightWheel - linearVelocityLeftWheel) * wheelInfo.inversedWheelSeparation; + + var linear = odomTranslationalVelocity * duration; + var angular = odomRotationalVelocity * duration; + + // Acculumate odometry: + if (Mathf.Abs(angular) < Quaternion.kEpsilon) // RungeKutta2 + { + var direction = _odomPose.y + angular * 0.5f; + + // Runge-Kutta 2nd order integration: + _odomPose.z += linear * Mathf.Cos(direction); + _odomPose.x += linear * Mathf.Sin(direction); + _odomPose.y += angular; + // Debug.LogFormat("RungeKutta2: {0:F4} {1:F4} {2:F4} {3:F4}", _odomPose.x, _odomPose.z, _odomPose.y, direction); + } + else + { + // Exact integration (should solve problems when angular is zero): + var heading_old = _odomPose.y; + var r = linear / angular; + + _odomPose.y += angular; + _odomPose.z += r * (Mathf.Sin(_odomPose.y) - Mathf.Sin(heading_old)); + _odomPose.x += -r * (Mathf.Cos(_odomPose.y) - Mathf.Cos(heading_old)); + // Debug.LogFormat("CalculateOdometry: {0:F4} {1:F4} {2:F4} {3:F4}->{4:F4}", _odomPose.x, _odomPose.z, _odomPose.y, heading_old, _odomPose.y); + } + } + public bool Update(messages.Micom.Odometry odomMessage, in float duration, SensorDevices.IMU imuSensor) { if (odomMessage == null || motorControl == null) @@ -95,14 +133,6 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso return false; } - // var motorLeft = motorControl.GetMotor(); - // var motorRight = motorControl.GetMotor(MotorControl.WheelLocation.RIGHT); - - // if (motorLeft == null || motorRight == null) - // { - // return false; - // } - if (motorControl.GetCurrentVelocity(MotorControl.WheelLocation.LEFT, out var angularVelocityLeft) && motorControl.GetCurrentVelocity(MotorControl.WheelLocation.RIGHT, out var angularVelocityRight)) { @@ -116,33 +146,31 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso return false; } - var deltaTheta = 0f; if (imuSensor != null) { var imuOrientation = imuSensor.GetOrientation(); var yaw = imuOrientation.y * Mathf.Deg2Rad; - deltaTheta = yaw - lastImuYaw; + var deltaThetaImu = yaw - lastImuYaw; lastImuYaw = yaw; - } - else - { - var yaw = (float)((angularVelocityRight - angularVelocityLeft) * wheelInfo.wheelRadius / wheelInfo.wheelTread); - deltaTheta = yaw * duration; - } - if (deltaTheta > _PI) - { - deltaTheta -= _2_PI; + if (deltaThetaImu > _PI) + { + deltaThetaImu -= _2_PI; + } + else if (deltaThetaImu < -_PI) + { + deltaThetaImu += _2_PI; + } + + deltaThetaImu = Mathf.Approximately(deltaThetaImu, Quaternion.kEpsilon) ? 0 : deltaThetaImu; + + CalculateOdometry(angularVelocityLeft, angularVelocityRight, deltaThetaImu, duration); } - else if (deltaTheta < -_PI) + else { - deltaTheta += _2_PI; + CalculateOdometry(angularVelocityLeft, angularVelocityRight, duration); } - deltaTheta = Mathf.Approximately(deltaTheta, Quaternion.kEpsilon) ? 0 : deltaTheta; - - CalculateOdometry(duration, angularVelocityLeft, angularVelocityRight, deltaTheta); - DeviceHelper.SetVector3d(odomMessage.Pose, DeviceHelper.Convert.Reverse(_odomPose)); odomMessage.TwistLinear.X = DeviceHelper.Convert.CurveOrientation(odomTranslationalVelocity); @@ -151,7 +179,6 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso motorControl.UpdateCurrentMotorFeedback(odomRotationalVelocity); // Debug.LogFormat("jointvel: {0}, {1}", angularVelocityLeft, angularVelocityRight); // Debug.LogFormat("Odom: {0}, {1}", odomMessage.AngularVelocity.Left, odomMessage.AngularVelocity.Right); - return true; } } \ No newline at end of file From 6eb9c405485cce87e60fafa865770fc49e911ce9 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 4 Dec 2023 16:59:19 +0900 Subject: [PATCH 08/11] remove velocity compensation in Motor::SetVelocityTarget() --- Assets/Scripts/Devices/Modules/Motor.cs | 9 +-------- 1 file changed, 1 insertion(+), 8 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index 5a34752f..f61aea37 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -139,8 +139,6 @@ public float GetCurrentVelocity() /// degree per second public void SetVelocityTarget(in float targetAngularVelocity) { - var compensatingVelocityRatio = 1f; - if (Mathf.Abs(targetAngularVelocity) < Quaternion.kEpsilon || float.IsInfinity(targetAngularVelocity)) { _enableMotor = false; @@ -154,14 +152,9 @@ public void SetVelocityTarget(in float targetAngularVelocity) var directionSwitch = (Mathf.Sign(_targetAngularVelocity) == Mathf.Sign(targetAngularVelocity)) ? false : true; _rapidControl.SetDirectionSwitched(directionSwitch); } - - const float CompensateThreshold = 9.0f; - const float CompensatingRatio = 1.15f; - - compensatingVelocityRatio = ((Mathf.Abs(targetAngularVelocity) < CompensateThreshold) ? CompensatingRatio : 1f); } - _targetAngularVelocity = targetAngularVelocity * compensatingVelocityRatio; + _targetAngularVelocity = targetAngularVelocity; } public void Update(in float duration) From b44211fb462c3669bcf69994584cf4a1bdfb5e2e Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Tue, 5 Dec 2023 17:08:37 +0900 Subject: [PATCH 09/11] Modify PID control module Modify Rapid direction change control Change Stop scheme in motor Remove Compenstate velocity code in Motor module --- Assets/Scripts/Devices/MicomCommand.cs | 1 - Assets/Scripts/Devices/Modules/Motor.cs | 131 ++++++------------ .../Scripts/Devices/Modules/MotorControl.cs | 34 ----- Assets/Scripts/Devices/Modules/Odometry.cs | 34 +++-- Assets/Scripts/Devices/Modules/PID.cs | 56 +++++--- 5 files changed, 99 insertions(+), 157 deletions(-) diff --git a/Assets/Scripts/Devices/MicomCommand.cs b/Assets/Scripts/Devices/MicomCommand.cs index 9664117d..9616888a 100644 --- a/Assets/Scripts/Devices/MicomCommand.cs +++ b/Assets/Scripts/Devices/MicomCommand.cs @@ -64,7 +64,6 @@ private void DoWheelDrive(in Vector3 linearVelocity, in Vector3 angularVelocity) var targetLinearVelocity = linearVelocity.z; var targetAngularVelocity = angularVelocity.y; motorControl.SetTwistDrive(targetLinearVelocity, targetAngularVelocity); - motorControl.UpdateTargetMotorFeedback(targetAngularVelocity); } } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index f61aea37..99292ddf 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -11,7 +11,7 @@ public class Motor : Articulation { public class RapidChangeControl { - private const int _maxWaitCount = 30; + private const int MaxWaitCount = 10; private bool _directionSwitched = false; private int _waitForStopCount = 0; @@ -26,7 +26,7 @@ public void SetDirectionSwitched(in bool switched) if (_directionSwitched) { - _waitForStopCount = _maxWaitCount; + _waitForStopCount = MaxWaitCount; } } @@ -46,14 +46,7 @@ public void Wait() public class MotorMotionFeedback { - public const float CompensatingVelocityIncrease = 0.10f; - public const float CompensatingVelocityDecrease = 0.50f; - private bool _isRotating = false; - private float _currentTwistAngularVelocity = 0; - private float _targetTwistAngularVelocity = 0; - - private float _compensateValue = 0; public bool IsMotionRotating => _isRotating; @@ -61,50 +54,6 @@ public void SetMotionRotating(in bool enable) { _isRotating = enable; } - - public void SetRotatingVelocity(in float currentTwistAngularVelocity) - { - _currentTwistAngularVelocity = currentTwistAngularVelocity; - } - - public void SetRotatingTargetVelocity(in float targetTwistAngularVelocity) - { - _targetTwistAngularVelocity = targetTwistAngularVelocity; - } - - public bool IsTargetReached() - { - const float accuracy = 1000f; - // Debug.Log(" is target reached: " + _currentTwistAngularVelocity + ", " + _targetTwistAngularVelocity); - return ((int)Mathf.Abs(_currentTwistAngularVelocity * accuracy) >= (int)Mathf.Abs(_targetTwistAngularVelocity * accuracy)); - } - - public float Compensate() - { - if (IsMotionRotating) - { - if (IsTargetReached()) - { - _compensateValue -= CompensatingVelocityDecrease; - - if (_compensateValue < 0) - { - _compensateValue = 0; - } - } - else - { - _compensateValue += CompensatingVelocityIncrease; - // Debug.Log("_test: it is low speed, " + _currentTwistAngularVelocity + " < " + _targetTwistAngularVelocity); - } - } - else - { - _compensateValue = 0; - } - - return _compensateValue; - } } private RapidChangeControl _rapidControl = new RapidChangeControl(); @@ -170,46 +119,68 @@ public void Update(in float duration) // do stop motion of motor when motor disabled if (_enableMotor) { - // Compensate target angular velocity - var targetAngularVelocityCompensation = (_targetAngularVelocity != 0) ? (Mathf.Sign(_targetAngularVelocity) * _feedback.Compensate()) : 0; - - var compensatedTargetAngularVelocity = _targetAngularVelocity + targetAngularVelocityCompensation; - - var adjustValue = _pidControl.Update(compensatedTargetAngularVelocity, _currentMotorVelocity, duration); + var adjustValue = _pidControl.Update(_targetAngularVelocity, _currentMotorVelocity, duration); // Improve motion for rapid direction change if (_rapidControl.DirectionSwitched()) { - Stop(); - // compensatedTargetAngularVelocity = 0; _rapidControl.Wait(); + if (_rapidControl.DirectionSwitched() == false) + Stop(0); } else { - Drive(compensatedTargetAngularVelocity + adjustValue); - // Drive(_targetAngularVelocity + adjustValue); + Drive(_targetAngularVelocity + adjustValue); } } else { - Stop(); + const float DecreasingVelocity = 60f; // in deg + + var decelerationVelocity = _currentMotorVelocity - Mathf.Sign(_currentMotorVelocity) * DecreasingVelocity; + if (Mathf.Abs(decelerationVelocity) <= DecreasingVelocity) + { + decelerationVelocity = 0; + } + Stop(decelerationVelocity); } } - public void Stop() + public void Stop(in float decelerationVelocity = 0f) { - Drive(0); - SetJointVelocity(0); + Drive(decelerationVelocity); + SetJointVelocity(decelerationVelocity); - _pidControl.Reset(); - _rapidControl.SetDirectionSwitched(false); + if (Mathf.Abs(decelerationVelocity) < Quaternion.kEpsilon) + { + _pidControl.Reset(); + _rapidControl.SetDirectionSwitched(false); - Reset(); + Reset(); + } + } + + private const int RollingMeanWindowSize = 20; + private Queue _rollingMeanAnguluarVelocity = new Queue(RollingMeanWindowSize); + private float _rollingMeanSumVelocity = 0f; + + private float GetFilteredAngularVelocity(in float motorVelocity) + { + // rolling mean filtering + if (_rollingMeanAnguluarVelocity.Count == RollingMeanWindowSize) + { + _rollingMeanSumVelocity -= _rollingMeanAnguluarVelocity.Dequeue(); + } + + _rollingMeanAnguluarVelocity.Enqueue(motorVelocity); + _rollingMeanSumVelocity += motorVelocity; + + var filteredVelocity = _rollingMeanSumVelocity / (float)_rollingMeanAnguluarVelocity.Count; + + return filteredVelocity; } private float _prevJointPosition = 0; // in deg - private const int RollingMeanWindowSize = 10; - private Queue rollingMeanAnguluarVelocity = new Queue(RollingMeanWindowSize); /// degree per second private float GetAngularVelocity(in float duration) @@ -228,20 +199,8 @@ private float GetAngularVelocity(in float duration) #endif } - // rolling mean filtering - if (rollingMeanAnguluarVelocity.Count == RollingMeanWindowSize) - rollingMeanAnguluarVelocity.Dequeue(); - - rollingMeanAnguluarVelocity.Enqueue(motorVelocity); - - var sumVelocity = 0f; - foreach (var velocity in rollingMeanAnguluarVelocity) - sumVelocity += velocity; - - var filteredVelocity = sumVelocity / (float)rollingMeanAnguluarVelocity.Count; - - // Debug.LogFormat("{0:F5} {1:F5}", motorVelocity, filteredVelocity); + var filteredVelocity = GetFilteredAngularVelocity(motorVelocity); - return (Mathf.Approximately(filteredVelocity, Quaternion.kEpsilon)) ? 0 : filteredVelocity; + return (Mathf.Abs(filteredVelocity) < Quaternion.kEpsilon) ? 0 : filteredVelocity; } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/MotorControl.cs b/Assets/Scripts/Devices/Modules/MotorControl.cs index e1a1fd04..75260d79 100644 --- a/Assets/Scripts/Devices/Modules/MotorControl.cs +++ b/Assets/Scripts/Devices/Modules/MotorControl.cs @@ -62,7 +62,6 @@ public void SetDifferentialDrive(in float linearVelocityLeft, in float linearVel { var angularVelocityLeft = SDF2Unity.CurveOrientation(linearVelocityLeft * odometry.InverseWheelRadius); var angularVelocityRight = SDF2Unity.CurveOrientation(linearVelocityRight * odometry.InverseWheelRadius); - SetMotorVelocity(angularVelocityLeft, angularVelocityRight); } @@ -75,42 +74,9 @@ public void SetTwistDrive(in float linearVelocity, in float angularVelocity) var linearVelocityLeft = linearVelocity - angularCalculation; var linearVelocityRight = linearVelocity + angularCalculation; - SetDifferentialDrive(linearVelocityLeft, linearVelocityRight); } - public void UpdateMotorFeedback(in float linearVelocityLeft, in float linearVelocityRight) - { - var linearVelocity = (linearVelocityLeft + linearVelocityRight) * 0.5f; - var angularVelocity = (linearVelocityRight - linearVelocity) / (odometry.WheelSeparation * 0.5f); - - UpdateTargetMotorFeedback(angularVelocity); - } - - public void UpdateTargetMotorFeedback(in float angularVelocity) - { - foreach (var wheel in wheelList) - { - var motor = wheel.Value; - if (motor != null) - { - motor.Feedback.SetRotatingTargetVelocity(angularVelocity); - } - } - } - - public void UpdateCurrentMotorFeedback(in float angularVelocity) - { - foreach (var wheel in wheelList) - { - var motor = wheel.Value; - if (motor != null) - { - motor.Feedback.SetRotatingVelocity(angularVelocity); - } - } - } - /// Set motor velocity /// degree per second private void SetMotorVelocity(in float angularVelocityLeft, in float angularVelocityRight) diff --git a/Assets/Scripts/Devices/Modules/Odometry.cs b/Assets/Scripts/Devices/Modules/Odometry.cs index 91e81054..56da00ea 100644 --- a/Assets/Scripts/Devices/Modules/Odometry.cs +++ b/Assets/Scripts/Devices/Modules/Odometry.cs @@ -31,11 +31,10 @@ public WheelInfo(in float radius = 0.1f, in float separation = 0) private MotorControl motorControl = null; private WheelInfo wheelInfo; - private float lastImuYaw = 0f; - private float lastYaw = 0f; + private float _lastImuYaw = 0f; private Vector3 _odomPose = Vector3.zero; - private float odomTranslationalVelocity = 0; - private float odomRotationalVelocity = 0; + private float _odomTranslationalVelocity = 0; + private float _odomRotationalVelocity = 0; public float WheelSeparation => this.wheelInfo.wheelSeparation; public float InverseWheelRadius => this.wheelInfo.inversedWheelRadius; @@ -52,10 +51,10 @@ public void SetMotorControl(in MotorControl motorControl) public void Reset() { - odomTranslationalVelocity = 0; - odomRotationalVelocity = 0; + _odomTranslationalVelocity = 0; + _odomRotationalVelocity = 0; _odomPose.Set(0, 0, 0); - lastImuYaw = 0.0f; + _lastImuYaw = 0.0f; } /// Calculate odometry on this robot @@ -87,8 +86,8 @@ void CalculateOdometry(in float angularVelocityLeftWheel, in float angularVeloci // compute odometric instantaneouse velocity var divideDuration = 1f / duration; - odomTranslationalVelocity = poseLinear * divideDuration; // translational velocity [m/s] - odomRotationalVelocity = deltaTheta * divideDuration; // rotational velocity [rad/s] + _odomTranslationalVelocity = poseLinear * divideDuration; // translational velocity [m/s] + _odomRotationalVelocity = deltaTheta * divideDuration; // rotational velocity [rad/s] } void CalculateOdometry(in float angularVelocityLeftWheel, in float angularVelocityRightWheel, in float duration) @@ -96,11 +95,11 @@ void CalculateOdometry(in float angularVelocityLeftWheel, in float angularVeloci var linearVelocityLeftWheel = angularVelocityLeftWheel * wheelInfo.wheelRadius; var linearVelocityRightWheel = angularVelocityRightWheel * wheelInfo.wheelRadius; - odomTranslationalVelocity = (linearVelocityLeftWheel + linearVelocityRightWheel) * 0.5f; - odomRotationalVelocity = (linearVelocityRightWheel - linearVelocityLeftWheel) * wheelInfo.inversedWheelSeparation; + _odomTranslationalVelocity = (linearVelocityLeftWheel + linearVelocityRightWheel) * 0.5f; + _odomRotationalVelocity = (linearVelocityRightWheel - linearVelocityLeftWheel) * wheelInfo.inversedWheelSeparation; - var linear = odomTranslationalVelocity * duration; - var angular = odomRotationalVelocity * duration; + var linear = _odomTranslationalVelocity * duration; + var angular = _odomRotationalVelocity * duration; // Acculumate odometry: if (Mathf.Abs(angular) < Quaternion.kEpsilon) // RungeKutta2 @@ -150,8 +149,8 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso { var imuOrientation = imuSensor.GetOrientation(); var yaw = imuOrientation.y * Mathf.Deg2Rad; - var deltaThetaImu = yaw - lastImuYaw; - lastImuYaw = yaw; + var deltaThetaImu = yaw - _lastImuYaw; + _lastImuYaw = yaw; if (deltaThetaImu > _PI) { @@ -173,10 +172,9 @@ public bool Update(messages.Micom.Odometry odomMessage, in float duration, Senso DeviceHelper.SetVector3d(odomMessage.Pose, DeviceHelper.Convert.Reverse(_odomPose)); - odomMessage.TwistLinear.X = DeviceHelper.Convert.CurveOrientation(odomTranslationalVelocity); - odomMessage.TwistAngular.Z = DeviceHelper.Convert.CurveOrientation(odomRotationalVelocity); + odomMessage.TwistLinear.X = DeviceHelper.Convert.CurveOrientation(_odomTranslationalVelocity); + odomMessage.TwistAngular.Z = DeviceHelper.Convert.CurveOrientation(_odomRotationalVelocity); - motorControl.UpdateCurrentMotorFeedback(odomRotationalVelocity); // Debug.LogFormat("jointvel: {0}, {1}", angularVelocityLeft, angularVelocityRight); // Debug.LogFormat("Odom: {0}, {1}", odomMessage.AngularVelocity.Left, odomMessage.AngularVelocity.Right); return true; diff --git a/Assets/Scripts/Devices/Modules/PID.cs b/Assets/Scripts/Devices/Modules/PID.cs index d85c8438..2c72a357 100644 --- a/Assets/Scripts/Devices/Modules/PID.cs +++ b/Assets/Scripts/Devices/Modules/PID.cs @@ -10,18 +10,21 @@ public class PID { private float _pGain, _iGain, _dGain; - private float integral = 0; - private float lastError = 0; + private float _integralError = 0; + private float _lastError = 0; private float _integralMax, _integralMin; - private float _outputMax, _outputMin; + private float _cmdMax, _cmdMin; - public PID(in float pGain, in float iGain, in float dGain, in float integralMax = 100, in float integralMin = -100, in float outputMax = 1000, in float outputMin = -1000) + public PID( + in float pGain, in float iGain, in float dGain, + in float integralMax = 100, in float integralMin = -100, + in float cmdMax = 1000, in float cmdMin = -1000) { Change(pGain, iGain, dGain); this._integralMax = integralMax; this._integralMin = integralMin; - this._outputMax = outputMax; - this._outputMin = outputMin; + this._cmdMax = cmdMax; + this._cmdMin = cmdMin; } public void Change(in float pGain, in float iGain, in float dGain) @@ -33,31 +36,48 @@ public void Change(in float pGain, in float iGain, in float dGain) public void Reset() { - integral = 0; - lastError = 0; + _integralError = 0; + _lastError = 0; } public float Update(in float target, in float actual, in float deltaTime) { - var error = target - actual; + if (UnityEngine.Mathf.Abs(deltaTime) < float.Epsilon || + float.IsNaN(deltaTime) || float.IsInfinity(deltaTime)) + return 0f; - integral += (error * deltaTime); + var error = actual - target; + + // Calculate proportional contribution to command + var pTerm = _pGain * error; + + // Calculate the integral error + _integralError += deltaTime * error; + + // Calculate integral contribution to command + var iTerm = _iGain * _integralError; // Limit iTerm so that the limit is meaningful in the output - if (integral > _integralMax) + if (iTerm > _integralMax) { - integral = _integralMax / _iGain; + iTerm = _integralMax; + _integralError = iTerm / _iGain; } - else if (integral < _integralMin) + else if (iTerm < _integralMin) { - integral = _integralMin / _iGain; + iTerm = _integralMin; + _integralError = iTerm / _iGain; } - var derive = (deltaTime == 0)? 0 : ((error - lastError) / deltaTime); + // Calculate the derivative error + var dErr = (error - _lastError) / deltaTime; + _lastError = error; + + // Calculate derivative contribution to command + var dTerm = _dGain * dErr; - lastError = error; + var cmd = -pTerm - iTerm - dTerm; - var output = (error * _pGain) + (integral * _iGain) + (derive * _dGain); - return UnityEngine.Mathf.Clamp(output, _outputMin, _outputMax); + return UnityEngine.Mathf.Clamp(cmd, _cmdMin, _cmdMax); } } \ No newline at end of file From 0541f545d51bf11ab4d980feed60b5ef2da1783c Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Tue, 5 Dec 2023 17:26:23 +0900 Subject: [PATCH 10/11] Stop changing props menu during modify scale factor --- Assets/Scripts/UI/SimulationDisplay.Props.cs | 8 +++--- Assets/Scripts/UI/SimulationDisplay.cs | 28 +++++++++++--------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/Assets/Scripts/UI/SimulationDisplay.Props.cs b/Assets/Scripts/UI/SimulationDisplay.Props.cs index ba7fb1a5..b575849d 100644 --- a/Assets/Scripts/UI/SimulationDisplay.Props.cs +++ b/Assets/Scripts/UI/SimulationDisplay.Props.cs @@ -18,11 +18,7 @@ public partial class SimulationDisplay : MonoBehaviour private bool checkScaleFactorFocused = false; private bool doCheckScaleFactorValue = false; private RectOffset zeroPadding; - - private void InitPropsMenu() - { - // toolbarStrings = new string[] { "Box", "Cylinder", "Sphere" }; - } + private bool isChangingScaleFactor = false; private void DrawPropsMenus() { @@ -59,6 +55,8 @@ private void DrawPropsMenus() GUI.skin.textField.alignment = TextAnchor.MiddleCenter; scaleFactorString = GUI.TextField(rectScale, scaleFactorString, 5); + isChangingScaleFactor = (GUI.GetNameOfFocusedControl().CompareTo("ScaleField") == 0); + rectHelpButton.x = rectScale.x + rectScale.width + textRightMargin; if (checkScaleFactorFocused && !GUI.GetNameOfFocusedControl().Equals("ScaleField")) diff --git a/Assets/Scripts/UI/SimulationDisplay.cs b/Assets/Scripts/UI/SimulationDisplay.cs index f1745596..7a6a3b63 100644 --- a/Assets/Scripts/UI/SimulationDisplay.cs +++ b/Assets/Scripts/UI/SimulationDisplay.cs @@ -91,7 +91,6 @@ void Awake() padding = new RectOffset(30, 30, 30, 30); zeroPadding = new RectOffset(0, 0, 0, 0); - InitPropsMenu(); UpdateHelpContents(); } @@ -179,27 +178,30 @@ void OnGUI() if (Event.current.type.Equals(EventType.KeyUp)) { var keyCode = Event.current.keyCode; - if (keyCode.CompareTo(KeyCode.F1) == 0) + if (keyCode == KeyCode.F1) { popupHelpDialog = !popupHelpDialog; cameraControl.BlockMouseWheelControl(true); } - else if (keyCode.CompareTo(KeyCode.Escape) == 0) + else if (keyCode == KeyCode.Escape) { popupHelpDialog = false; cameraControl.BlockMouseWheelControl(false); } - else if (keyCode.CompareTo(KeyCode.Alpha1) == 0) + else if (keyCode >= KeyCode.Alpha1 && keyCode <= KeyCode.Alpha9 && isChangingScaleFactor == false) { - _toolbarSelected = 0; - } - else if (keyCode.CompareTo(KeyCode.Alpha2) == 0) - { - _toolbarSelected = 1; - } - else if (keyCode.CompareTo(KeyCode.Alpha3) == 0) - { - _toolbarSelected = 2; + switch (keyCode) + { + case KeyCode.Alpha1: + _toolbarSelected = 0; + break; + case KeyCode.Alpha2: + _toolbarSelected = 1; + break; + case KeyCode.Alpha3: + _toolbarSelected = 2; + break; + } } } From cefdfc0bf2605b8bb47578ec4068249734021ab7 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Tue, 5 Dec 2023 17:37:01 +0900 Subject: [PATCH 11/11] Update app version info in Project settings : 4.2.1 -> 4.2.2 --- ProjectSettings/ProjectSettings.asset | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 4356d57c..f3e37eae 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -136,7 +136,7 @@ PlayerSettings: vulkanEnableLateAcquireNextImage: 0 vulkanEnableCommandBufferRecycling: 1 loadStoreDebugModeEnabled: 0 - bundleVersion: 4.2.1 + bundleVersion: 4.2.2 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0