From dd46bb08c7692ab06603c6f9f7064738dc5379a5 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 16 Oct 2023 11:17:17 +0900 Subject: [PATCH 01/17] Remove and combined redundacy common method for MicomPluing and JointControlPlugin --- .../CLOiSimPlugins/JointControlPlugin.cs | 52 ------------------- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 49 ----------------- .../Base/CLOiSimPlugin.CommonMethod.cs | 47 +++++++++++++++++ .../Modules/Base/CLOiSimPlugin.cs | 2 + 4 files changed, 49 insertions(+), 101 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs index bef7aa5a..4882dc2b 100644 --- a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs @@ -10,7 +10,6 @@ public class JointControlPlugin : CLOiSimPlugin { - private List staticTfList = new List(); private List tfList = new List(); private SensorDevices.JointCommand jointCommand = null; private SensorDevices.JointState jointState = null; @@ -91,59 +90,8 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any { switch (requestType) { - case "request_static_transforms": - SetStaticTransformsResponse(ref response); - break; - - case "reset_odometry": - Reset(); - SetEmptyResponse(ref response); - break; - default: break; } } - - private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info) - { - if (msRos2Info == null) - { - return; - } - - var ros2CommonInfo = new messages.Param(); - ros2CommonInfo.Name = "static_transforms"; - ros2CommonInfo.Value = new Any { Type = Any.ValueType.None }; - - foreach (var tf in staticTfList) - { - var ros2StaticTransformLink = new messages.Param(); - ros2StaticTransformLink.Name = "parent_frame_id"; - ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId }; - - { - var tfPose = tf.GetPose(); - - var poseMessage = new messages.Pose(); - poseMessage.Position = new messages.Vector3d(); - poseMessage.Orientation = new messages.Quaternion(); - - poseMessage.Name = tf.childFrameId; - DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position); - DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation); - - var ros2StaticTransformElement = new messages.Param(); - ros2StaticTransformElement.Name = "pose"; - ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage }; - - ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement); - // Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation); - } - - ros2CommonInfo.Childrens.Add(ros2StaticTransformLink); - } - - msRos2Info.SetMessage(ros2CommonInfo); - } } \ No newline at end of file diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 62519b55..3093719f 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -7,12 +7,10 @@ using System.Collections.Generic; using System.Text; using Any = cloisim.msgs.Any; -using messages = cloisim.msgs; using UnityEngine; public class MicomPlugin : CLOiSimPlugin { - private List staticTfList = new List(); private List tfList = new List(); private SensorDevices.MicomCommand micomCommand = null; private SensorDevices.MicomSensor micomSensor = null; @@ -184,10 +182,6 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any { switch (requestType) { - case "request_static_transforms": - SetStaticTransformsResponse(ref response); - break; - case "reset_odometry": Reset(); SetEmptyResponse(ref response); @@ -197,47 +191,4 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any break; } } - - private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info) - { - if (msRos2Info == null) - { - return; - } - - var ros2CommonInfo = new messages.Param(); - ros2CommonInfo.Name = "static_transforms"; - ros2CommonInfo.Value = new Any { Type = Any.ValueType.None }; - - foreach (var tf in staticTfList) - { - var ros2StaticTransformLink = new messages.Param(); - ros2StaticTransformLink.Name = "parent_frame_id"; - ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId }; - - { - var tfPose = tf.GetPose(); - // Debug.Log(tf.parentFrameId + " <= " + tf.childFrameId + " = " + tf.link.JointAxis + ", " + tfPose); - - var poseMessage = new messages.Pose(); - poseMessage.Position = new messages.Vector3d(); - poseMessage.Orientation = new messages.Quaternion(); - - poseMessage.Name = tf.childFrameId; - DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position); - DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation); - - var ros2StaticTransformElement = new messages.Param(); - ros2StaticTransformElement.Name = "pose"; - ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage }; - - ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement); - // Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation); - } - - ros2CommonInfo.Childrens.Add(ros2StaticTransformLink); - } - - msRos2Info.SetMessage(ros2CommonInfo); - } } diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index f87e0488..3b4bff42 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -166,6 +166,10 @@ private void SetCustomHandleRequestMessage() } break; + case "request_static_transforms": + SetStaticTransformsResponse(ref response); + break; + default: HandleCustomRequestMessage(requestType, requestValue, ref response); break; @@ -178,6 +182,49 @@ private void SetCustomHandleRequestMessage() }; } + private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info) + { + if (msRos2Info == null) + { + return; + } + + var ros2CommonInfo = new messages.Param(); + ros2CommonInfo.Name = "static_transforms"; + ros2CommonInfo.Value = new Any { Type = Any.ValueType.None }; + + foreach (var tf in staticTfList) + { + var ros2StaticTransformLink = new messages.Param(); + ros2StaticTransformLink.Name = "parent_frame_id"; + ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId }; + + { + var tfPose = tf.GetPose(); + // Debug.Log(tf.parentFrameId + " <= " + tf.childFrameId + " = " + tf.link.JointAxis + ", " + tfPose); + + var poseMessage = new messages.Pose(); + poseMessage.Position = new messages.Vector3d(); + poseMessage.Orientation = new messages.Quaternion(); + + poseMessage.Name = tf.childFrameId; + DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position); + DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation); + + var ros2StaticTransformElement = new messages.Param(); + ros2StaticTransformElement.Name = "pose"; + ros2StaticTransformElement.Value = new Any { Type = Any.ValueType.Pose3d, Pose3dValue = poseMessage }; + + ros2StaticTransformLink.Childrens.Add(ros2StaticTransformElement); + // Debug.Log(poseMessage.Name + ", " + poseMessage.Position + ", " + poseMessage.Orientation); + } + + ros2CommonInfo.Childrens.Add(ros2StaticTransformLink); + } + + msRos2Info.SetMessage(ros2CommonInfo); + } + protected virtual void HandleCustomRequestMessage(in string requestType, in List requestChildren, ref DeviceMessage response) { } protected virtual void HandleCustomRequestMessage(in string requestType, in Any requestValue, ref DeviceMessage response) { } } \ No newline at end of file diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs index 4e1c73fb..1ae589bc 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.cs @@ -23,6 +23,8 @@ public abstract partial class CLOiSimPlugin : MonoBehaviour, ICLOiSimPlugin public string modelName { get; protected set; } = string.Empty; public string partsName { get; protected set; } = string.Empty; + protected List staticTfList = new List(); + private Pose pluginPose = Pose.identity; private SDF.Plugin pluginParameters; From 372562b6342398e7caa8df46dcb3d2b7f6ebd861 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 18 Oct 2023 12:13:59 +0900 Subject: [PATCH 02/17] Update README in CLOiSimPluings --- Assets/Scripts/CLOiSimPlugins/README.md | 31 +++++++++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/README.md b/Assets/Scripts/CLOiSimPlugins/README.md index abc1af96..61805ba5 100644 --- a/Assets/Scripts/CLOiSimPlugins/README.md +++ b/Assets/Scripts/CLOiSimPlugins/README.md @@ -2,9 +2,36 @@ These plugin scripts are for sensor connection. -Each class name is important to load plugin in SDF. Class name should be exact to filename which means if the class name is 'SensorPlugin' filename would be a 'libSensorPlugin.so'. But 'lib' or '.so' words can be skipped. +Each class name is important to load plugin in SDF. Class name should be exact to filename which means if the class name is 'SensorPlugin' filename would be a 'libSensorPlugin.so'. But 'lib' or '.so' words will be discarded. -For example, if it describes a name with 'RobotControl' in `` attributesm, SDF Parser will start to find a filename in plugin element as 'RobotControl' in Unity project. +For example, if it describes a name with 'RobotControl' in `` attributes, SDF Parser will start to find a filename in plugin element as 'RobotControl' in Unity project. + + +## List of plugins + +Just take one of below plugins list and write it inside of `filename` attirbute. +It is optional but recommend to append 'lib' and '.so' to the name as a prefix and postfix. + +### Model Specific + +- `LaserPlugin`: help to publish 2D or 3D lidar data +- `CameraPlugin`: help to publish 2D color image data or depth image data +- `MultiCameraPlugin`: help to publish multiple color image data +- `RealSensePlugin`: can handle ir1(left), ir2(right), depth, color +- `MicomPlugin`: control micom(differential drive) input/output(sensor) +- `GpsPlugin`: gps position in world +- `JointControlPlugin`: can control joints and help to publish joints status. +- `ActorPlugin`: add actor control functionality using AI(Unity) components +- `ImuPlugin`: help to publish IMU sensor data +- `SonarPlugin`: help to publish Sonar range data + +### World Specific + +- `ElevatorSystemPlugin`: control(lifting, cal) elevators +- `GroundTruthPlugin`: retrieve all information(position, size, velocity) for objects +- `ActorControlPlugin`: controls actor using AI(Unity) components(actor which loaded `ActorPlugin`) + +## Example of RobotControl ```xml From 6b35b0951f703fc7da82efa4b57b1a485c9afbed Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Wed, 18 Oct 2023 16:26:20 +0900 Subject: [PATCH 03/17] plugin modified to support robot_description for JointControl --- .../CLOiSimPlugins/JointControlPlugin.cs | 26 +++++++++++++++++-- Assets/Scripts/Tools/SDF/Parser/Plugin.cs | 12 +++++++++ 2 files changed, 36 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs index 4882dc2b..b00ee09c 100644 --- a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs @@ -11,6 +11,7 @@ public class JointControlPlugin : CLOiSimPlugin { private List tfList = new List(); + private string robotDescription_ = ""; private SensorDevices.JointCommand jointCommand = null; private SensorDevices.JointState jointState = null; @@ -48,6 +49,9 @@ protected override void OnStart() } LoadJoints(); + + robotDescription_ = "" + GetPluginParameters().ParentRawXml() + ""; + UnityEngine.Debug.Log(robotDescription_); } protected override void OnReset() @@ -63,11 +67,10 @@ private void LoadJoints() { foreach (var jointName in joints) { - var parentFrameId = GetPluginParameters().GetAttributeInPath("joints/joint[text()='" + jointName + "']", "parent_frame_id"); - // UnityEngine.Debug.Log("Joints loaded "+ jointName); if (jointState.AddTargetJoint(jointName, out var targetLink, out var isStatic)) { + var parentFrameId = GetPluginParameters().GetAttributeInPath("joints/joint[text()='" + jointName + "']", "parent_frame_id"); var jointParentLinkName = (parentFrameId == null) ? targetLink.JointParentLinkName : parentFrameId; var tf = new TF(targetLink, targetLink.JointChildLinkName, jointParentLinkName); if (isStatic) @@ -90,8 +93,27 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any { switch (requestType) { + case "robot_description": + SetRobotDescription(ref response); + break; + default: break; } } + + private void SetRobotDescription(ref DeviceMessage msRos2Info) + { + if (msRos2Info == null) + { + return; + } + + var ros2CommonInfo = new messages.Param(); + ros2CommonInfo.Name = "description"; + ros2CommonInfo.Value = new Any { Type = Any.ValueType.String }; + ros2CommonInfo.Value.StringValue = robotDescription_; + + msRos2Info.SetMessage(ros2CommonInfo); + } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs index 73d82748..65db32e3 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Plugin.cs +++ b/Assets/Scripts/Tools/SDF/Parser/Plugin.cs @@ -5,6 +5,8 @@ */ using System.Xml; +using System.Xml.Serialization; +using System.IO; namespace SDF { @@ -45,5 +47,15 @@ public string LibraryName() return pluginName; } + + public string ParentRawXml() + { + return root.ParentNode.OuterXml; + } + + public string RawXml() + { + return root.OuterXml; + } } } \ No newline at end of file From ab4a8f63fc3a63a25b50a2b3864ac0ea4c1b3c82 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 13:21:14 +0900 Subject: [PATCH 04/17] Upgrade Unity editor version -> 2022.3.12f1 (LTS) --- Packages/packages-lock.json | 2 +- ProjectSettings/ProjectVersion.txt | 4 ++-- ProjectSettings/ShaderGraphSettings.asset | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index ace3fb2b..f59a84c1 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -56,7 +56,7 @@ "source": "builtin", "dependencies": { "com.unity.mathematics": "1.2.1", - "com.unity.burst": "1.8.4", + "com.unity.burst": "1.8.9", "com.unity.render-pipelines.core": "14.0.9", "com.unity.shadergraph": "14.0.9" } diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 37f39b7b..e4eac159 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.11f1 -m_EditorVersionWithRevision: 2022.3.11f1 (d00248457e15) +m_EditorVersion: 2022.3.12f1 +m_EditorVersionWithRevision: 2022.3.12f1 (4fe6e059c7ef) diff --git a/ProjectSettings/ShaderGraphSettings.asset b/ProjectSettings/ShaderGraphSettings.asset index 9b28428b..3250b068 100644 --- a/ProjectSettings/ShaderGraphSettings.asset +++ b/ProjectSettings/ShaderGraphSettings.asset @@ -12,5 +12,6 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: de02f9e1d18f588468e474319d09a723, type: 3} m_Name: m_EditorClassIdentifier: + shaderVariantLimit: 128 customInterpolatorErrorThreshold: 32 customInterpolatorWarningThreshold: 16 From bf07c81d113df64146fde3d740fe20e0eb971044 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 14:51:27 +0900 Subject: [PATCH 05/17] Modify CLOiSimPlugin Connection Module - option for netmq socket - DisableTimeWait false --- .../Scripts/CLOiSimPlugins/Modules/Connection/Publisher.cs | 3 ++- .../Scripts/CLOiSimPlugins/Modules/Connection/Subscriber.cs | 5 +++-- .../CLOiSimPlugins/Modules/Connection/TransportHelper.cs | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Publisher.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Publisher.cs index 8792b4ff..7c0dca68 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Publisher.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Publisher.cs @@ -28,8 +28,9 @@ public bool Initialize(in ushort targetPort) Options.Linger = TimeSpan.FromTicks(0); Options.IPv4Only = true; Options.TcpKeepalive = true; - Options.DisableTimeWait = true; + Options.DisableTimeWait = false; Options.SendHighWatermark = TransportHelper.HighWaterMark; + Bind(TransportHelper.GetAddress(targetPort)); // publisherSocket.BindRandomPort(TransportHelper.GetAddress()); // Console.WriteLine("Publisher socket binding for - " + targetPort); diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Subscriber.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Subscriber.cs index b86720a0..009afff9 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Subscriber.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Subscriber.cs @@ -10,7 +10,7 @@ public class Subscriber : SubscriberSocket { - private TimeSpan timeout = TimeSpan.FromMilliseconds(500); + private TimeSpan timeout = TimeSpan.FromMilliseconds(100); private byte[] hashValue = null; @@ -29,7 +29,7 @@ public bool Initialize(in ushort targetPort) Options.Linger = TimeSpan.FromTicks(0); Options.IPv4Only = true; Options.TcpKeepalive = true; - Options.DisableTimeWait = true; + Options.DisableTimeWait = false; Options.ReceiveHighWatermark = TransportHelper.HighWaterMark; if (hashValue != null) @@ -53,6 +53,7 @@ public byte[] Subscribe() { if (this.TryReceiveFrameBytes(timeout, out var frameReceived)) { + // Console.Error.WriteLine(frameReceived.Length); return TransportHelper.RetrieveData(frameReceived); } } diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/TransportHelper.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/TransportHelper.cs index ef4b55b2..818b20e0 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/TransportHelper.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/TransportHelper.cs @@ -122,7 +122,7 @@ public static byte[] RetrieveData(in byte[] receivedFrame, in byte[] targetTag = { var retrievedData = new byte[dataLength]; Buffer.BlockCopy(receivedFrame, TagSize, retrievedData, 0, dataLength); - // Debug.LogWarning("dataReceived Length - " + dataLength + "," + topic.ToString()); + // UnityEngine.Debug.LogWarning("dataReceived Length - " + dataLength + "," + targetTag?.ToString()); return retrievedData; } catch From 7dab1435a6a8f77dd30096e39a887f6b84824a84 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 14:52:06 +0900 Subject: [PATCH 06/17] Modfiy DeviceMessageQueue - MaxQueue = 15 - TimeoutInMilliseconds = 200 --- Assets/Scripts/Devices/Modules/Base/Device.cs | 11 ++++++----- .../Devices/Modules/Base/DeviceMessageQueue.cs | 10 ++++++---- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Base/Device.cs b/Assets/Scripts/Devices/Modules/Base/Device.cs index e7acb86c..68987ba6 100644 --- a/Assets/Scripts/Devices/Modules/Base/Device.cs +++ b/Assets/Scripts/Devices/Modules/Base/Device.cs @@ -4,6 +4,7 @@ * SPDX-License-Identifier: MIT */ +using System; using System.Threading; using System.Collections; using UnityEngine; @@ -152,9 +153,9 @@ protected void OnDestroy() protected abstract void OnAwake(); - protected virtual void OnStart() {} + protected virtual void OnStart() { } - protected virtual void OnReset() {} + protected virtual void OnReset() { } protected virtual IEnumerator OnVisualize() @@ -190,7 +191,7 @@ private IEnumerator DeviceCoroutineTx() private IEnumerator DeviceCoroutineRx() { - var waitUntil = new WaitUntil(() => deviceMessageQueue.Count > 0); + var waitUntil = new WaitUntil(() => (deviceMessageQueue.Count > 0)); while (runningDevice) { yield return waitUntil; @@ -244,10 +245,10 @@ public bool PopDeviceMessage(out T instance) instance = (result) ? data.GetMessage() : default(T); return result; } - catch + catch (Exception ex) { instance = default(T); - Debug.LogWarning("PopDeviceMessage(): ERROR"); + Debug.LogWarning("ERROR: PopDeviceMessage(): " + ex.Message); } return false; diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceMessageQueue.cs b/Assets/Scripts/Devices/Modules/Base/DeviceMessageQueue.cs index 2b612ffa..dc409ef4 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceMessageQueue.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceMessageQueue.cs @@ -5,11 +5,12 @@ */ using System.Collections.Concurrent; +using System; public class DeviceMessageQueue : BlockingCollection { - private const int MaxQueue = 5; - private const int TimeoutInMilliseconds = 100; + private const int MaxQueue = 15; + private const int TimeoutInMilliseconds = 200; public DeviceMessageQueue() : base(MaxQueue) @@ -36,7 +37,7 @@ public bool Push(in DeviceMessage data) { if (Count >= MaxQueue) { - // Debug.LogWarningFormat("Outbound queue is reached to maximum capacity({0})!!", maxQueue); + // UnityEngine.Debug.LogWarningFormat("Outbound queue is reached to maximum capacity({0})!!", MaxQueue); FlushHalf(); } @@ -57,8 +58,9 @@ public bool Pop(out DeviceMessage item) return true; } } - catch + catch (Exception ex) { + UnityEngine.Debug.LogWarning(ex.Message); item = default(DeviceMessage); } return false; From ad0ab72f7a6db3a5bb515943df0594cb7ac582a6 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 16:40:26 +0900 Subject: [PATCH 07/17] Add new cloisim message - joint cmd V Regenerate protobuf message --- .../Messages/.gen_proto_code.sh | 4 ++- .../Scripts/CLOiSimPlugins/Messages/color.cs | 4 +-- .../CLOiSimPlugins/Messages/joint_cmd_v.cs | 30 +++++++++++++++++++ .../Messages/joint_cmd_v.cs.meta | 11 +++++++ Assets/Scripts/CLOiSimPlugins/Messages/pid.cs | 28 ++++++++--------- 5 files changed, 60 insertions(+), 17 deletions(-) create mode 100644 Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs create mode 100644 Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs.meta diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh b/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh index 58747cbd..01857770 100755 --- a/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh +++ b/Assets/Scripts/CLOiSimPlugins/Messages/.gen_proto_code.sh @@ -36,7 +36,9 @@ MSG+="sensor_noise " MSG+="world_stats log_playback_stats " MSG+="request response " MSG+="perception perception_v " -MSG+="pid joint_cmd wrench joint_wrench joint_state joint_state_v " +MSG+="pid wrench joint_wrench " +MSG+="joint_state joint_state_v " +MSG+="joint_cmd joint_cmd_v " MSG+="transform_stamped " # MSG+=" " diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/color.cs b/Assets/Scripts/CLOiSimPlugins/Messages/color.cs index b6a5fdd8..f97b6f54 100644 --- a/Assets/Scripts/CLOiSimPlugins/Messages/color.cs +++ b/Assets/Scripts/CLOiSimPlugins/Messages/color.cs @@ -26,10 +26,10 @@ public partial class Color : global::ProtoBuf.IExtensible public float B { get; set; } [global::ProtoBuf.ProtoMember(5, Name = @"a")] - [global::System.ComponentModel.DefaultValue(1)] + [global::System.ComponentModel.DefaultValue(1f)] public float A { - get => __pbn__A ?? 1; + get => __pbn__A ?? 1f; set => __pbn__A = value; } public bool ShouldSerializeA() => __pbn__A != null; diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs b/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs new file mode 100644 index 00000000..378aaae6 --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs @@ -0,0 +1,30 @@ +// +// This file was generated by a tool; you should avoid making direct changes. +// Consider using 'partial classes' to extend these types +// Input: joint_cmd_v.proto +// + +#region Designer generated code +#pragma warning disable CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +namespace cloisim.msgs +{ + + [global::ProtoBuf.ProtoContract(Name = @"JointCmd_V")] + public partial class JointCmdV : global::ProtoBuf.IExtensible + { + private global::ProtoBuf.IExtension __pbn__extensionData; + global::ProtoBuf.IExtension global::ProtoBuf.IExtensible.GetExtensionObject(bool createIfMissing) + => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); + + [global::ProtoBuf.ProtoMember(1, Name = @"time", IsRequired = true)] + public Time Time { get; set; } + + [global::ProtoBuf.ProtoMember(2, Name = @"JointCmd")] + public global::System.Collections.Generic.List JointCmds { get; } = new global::System.Collections.Generic.List(); + + } + +} + +#pragma warning restore CS0612, CS0618, CS1591, CS3021, IDE0079, IDE1006, RCS1036, RCS1057, RCS1085, RCS1192 +#endregion diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs.meta b/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs.meta new file mode 100644 index 00000000..79a80d1a --- /dev/null +++ b/Assets/Scripts/CLOiSimPlugins/Messages/joint_cmd_v.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a44adb8d2e7d79db39cc6ae7c4524b26 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/CLOiSimPlugins/Messages/pid.cs b/Assets/Scripts/CLOiSimPlugins/Messages/pid.cs index 35ede466..24c902c5 100644 --- a/Assets/Scripts/CLOiSimPlugins/Messages/pid.cs +++ b/Assets/Scripts/CLOiSimPlugins/Messages/pid.cs @@ -17,10 +17,10 @@ public partial class Pid : global::ProtoBuf.IExtensible => global::ProtoBuf.Extensible.GetExtensionObject(ref __pbn__extensionData, createIfMissing); [global::ProtoBuf.ProtoMember(1, Name = @"target")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double Target { - get => __pbn__Target ?? 0; + get => __pbn__Target ?? 0d; set => __pbn__Target = value; } public bool ShouldSerializeTarget() => __pbn__Target != null; @@ -28,10 +28,10 @@ public double Target private double? __pbn__Target; [global::ProtoBuf.ProtoMember(2, Name = @"p_gain")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double PGain { - get => __pbn__PGain ?? 0; + get => __pbn__PGain ?? 0d; set => __pbn__PGain = value; } public bool ShouldSerializePGain() => __pbn__PGain != null; @@ -39,10 +39,10 @@ public double PGain private double? __pbn__PGain; [global::ProtoBuf.ProtoMember(3, Name = @"i_gain")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double IGain { - get => __pbn__IGain ?? 0; + get => __pbn__IGain ?? 0d; set => __pbn__IGain = value; } public bool ShouldSerializeIGain() => __pbn__IGain != null; @@ -50,10 +50,10 @@ public double IGain private double? __pbn__IGain; [global::ProtoBuf.ProtoMember(4, Name = @"d_gain")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double DGain { - get => __pbn__DGain ?? 0; + get => __pbn__DGain ?? 0d; set => __pbn__DGain = value; } public bool ShouldSerializeDGain() => __pbn__DGain != null; @@ -61,10 +61,10 @@ public double DGain private double? __pbn__DGain; [global::ProtoBuf.ProtoMember(5, Name = @"i_max")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double IMax { - get => __pbn__IMax ?? 0; + get => __pbn__IMax ?? 0d; set => __pbn__IMax = value; } public bool ShouldSerializeIMax() => __pbn__IMax != null; @@ -72,10 +72,10 @@ public double IMax private double? __pbn__IMax; [global::ProtoBuf.ProtoMember(6, Name = @"i_min")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double IMin { - get => __pbn__IMin ?? 0; + get => __pbn__IMin ?? 0d; set => __pbn__IMin = value; } public bool ShouldSerializeIMin() => __pbn__IMin != null; @@ -83,10 +83,10 @@ public double IMin private double? __pbn__IMin; [global::ProtoBuf.ProtoMember(7, Name = @"limit")] - [global::System.ComponentModel.DefaultValue(0)] + [global::System.ComponentModel.DefaultValue(0d)] public double Limit { - get => __pbn__Limit ?? 0; + get => __pbn__Limit ?? 0d; set => __pbn__Limit = value; } public bool ShouldSerializeLimit() => __pbn__Limit != null; From e4d1b6ea9791946751a2980f91107233846b14b6 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 17:24:24 +0900 Subject: [PATCH 08/17] Apply JointCommand_V message in JointControlPlugin --- .../CLOiSimPlugins/JointControlPlugin.cs | 2 +- Assets/Scripts/Devices/JointCommand.cs | 64 +++++++++++-------- 2 files changed, 40 insertions(+), 26 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs index b00ee09c..4f44b25a 100644 --- a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs @@ -51,7 +51,7 @@ protected override void OnStart() LoadJoints(); robotDescription_ = "" + GetPluginParameters().ParentRawXml() + ""; - UnityEngine.Debug.Log(robotDescription_); + // UnityEngine.Debug.Log(robotDescription_); } protected override void OnReset() diff --git a/Assets/Scripts/Devices/JointCommand.cs b/Assets/Scripts/Devices/JointCommand.cs index ce097cb7..f8a6dba1 100644 --- a/Assets/Scripts/Devices/JointCommand.cs +++ b/Assets/Scripts/Devices/JointCommand.cs @@ -5,6 +5,7 @@ */ using System.Collections.Generic; +using System.Text; using UnityEngine; using messages = cloisim.msgs; @@ -18,24 +19,25 @@ struct Command public float targetPosition; public float targetVelocity; - public Command(in Articulation joint, in float targetPosition, in float targetVelocity) + public Command(in Articulation joint, in float targetPosition_, in float targetVelocity_) { this.joint = joint; this.targetPosition = float.NaN; this.targetVelocity = float.NaN; - Set(targetPosition, targetVelocity); + Set(targetPosition_, targetVelocity_); } - public void Set(in float targetPosition, in float targetVelocity) + public void Set(in float targetPosition_, in float targetVelocity_) { - if (targetPosition != float.NaN) - this.targetPosition = targetPosition * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1); + if (targetPosition_ != float.NaN) + this.targetPosition = (this.joint.IsRevoluteType() ? SDF2Unity.CurveOrientation(targetPosition_) : targetPosition_); - if (targetVelocity != float.NaN) - this.targetVelocity = targetVelocity * (this.joint.IsRevoluteType() ? Mathf.Rad2Deg : 1); + if (targetVelocity_ != float.NaN) + this.targetVelocity = (this.joint.IsRevoluteType() ? SDF2Unity.CurveOrientation(targetVelocity_) : targetVelocity_); } } + private StringBuilder commandLog = new StringBuilder(); private JointState jointState = null; private Queue jointCommandQueue = new Queue(); @@ -55,30 +57,42 @@ protected override void OnReset() protected override void ProcessDevice() { - if (PopDeviceMessage(out var jointCommand)) + if (PopDeviceMessage(out var jointCommandV)) { - var jointName = jointCommand.Name; - Debug.Log(jointName); - var articulation = jointState.GetArticulation(jointName); - if (articulation != null) + if (jointCommandV == null) { - var targetPosition = float.NaN; - if (jointCommand.Position != null) - { - targetPosition = (float)jointCommand.Position.Target; - Debug.Log("targetPosition=" + targetPosition); - } + Debug.LogWarning("JointCommand: Pop Message failed."); + return; + } - var targetVelocity = float.NaN; - if (jointCommand.Velocity != null) + commandLog.Clear(); + + foreach (var jointCommand in jointCommandV.JointCmds) + { + var jointName = jointCommand.Name; + var articulation = jointState.GetArticulation(jointName); + if (articulation != null) { - targetVelocity = (float)jointCommand.Velocity.Target; - Debug.Log("targetVelocity=" + targetVelocity); - } + var targetPosition = float.NaN; + if (jointCommand.Position != null) + { + targetPosition = (float)jointCommand.Position.Target; + commandLog.AppendLine(jointName + ": targetPosition=" + targetPosition); + } + + var targetVelocity = float.NaN; + if (jointCommand.Velocity != null) + { + targetVelocity = (float)jointCommand.Velocity.Target; + commandLog.AppendLine(jointName + ": targetVelocity=" + targetVelocity); + } - var newCommand = new Command(articulation, targetPosition, targetVelocity); - jointCommandQueue.Enqueue(newCommand); + var newCommand = new Command(articulation, targetPosition, targetVelocity); + jointCommandQueue.Enqueue(newCommand); + } } + + Debug.Log(commandLog.ToString()); } } From 4e68c3c1179c61919e46567bfed74d89bc7cd671 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 17:25:00 +0900 Subject: [PATCH 09/17] Modify inertia tensor for non-collider link model when importing Link element --- .../Scripts/Tools/SDF/Import/Import.Link.cs | 24 +++++++++---------- 1 file changed, 11 insertions(+), 13 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index 89c94bb2..0fb26224 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs @@ -142,26 +142,24 @@ private static UE.ArticulationBody CreateArticulationBody(in UE.GameObject linkO // Debug.Log(linkObject.name + " => Center Of Mass: " + articulationBody.centerOfMass.ToString("F6") + ", intertia: " + articulationBody.inertiaTensor.ToString("F6") + ", " + articulationBody.inertiaTensorRotation.ToString("F6")); } - if (colliders.Length > 0) + if (colliders.Length == 0) { - if (inertial?.inertia != null) - { - var momentum = GetInertiaTensor(inertial?.inertia); - articulationBody.inertiaTensor = momentum.position; - articulationBody.inertiaTensorRotation = momentum.rotation; - } - else - { - articulationBody.ResetInertiaTensor(); - articulationBody.automaticInertiaTensor = true; - } + Debug.LogWarningFormat(articulationBody.name + " => no mesh collider exists in child"); + } + + if (inertial?.inertia != null) + { + var momentum = GetInertiaTensor(inertial?.inertia); + articulationBody.inertiaTensor = momentum.position; + articulationBody.inertiaTensorRotation = momentum.rotation; } else { articulationBody.inertiaTensor = UE.Vector3.one * MinimumInertiaTensor; articulationBody.inertiaTensorRotation = UE.Quaternion.identity; - Debug.LogWarningFormat(articulationBody.name + " => no mesh collider exists in child"); + articulationBody.automaticInertiaTensor = true; + articulationBody.ResetInertiaTensor(); } // Debug.Log("Create link body " + linkObject.name); From 18a8bfee976d91e1f754129b61d1895d414dd8ee Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Fri, 27 Oct 2023 17:48:16 +0900 Subject: [PATCH 10/17] Fix sign for curve orientation in JointState --- Assets/Scripts/Devices/JointState.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Devices/JointState.cs b/Assets/Scripts/Devices/JointState.cs index c6113933..e1b4abc1 100644 --- a/Assets/Scripts/Devices/JointState.cs +++ b/Assets/Scripts/Devices/JointState.cs @@ -106,8 +106,10 @@ void FixedUpdate() var jointState = item.Item2; jointState.Effort = articulation.GetEffort(); - jointState.Position = articulation.GetJointPosition(); - jointState.Velocity = articulation.GetJointVelocity(); + jointState.Position = (articulation.IsRevoluteType() ? + DeviceHelper.Convert.CurveOrientation(articulation.GetJointPosition()) : articulation.GetJointPosition()); + jointState.Velocity = (articulation.IsRevoluteType() ? + DeviceHelper.Convert.CurveOrientation(articulation.GetJointVelocity()) : articulation.GetJointVelocity()); } } } From ce7e3f43234217a518192ee8b479bdc110fb0e27 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 15:03:52 +0900 Subject: [PATCH 11/17] Fix SDF Implement Joint - remove unused code --- Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index 8f4097d7..ed7662b5 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -224,8 +224,8 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, if (axis.limit.Use()) { // Debug.LogWarningFormat("limit uppper{0}, lower{1}", axis.limit.upper, axis.limit.lower); - drive.lowerLimit = (float)(axis.limit.lower); - drive.upperLimit = (float)(axis.limit.upper); + drive.lowerLimit = (float)axis.limit.lower; + drive.upperLimit = (float)axis.limit.upper; } drive.forceLimit = (axis.limit.effort > -1d) ? (float)axis.limit.effort : float.MaxValue; @@ -236,12 +236,6 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis, drive.target = (float)axis.dynamics.spring_reference; drive.damping = (float)axis.dynamics.damping; - // Check if spring - if (axis.limit.Use()) - { - drive.targetVelocity = 1; - } - body.jointFriction = (float)axis.dynamics.friction; } else From b88a62b865bcdc35f78be51cb6c37ad23478fe2d Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 16:16:32 +0900 Subject: [PATCH 12/17] Modify GetJointPosition for Articulation module --- Assets/Scripts/Devices/JointCommand.cs | 18 +++++++--- Assets/Scripts/Devices/JointState.cs | 9 ++--- .../Scripts/Devices/Modules/Articulation.cs | 36 ++++++++++++++++--- Assets/Scripts/Devices/Modules/Motor.cs | 2 +- Assets/Scripts/Main.cs | 1 + 5 files changed, 50 insertions(+), 16 deletions(-) diff --git a/Assets/Scripts/Devices/JointCommand.cs b/Assets/Scripts/Devices/JointCommand.cs index f8a6dba1..e3e77113 100644 --- a/Assets/Scripts/Devices/JointCommand.cs +++ b/Assets/Scripts/Devices/JointCommand.cs @@ -3,6 +3,7 @@ * * SPDX-License-Identifier: MIT */ +// #define PRINT_COMMAND_LOG using System.Collections.Generic; using System.Text; @@ -37,7 +38,10 @@ public void Set(in float targetPosition_, in float targetVelocity_) } } +#if PRINT_COMMAND_LOG private StringBuilder commandLog = new StringBuilder(); +#endif + private JointState jointState = null; private Queue jointCommandQueue = new Queue(); @@ -64,9 +68,9 @@ protected override void ProcessDevice() Debug.LogWarning("JointCommand: Pop Message failed."); return; } - +#if PRINT_COMMAND_LOG commandLog.Clear(); - +#endif foreach (var jointCommand in jointCommandV.JointCmds) { var jointName = jointCommand.Name; @@ -77,22 +81,28 @@ protected override void ProcessDevice() if (jointCommand.Position != null) { targetPosition = (float)jointCommand.Position.Target; +#if PRINT_COMMAND_LOG commandLog.AppendLine(jointName + ": targetPosition=" + targetPosition); +#endif } var targetVelocity = float.NaN; if (jointCommand.Velocity != null) { targetVelocity = (float)jointCommand.Velocity.Target; +#if PRINT_COMMAND_LOG commandLog.AppendLine(jointName + ": targetVelocity=" + targetVelocity); +#endif } var newCommand = new Command(articulation, targetPosition, targetVelocity); jointCommandQueue.Enqueue(newCommand); } } - - Debug.Log(commandLog.ToString()); +#if PRINT_COMMAND_LOG + if (commandLog.Length > 0) + Debug.Log(commandLog.ToString()); +#endif } } diff --git a/Assets/Scripts/Devices/JointState.cs b/Assets/Scripts/Devices/JointState.cs index e1b4abc1..b30ab58a 100644 --- a/Assets/Scripts/Devices/JointState.cs +++ b/Assets/Scripts/Devices/JointState.cs @@ -62,7 +62,6 @@ public bool AddTargetJoint(in string targetJointName, out SDF.Helper.Link link, var parentObject = childArticulationBody.transform.parent; var parentModelName = parentObject.name; - // var linkName = ((!parentObject.CompareTag("Model") || rootModelName.CompareTo(parentModelName) == 0) ? "" : parentModelName + "::") + childArticulationBody.name; var linkHelper = childArticulationBody.GetComponentInChildren(); // Debug.Log("linkHelper.JointName " + linkHelper.JointName); if (linkHelper.JointName.Equals(targetJointName)) @@ -84,8 +83,6 @@ public bool AddTargetJoint(in string targetJointName, out SDF.Helper.Link link, articulationTable.Add(targetJointName, new Tuple(articulation, jointState)); jointStateV.JointStates.Add(jointState); - - // link = articulation.gameObject.GetComponentInChildren(); return true; } } @@ -106,10 +103,8 @@ void FixedUpdate() var jointState = item.Item2; jointState.Effort = articulation.GetEffort(); - jointState.Position = (articulation.IsRevoluteType() ? - DeviceHelper.Convert.CurveOrientation(articulation.GetJointPosition()) : articulation.GetJointPosition()); - jointState.Velocity = (articulation.IsRevoluteType() ? - DeviceHelper.Convert.CurveOrientation(articulation.GetJointVelocity()) : articulation.GetJointVelocity()); + jointState.Position = articulation.GetJointPosition(); + jointState.Velocity = articulation.GetJointVelocity(); } } } diff --git a/Assets/Scripts/Devices/Modules/Articulation.cs b/Assets/Scripts/Devices/Modules/Articulation.cs index acb5d01d..48acef08 100644 --- a/Assets/Scripts/Devices/Modules/Articulation.cs +++ b/Assets/Scripts/Devices/Modules/Articulation.cs @@ -18,7 +18,7 @@ public Articulation(in ArticulationBody jointBody) if (jointBody != null) { _jointBody = jointBody; - _jointType = _jointBody.jointType; + _jointType = jointBody.jointType; } } @@ -62,8 +62,35 @@ private int GetValidIndex(in int index) /// in radian for angular and in meters for linear public float GetJointPosition(int index = 0) { - index = GetValidIndex(index); - return (_jointBody == null || index == -1) ? 0 : _jointBody.jointPosition[index]; + if (IsRevoluteType()) + { + index = GetValidIndex(index); + return (_jointBody == null || index == -1) ? 0 : + DeviceHelper.Convert.CurveOrientation(_jointBody.jointPosition[index]); + } + else + { + if (Type == ArticulationJointType.PrismaticJoint) + { + if (_jointBody.linearLockX == ArticulationDofLock.LockedMotion && + _jointBody.linearLockY == ArticulationDofLock.LockedMotion) + { + return _jointBody.transform.localPosition.z; + } + else if (_jointBody.linearLockY == ArticulationDofLock.LockedMotion && + _jointBody.linearLockZ == ArticulationDofLock.LockedMotion) + { + return _jointBody.transform.localPosition.x; + } + else if (_jointBody.linearLockX == ArticulationDofLock.LockedMotion && + _jointBody.linearLockZ == ArticulationDofLock.LockedMotion) + { + return _jointBody.transform.localPosition.y; + } + } + } + + return 0; } /// torque for angular and force for linear @@ -78,7 +105,8 @@ public float GetJointForce(int index = 0) public float GetJointVelocity(int index = 0) { index = GetValidIndex(index); - return (_jointBody == null || index == -1) ? 0 : _jointBody.jointVelocity[index]; + var value = (_jointBody == null || index == -1) ? 0 : _jointBody.jointVelocity[index]; + return (IsRevoluteType() ? DeviceHelper.Convert.CurveOrientation(value) : value); } /// torque for angular and force for linear diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index 3b997bdf..9e100591 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -206,8 +206,8 @@ public void Update(in float duration) public void Stop() { - SetJointVelocity(0); Drive(0); + SetJointVelocity(0); _pidControl.Reset(); _rapidControl.SetDirectionSwitched(false); diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 30e8b4a3..f11fee63 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -398,6 +398,7 @@ void LateUpdate() if (Input.GetKey(KeyCode.LeftControl) && Input.GetKeyUp(KeyCode.R)) { resetTriggered = true; + // Debug.Log("Reset Triggered"); } if (resetTriggered && !isResetting) From 3d85c5aeea4b08f906e80fc5804b16d4e43d2046 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 16:48:03 +0900 Subject: [PATCH 13/17] Change range of scaling factor for object spawning --- Assets/Scripts/Core/ObjectSpawning.cs | 6 ++---- Assets/Scripts/UI/SimulationDisplay.Props.cs | 8 ++++---- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/Assets/Scripts/Core/ObjectSpawning.cs b/Assets/Scripts/Core/ObjectSpawning.cs index c84dc2f6..aa2d97e7 100644 --- a/Assets/Scripts/Core/ObjectSpawning.cs +++ b/Assets/Scripts/Core/ObjectSpawning.cs @@ -122,7 +122,7 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3 { var newTempPropsObject = CreateProps(propsName, mesh, scale); props.Add(type, newTempPropsObject); - newTempPropsObject.hideFlags = HideFlags.HideAndDontSave; + newTempPropsObject.hideFlags = HideFlags.DontSaveInEditor | HideFlags.DontSave; newTempPropsObject.SetActive(false); } } @@ -136,7 +136,7 @@ private IEnumerator SpawnTargetObject(PropsType type, Vector3 position, Vector3 mesh = meshFilter.sharedMesh; var meshRender = spawnedObject.GetComponentInChildren(); - meshRender.material.color = Random.ColorHSV(0f, 1f, 0.7f, 1f, 0.6f, 1f); + meshRender.material.color = Random.ColorHSV(0f, 1f, 0.6f, 1f, 0.5f, 1f); var rigidBody = spawnedObject.GetComponentInChildren(); rigidBody.mass = CalculateMass(scale); @@ -177,8 +177,6 @@ private GameObject CreateProps(in string name, in Mesh targetMesh, in Vector3 sc newMaterial.name = targetMesh.name; newMaterial.color = Color.white; - // Debug.Log(Random.ColorHSV(0f, 1f, 0.7f, 1f, 0.6f, 1f)); - var meshRenderer = newObject.AddComponent(); meshRenderer.material = newMaterial; meshRenderer.shadowCastingMode = ShadowCastingMode.Off; diff --git a/Assets/Scripts/UI/SimulationDisplay.Props.cs b/Assets/Scripts/UI/SimulationDisplay.Props.cs index 8768baa4..ba7fb1a5 100644 --- a/Assets/Scripts/UI/SimulationDisplay.Props.cs +++ b/Assets/Scripts/UI/SimulationDisplay.Props.cs @@ -85,13 +85,13 @@ private void DrawPropsMenus() { if (float.TryParse(scaleFactorString, out var scaleFactor)) { - if (scaleFactor < 0.1f) + if (scaleFactor < 0.01f) { - scaleFactorString = "0.1"; + scaleFactorString = "0.01"; } - else if (scaleFactor > 5f) + else if (scaleFactor > 10f) { - scaleFactorString = "5"; + scaleFactorString = "10"; } } else From 77e32388086ad4dafea6248c8f5b4261a309d06a Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 18:40:17 +0900 Subject: [PATCH 14/17] Add protetion code in Jointcommand --- Assets/Scripts/Devices/JointCommand.cs | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Assets/Scripts/Devices/JointCommand.cs b/Assets/Scripts/Devices/JointCommand.cs index e3e77113..761988a3 100644 --- a/Assets/Scripts/Devices/JointCommand.cs +++ b/Assets/Scripts/Devices/JointCommand.cs @@ -116,8 +116,10 @@ void FixedUpdate() while (jointCommandQueue.Count > 0) { var command = jointCommandQueue.Dequeue(); - // Debug.Log(command.targetVelocity + "," + command.targetPosition); - command.joint.Drive(command.targetVelocity, command.targetPosition); + if (command.joint != null) + command.joint.Drive(command.targetVelocity, command.targetPosition); + else + Debug.LogWarning("Command joint is null. " + command.targetVelocity + ", " + command.targetPosition); } } } From a1876b70c03bc3af487ed0e93a4a66e6eab0b2ec Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 18:40:38 +0900 Subject: [PATCH 15/17] Set Seek to begin for memory steam before deserializing a protobuf message --- Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs | 1 + 1 file changed, 1 insertion(+) diff --git a/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs b/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs index f7b331d4..20880bba 100644 --- a/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs +++ b/Assets/Scripts/Devices/Modules/Base/DeviceMessage.cs @@ -65,6 +65,7 @@ public T GetMessage() lock (this) { + Seek(0, SeekOrigin.Begin); result = Serializer.Deserialize(this); } From fbff6995b02a4673c8c38ac278dea4717ab717ea Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 18:59:26 +0900 Subject: [PATCH 16/17] Fix Fix joint position value for prismatic joint type in Articulation module --- Assets/Scripts/Devices/Modules/Articulation.cs | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Articulation.cs b/Assets/Scripts/Devices/Modules/Articulation.cs index 48acef08..852778d8 100644 --- a/Assets/Scripts/Devices/Modules/Articulation.cs +++ b/Assets/Scripts/Devices/Modules/Articulation.cs @@ -72,20 +72,25 @@ public float GetJointPosition(int index = 0) { if (Type == ArticulationJointType.PrismaticJoint) { + var anchorRotation = _jointBody.anchorRotation.eulerAngles; + var direction = (Mathf.Approximately(anchorRotation.x, 180) || + Mathf.Approximately(anchorRotation.y, 180) || + Mathf.Approximately(anchorRotation.z, 180)) ? -1f : 1f; + if (_jointBody.linearLockX == ArticulationDofLock.LockedMotion && _jointBody.linearLockY == ArticulationDofLock.LockedMotion) { - return _jointBody.transform.localPosition.z; + return direction * _jointBody.transform.localPosition.z; } else if (_jointBody.linearLockY == ArticulationDofLock.LockedMotion && _jointBody.linearLockZ == ArticulationDofLock.LockedMotion) { - return _jointBody.transform.localPosition.x; + return direction * _jointBody.transform.localPosition.x; } else if (_jointBody.linearLockX == ArticulationDofLock.LockedMotion && _jointBody.linearLockZ == ArticulationDofLock.LockedMotion) { - return _jointBody.transform.localPosition.y; + return direction * _jointBody.transform.localPosition.y; } } } From 423c6f88a6e4963203f1c63c1bba83d05ca34ca2 Mon Sep 17 00:00:00 2001 From: "(=YG=) Hyunseok Yang" Date: Mon, 6 Nov 2023 19:01:32 +0900 Subject: [PATCH 17/17] Update app version info in Project settings : 4.1.1 -> 4.2.0 --- ProjectSettings/ProjectSettings.asset | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index ae60e873..5a4e328b 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -48,6 +48,7 @@ PlayerSettings: defaultScreenHeightWeb: 600 m_StereoRenderingPath: 0 m_ActiveColorSpace: 1 + unsupportedMSAAFallback: 0 m_SpriteBatchVertexThreshold: 300 m_MTRendering: 1 mipStripping: 1 @@ -135,7 +136,7 @@ PlayerSettings: vulkanEnableLateAcquireNextImage: 0 vulkanEnableCommandBufferRecycling: 1 loadStoreDebugModeEnabled: 0 - bundleVersion: 4.1.1 + bundleVersion: 4.2.0 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0