From 18a6b9edada380fa49888cfea0f2a580043bdc2d Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 18 Nov 2024 15:58:33 +0900 Subject: [PATCH 01/19] Move DifferentialDrive.IsZero() into MathUtil.IsZero() Modify epsilon value in IsZero() --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 1 + .../DifferentialDrive.cs | 10 +++++----- .../Motor/DifferentialDriveControl/Odometry.cs | 18 ++++-------------- Assets/Scripts/Tools/MathUtil.cs | 5 +++++ 4 files changed, 15 insertions(+), 19 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index 66fcaab5..b2c33d79 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -249,6 +249,7 @@ private void SetDriveForWheel(in string parameterPrefix) var wheelTread = GetPluginParameters().GetValue($"{parameterPrefix}/tread"); // TODO: to be deprecated var wheelSeparation = GetPluginParameters().GetValue($"{parameterPrefix}/separation", wheelTread); + _log.AppendLine($"wheel separation/radius: {wheelSeparation}/{wheelRadius}"); _motorControl.SetWheelInfo(wheelRadius, wheelSeparation); var wheelLeftName = GetPluginParameters().GetValue($"{parameterPrefix}/location[@type='left']", string.Empty); diff --git a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/DifferentialDrive.cs b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/DifferentialDrive.cs index eefd7247..afed728b 100644 --- a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/DifferentialDrive.cs +++ b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/DifferentialDrive.cs @@ -35,16 +35,16 @@ public override void SetWheelInfo(in float radius, in float separation) public override void Drive(in float linearVelocity, in float angularVelocity) { // m/s, rad/s - // var linearVelocityLeft = ((2 * linearVelocity) + (angularVelocity * WheelSeparation)) / (2 * wheelRadius); + // var linearVelocityLeft = ((2 * linearVelocity) - (angularVelocity * WheelSeparation)) / (2 * wheelRadius); // var linearVelocityRight = ((2 * linearVelocity) + (angularVelocity * WheelSeparation)) / (2 * wheelRadius); var angularCalculation = (angularVelocity * _odometry.WheelSeparation * 0.5f); // Velocity(rad per second) for wheels - var linearVelocityLeft = linearVelocity - angularCalculation; - var linearVelocityRight = linearVelocity + angularCalculation; + var linearVelocityLeft = (linearVelocity - angularCalculation) * _odometry.InverseWheelRadius; + var linearVelocityRight = (linearVelocity + angularCalculation) * _odometry.InverseWheelRadius; - var angularVelocityLeft = SDF2Unity.CurveOrientation(linearVelocityLeft * _odometry.InverseWheelRadius); - var angularVelocityRight = SDF2Unity.CurveOrientation(linearVelocityRight * _odometry.InverseWheelRadius); + var angularVelocityLeft = SDF2Unity.CurveOrientation(linearVelocityLeft); + var angularVelocityRight = SDF2Unity.CurveOrientation(linearVelocityRight); SetMotorVelocity(angularVelocityLeft, angularVelocityRight); } diff --git a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs index 78ce2315..0e9d7ea5 100644 --- a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs +++ b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs @@ -45,16 +45,6 @@ public void Reset() #endif } - private bool IsZero(in float value) - { - return Mathf.Abs(value) < Quaternion.kEpsilon; - } - - private bool IsZero(in double value) - { - return Math.Abs(value) < Quaternion.kEpsilon; - } - /// Calculate odometry on this robot /// rad per second for `angularVelocity` private void CalculateOdometry( @@ -68,14 +58,14 @@ private void CalculateOdometry( var sumLeftRight = linearVelocityLeftWheel + linearVelocityRightWheel; var diffRightLeft = linearVelocityRightWheel - linearVelocityLeftWheel; - _odomTranslationalVelocity = IsZero(sumLeftRight) ? 0 : (sumLeftRight * 0.5f); - _odomRotationalVelocity = IsZero(diffRightLeft) ? 0 : (diffRightLeft * _wheelInfo.inversedWheelSeparation); + _odomTranslationalVelocity = MathUtil.IsZero(sumLeftRight) ? 0 : (sumLeftRight * 0.5f); + _odomRotationalVelocity = MathUtil.IsZero(diffRightLeft) ? 0 : (diffRightLeft * _wheelInfo.inversedWheelSeparation); var linear = _odomTranslationalVelocity * duration; var angular = (float.IsNaN(deltaTheta)) ? (_odomRotationalVelocity * duration) : deltaTheta; // Acculumate odometry: - if (IsZero(angular)) // RungeKutta2 + if (MathUtil.IsZero(angular)) // RungeKutta2 { var direction = _odomPose.y + angular * 0.5f; @@ -130,7 +120,7 @@ public bool Update( var deltaAngleImu = Mathf.DeltaAngle(_lastImuYaw, imuYaw); _lastImuYaw = imuYaw; - var deltaThetaIMU = IsZero(deltaAngleImu) ? 0 : deltaAngleImu * Mathf.Deg2Rad; + var deltaThetaIMU = MathUtil.IsZero(deltaAngleImu) ? 0 : deltaAngleImu * Mathf.Deg2Rad; // Debug.Log("IMUE deltatheta = " + deltaThetaIMU); CalculateOdometry(angularVelocityLeft, angularVelocityRight, duration, deltaThetaIMU); } diff --git a/Assets/Scripts/Tools/MathUtil.cs b/Assets/Scripts/Tools/MathUtil.cs index 72d222fd..4885408f 100644 --- a/Assets/Scripts/Tools/MathUtil.cs +++ b/Assets/Scripts/Tools/MathUtil.cs @@ -58,4 +58,9 @@ public static void NormalizeAngle(this ref UnityEngine.Vector3 vector) vector.y = (float)Angle.Normalize(vector.y); vector.z = (float)Angle.Normalize(vector.z); } + + public static bool IsZero(in double value) + { + return Math.Abs(value) < float.Epsilon; + } } \ No newline at end of file From bc3c661574c97b460c9bfb34cd51fa2d29275629 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Mon, 18 Nov 2024 16:23:14 +0900 Subject: [PATCH 02/19] Bug fix inertia tensor handling - remove UpdateInertiaTensorRotation() --- Assets/Scripts/Tools/SDF/Import/Import.Base.cs | 3 --- Assets/Scripts/Tools/SDF/Import/Import.Util.cs | 17 ----------------- 2 files changed, 20 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs index e9bbbe2c..fe21f2f6 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs @@ -133,8 +133,6 @@ public IEnumerator Start(World world) ImportPlugins(world.GetPlugins(), worldObject); worldObject.SpecifyPose(); - worldObject.UpdateInertiaTensorRotation(); - yield return null; ImportActors(world.GetActors()); @@ -156,7 +154,6 @@ public IEnumerator Start(Model model) } modelObject.SpecifyPose(); - modelObject.UpdateInertiaTensorRotation(); yield return null; } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs index b6df505c..220d1221 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs @@ -167,23 +167,6 @@ public static void SpecifyPose(this Object targetObject) body.enabled = true; } } - - public static void UpdateInertiaTensorRotation(this Object targetObject) - { - var rootObject = (targetObject as UE.GameObject); - foreach (var artBody in rootObject.GetComponentsInChildren()) - { - var linkHelper = artBody.GetComponent(); - - if (linkHelper != null && - linkHelper.Model != null && - linkHelper.Model.IsFirstChild == false && - artBody.automaticInertiaTensor == false) - { - artBody.inertiaTensorRotation = linkHelper.Model.transform.rotation; - } - } - } } } } \ No newline at end of file From 617343911f8125fb5c84a63a5d74688379f3da3d Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Wed, 20 Nov 2024 01:56:18 +0900 Subject: [PATCH 03/19] Fix calculation anglestep in case of 360 angle in LaserData Modify default HFOV unit angle for 3D-Lidar : 10 -> 90 --- Assets/Scripts/Devices/Lidar.cs | 22 ++++++++++----------- Assets/Scripts/Devices/Modules/LaserData.cs | 15 +++++++++----- 2 files changed, 21 insertions(+), 16 deletions(-) diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index 27684647..8a3d8464 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -31,7 +31,7 @@ public partial class Lidar : Device private const float DEG360 = DEG180 * 2; private const float HFOV_FOR_2D_LIDAR = 120f; - private const float HFOV_FOR_3D_LIDAR = 10f; + private const float HFOV_FOR_3D_LIDAR = 90f; private float LaserCameraHFov = 0f; private float LaserCameraHFovHalf = 0; private float LaserCameraVFov = 0; @@ -157,7 +157,7 @@ protected override void SetupMessages() private void SetupLaserCamera() { - LaserCameraVFov = (vertical.samples == 1) ? 1 : (float)vertical.angle.max - (float)vertical.angle.min; + LaserCameraVFov = (vertical.samples == 1) ? 1 : vertical.angle.range; LaserCameraHFov = (vertical.samples > 1) ? HFOV_FOR_3D_LIDAR : HFOV_FOR_2D_LIDAR; LaserCameraHFovHalf = LaserCameraHFov * 0.5f; @@ -172,8 +172,8 @@ private void SetupLaserCamera() laserCam.stereoTargetEye = StereoTargetEyeMask.None; laserCam.orthographic = false; - laserCam.nearClipPlane = (float)scanRange.min; - laserCam.farClipPlane = (float)scanRange.max; + laserCam.nearClipPlane = scanRange.min; + laserCam.farClipPlane = scanRange.max; laserCam.cullingMask = LayerMask.GetMask("Default") | LayerMask.GetMask("Plane"); laserCam.clearFlags = CameraClearFlags.Nothing; @@ -183,8 +183,7 @@ private void SetupLaserCamera() var renderTextrueWidth = Mathf.CeilToInt(LaserCameraHFov / laserAngleResolution.H); var renderTextrueHeight = Mathf.CeilToInt(LaserCameraVFov / laserAngleResolution.V); - // Debug.Log(maxVFov + "," + LaserCameraVFov + "," + renderTextrueWidth + "," + renderTextrueHeight); - // Debug.Log(LaserCameraVFov + "," + renderTextrueWidth + "," + renderTextrueHeight); + // Debug.Log("SetupLaserCamera: " + LaserCameraVFov + "," + laserAngleResolution.V + "," + renderTextrueWidth + "," + renderTextrueHeight); RTHandles.SetHardwareDynamicResolutionState(true); _rtHandle = RTHandles.Alloc( @@ -408,6 +407,8 @@ protected void OnCompleteAsyncReadback(AsyncGPUReadbackRequest request) private IEnumerator LaserProcsssing() { + const int BufferUnitSize = sizeof(double); + var laserScanStamped = new messages.LaserScanStamped(); laserScanStamped.Time = new messages.Time(); @@ -424,8 +425,7 @@ private IEnumerator LaserProcsssing() laserScan.WorldPose.Position.Set(lidarPosition); laserScan.WorldPose.Orientation.Set(lidarRotation); - - const int BufferUnitSize = sizeof(double); + var laserSamplesH = (int)horizontal.samples; var laserStartAngleH = (float)horizontal.angle.min; var laserEndAngleH = (float)horizontal.angle.max; @@ -433,9 +433,9 @@ private IEnumerator LaserProcsssing() var dividedLaserTotalAngleH = 1f / laserTotalAngleH; var laserSamplesV = (int)vertical.samples; - var laserStartAngleV = (float)vertical.angle.min; - var laserEndAngleV = (float)vertical.angle.max; - var laserTotalAngleV = (float)vertical.angle.range; + // var laserStartAngleV = (float)vertical.angle.min; + // var laserEndAngleV = (float)vertical.angle.max; + // var laserTotalAngleV = (float)vertical.angle.range; var capturedTime = 0f; var processingTimeSum = 0f; diff --git a/Assets/Scripts/Devices/Modules/LaserData.cs b/Assets/Scripts/Devices/Modules/LaserData.cs index b5191ed7..250c8813 100644 --- a/Assets/Scripts/Devices/Modules/LaserData.cs +++ b/Assets/Scripts/Devices/Modules/LaserData.cs @@ -7,6 +7,7 @@ using Unity.Collections; using Unity.Jobs; using Unity.Burst; +using System; namespace SensorDevices { @@ -25,16 +26,20 @@ public Scan(in uint samples, in double angleMinRad, in double angleMaxRad, in do this.resolution = resolution; this.angle = new MathUtil.MinMax(angleMinRad * Mathf.Rad2Deg, angleMaxRad * Mathf.Rad2Deg); - if (System.Math.Abs(this.angle.range) < double.Epsilon) + if (Math.Abs(this.angle.range) < double.Epsilon) { this.angleStep = 1; } else { - var residual = (System.Math.Abs(this.angle.range - 360d) < double.Epsilon) ? 0 : 1; - var rangeCount = resolution * samples - residual; - - this.angleStep = (rangeCount <= 0) ? 0 : ((angle.range) / rangeCount); + var rangeCount = resolution * samples; + this.angleStep = (rangeCount <= 0) ? 0 : (this.angle.range / rangeCount); + + var residual = (Math.Abs(360d - this.angle.range) < this.angleStep) ? 0 : 1; + if (residual > 0 && rangeCount > 0) + { + this.angleStep = this.angle.range / (rangeCount + 1); + } } } From 105ec455c842ab42b4f42d427fe5146a171f980c Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 20 Nov 2024 14:59:09 +0900 Subject: [PATCH 04/19] Fix typo ;; -> ; --- Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs | 2 +- Assets/Scripts/Devices/JointState.cs | 1 + .../Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs | 1 + Assets/Scripts/Devices/Modules/Motor/MotorControl.cs | 2 +- Assets/Scripts/UI/CameraControl.cs | 2 +- 5 files changed, 5 insertions(+), 3 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs index d15dee26..a331ce0b 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Connection/Requestor.cs @@ -88,7 +88,7 @@ public byte[] ReceiveResponse(in bool checkTag = false) if (!IsDisposed) { var frameReceived = this.ReceiveFrameBytes(); - return TransportHelper.RetrieveData(frameReceived, (checkTag)? hashValue : null);; + return TransportHelper.RetrieveData(frameReceived, (checkTag)? hashValue : null); } else { diff --git a/Assets/Scripts/Devices/JointState.cs b/Assets/Scripts/Devices/JointState.cs index 31b27d27..d342e03d 100644 --- a/Assets/Scripts/Devices/JointState.cs +++ b/Assets/Scripts/Devices/JointState.cs @@ -62,6 +62,7 @@ public bool AddTargetJoint(in string targetJointName, out SDF.Helper.Link link, var parentObject = childArticulationBody.transform.parent; var parentModelName = parentObject.name; + var linkHelper = childArticulationBody.GetComponentInChildren(); // Debug.Log("linkHelper.JointName " + linkHelper.JointName); if (linkHelper.JointName.Equals(targetJointName)) diff --git a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs index 0e9d7ea5..bcd337ff 100644 --- a/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs +++ b/Assets/Scripts/Devices/Modules/Motor/DifferentialDriveControl/Odometry.cs @@ -63,6 +63,7 @@ private void CalculateOdometry( var linear = _odomTranslationalVelocity * duration; var angular = (float.IsNaN(deltaTheta)) ? (_odomRotationalVelocity * duration) : deltaTheta; + // Debug.Log($"angular: {angular}, angular(calc): {(_odomRotationalVelocity * duration)} delta: {((_odomRotationalVelocity * duration)-angular).ToString("F5")}"); // Acculumate odometry: if (MathUtil.IsZero(angular)) // RungeKutta2 diff --git a/Assets/Scripts/Devices/Modules/Motor/MotorControl.cs b/Assets/Scripts/Devices/Modules/Motor/MotorControl.cs index 5ddab495..4ef2d99c 100644 --- a/Assets/Scripts/Devices/Modules/Motor/MotorControl.cs +++ b/Assets/Scripts/Devices/Modules/Motor/MotorControl.cs @@ -111,7 +111,7 @@ protected void SetMotorPID( var motor = _motorList[targetMotorLocation]; if (motor == null) { - Debug.LogWarning("There is no Wheel, AttachWheel() first"); + Debug.LogWarning($"There is no Wheel on {targetMotorLocation}, AttachWheel() first"); return; } diff --git a/Assets/Scripts/UI/CameraControl.cs b/Assets/Scripts/UI/CameraControl.cs index be166fca..29e6097b 100644 --- a/Assets/Scripts/UI/CameraControl.cs +++ b/Assets/Scripts/UI/CameraControl.cs @@ -78,7 +78,7 @@ void Awake() { _targetLayerMask = LayerMask.GetMask("Default"); _uiController = Main.UIObject?.GetComponent(); - // Debug.Log(_uiController);; + // Debug.Log(_uiController); } void LateUpdate() From aec13da85e715c14419e5493da139ff09376ef44 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 20 Nov 2024 15:00:25 +0900 Subject: [PATCH 05/19] Check a code to remove for velocity limitation in Joint --- .../Scripts/Devices/Modules/Articulation.cs | 11 +++++ Assets/Scripts/Tools/SDF/Helper/Link.cs | 5 +- .../Scripts/Tools/SDF/Helper/PoseControl.cs | 49 ++++++++++--------- .../Tools/SDF/Implement/Implement.Joint.cs | 18 ++++--- .../Scripts/Tools/SDF/Import/Import.Joint.cs | 20 ++++---- 5 files changed, 60 insertions(+), 43 deletions(-) diff --git a/Assets/Scripts/Devices/Modules/Articulation.cs b/Assets/Scripts/Devices/Modules/Articulation.cs index 1c8f6fb9..3004f902 100644 --- a/Assets/Scripts/Devices/Modules/Articulation.cs +++ b/Assets/Scripts/Devices/Modules/Articulation.cs @@ -13,7 +13,10 @@ public class Articulation protected ArticulationBody _jointBody = null; protected ArticulationJointType _jointType = ArticulationJointType.FixedJoint; private ArticulationDriveType _driveType = ArticulationDriveType.Force; + +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity protected float _velocityLimit = float.NaN; +#endif public ArticulationDriveType DriveType { @@ -51,10 +54,12 @@ public Articulation(in GameObject target) { } +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity public void SetVelocityLimit(in float value) { _velocityLimit = value; } +#endif public virtual void Reset() { @@ -81,11 +86,13 @@ public bool IsPrismaticType() _jointType == ArticulationJointType.PrismaticJoint) ? true : false; } +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity private float GetLimitedVelocity(in float velocity) { return (!float.IsNaN(_velocityLimit) && Mathf.Abs(velocity) > Mathf.Abs(_velocityLimit)) ? Mathf.Sign(velocity) * Mathf.Abs(_velocityLimit) : velocity; } +#endif protected void SetJointVelocity(in float velocity, in int targetDegree = 0) { @@ -94,7 +101,11 @@ protected void SetJointVelocity(in float velocity, in int targetDegree = 0) var jointVelocity = _jointBody.jointVelocity; if (targetDegree < jointVelocity.dofCount) { +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity jointVelocity[targetDegree] = GetLimitedVelocity(velocity); +#else + jointVelocity[targetDegree] = velocity; +#endif _jointBody.jointVelocity = jointVelocity; } } diff --git a/Assets/Scripts/Tools/SDF/Helper/Link.cs b/Assets/Scripts/Tools/SDF/Helper/Link.cs index 252dfd0b..8db9497f 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Link.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Link.cs @@ -35,8 +35,10 @@ public class Link : Base private UE.Pose _jointAnchorPose = new UE.Pose(); +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity private float _jointAxisLimitVelocity = float.NaN; private float _jointAxis2LimitVelocity = float.NaN; +#endif private List collisionContacts = new List(); @@ -60,7 +62,7 @@ public string JointChildLinkName get => this.jointChildLinkName; set => this.jointChildLinkName = value; } - +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity public float JointAxisLimitVelocity { get => this._jointAxisLimitVelocity; @@ -72,6 +74,7 @@ public float JointAxis2LimitVelocity get => this._jointAxis2LimitVelocity; set => this._jointAxis2LimitVelocity = value; } +#endif public UE.Pose LinkJointPose => _jointPose; diff --git a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs index 7085b8f8..c10ac21d 100644 --- a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs +++ b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs @@ -118,8 +118,6 @@ private void ResetArticulationBody(in int targetFrame = 0) { if (_articulationBody != null) { - var isPrismatic = _articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); - _articulationBody.velocity = UE.Vector3.zero; _articulationBody.angularVelocity = UE.Vector3.zero; @@ -134,16 +132,18 @@ private void ResetArticulationBody(in int targetFrame = 0) _articulationBody.jointVelocity = zeroSpace; _articulationBody.jointForce = zeroSpace; + if (_articulationBody.jointType.Equals(UE.ArticulationJointType.FixedJoint)) + { + return; + } + GetJointTarget(out var targetJoint1, out var targetJoint2, targetFrame); - // Debug.Log($"PoseControl {_articulationBody.name} targetJoint1={targetJoint1.ToString("F6")} targetJoint2={targetJoint2}"); + // Debug.Log($"PoseControl {_articulationBody.name} targetJoint1={targetJoint1.ToString("F6")} targetJoint2={targetJoint2.ToString("F6")}"); + // Debug.Log($"PoseControl {_articulationBody.name} X({_articulationBody.linearLockX}|{_articulationBody.twistLock}) Y({_articulationBody.linearLockY}|{_articulationBody.swingYLock}) Z({_articulationBody.linearLockZ}|{_articulationBody.swingZLock})"); var xDrive = _articulationBody.xDrive; - if (!isPrismatic) - { - xDrive.targetVelocity = 0; - } - if (!_articulationBody.linearLockX.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) + + if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) { if (!_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion) || !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) @@ -159,15 +159,10 @@ private void ResetArticulationBody(in int targetFrame = 0) { xDrive.target = 0; } - _articulationBody.xDrive = xDrive; var yDrive = _articulationBody.yDrive; - if (!isPrismatic) - { - yDrive.targetVelocity = 0; - } - if (!_articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) + + if (!_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) { if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) @@ -176,22 +171,17 @@ private void ResetArticulationBody(in int targetFrame = 0) } else { - yDrive.target = targetJoint1; + yDrive.target = targetJoint2; } } else { yDrive.target = 0; } - _articulationBody.yDrive = yDrive; var zDrive = _articulationBody.zDrive; - if (!isPrismatic) - { - zDrive.targetVelocity = 0; - } - if (!_articulationBody.linearLockY.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) + + if (!_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) { if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || !_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) @@ -207,6 +197,17 @@ private void ResetArticulationBody(in int targetFrame = 0) { zDrive.target = 0; } + + var isPrismatic = _articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); + if (!isPrismatic) + { + xDrive.targetVelocity = 0; + yDrive.targetVelocity = 0; + zDrive.targetVelocity = 0; + } + + _articulationBody.xDrive = xDrive; + _articulationBody.yDrive = yDrive; _articulationBody.zDrive = zDrive; } } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index a90a471d..63105128 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -97,14 +97,11 @@ public static void MakeRevoluteJoint(this UE.ArticulationBody body, in SDF.Axis drive.stiffness = (float)axis.dynamics.spring_stiffness; drive.target = SDF2Unity.CurveOrientation((float)axis.dynamics.spring_reference); drive.damping = (float)axis.dynamics.damping; - body.jointFriction = (float)axis.dynamics.friction; - } - else - { - body.jointFriction = 0.5f; + UE.Debug.LogWarning($"{body.name} axis.dynamics.spring_reference=" + drive.target); } - body.maxJointVelocity = (float)axis.limit.velocity;; + body.jointFriction = (axis.dynamics != null) ? (float)axis.dynamics.friction : 0.5f; + body.maxJointVelocity = (float)axis.limit.velocity; var jointAxis = SDF2Unity.Axis(axis.xyz); // UE.Debug.LogWarning(body.transform.parent.name + "::" + body.name + " = " + jointAxis + " - revolute"); @@ -159,10 +156,15 @@ private static void MakeRevoluteJoint2(this UE.ArticulationBody body, in SDF.Axi drive.SetRevoluteDriveLimit(axis2.limit); } - body.maxJointVelocity = (float)axis2.limit.velocity; drive.forceLimit = (double.IsInfinity(axis2.limit.effort)) ? float.MaxValue : (float)axis2.limit.effort; + var axis2JointFriction = (axis2.dynamics != null) ? (float)axis2.dynamics.friction : 0.5f; + + body.jointFriction = UE.Mathf.Min(body.jointFriction, axis2JointFriction); + body.maxJointVelocity = UE.Mathf.Min(body.maxJointVelocity, (float)axis2.limit.velocity); + var joint2Axis = SDF2Unity.Axis(axis2.xyz); + if (joint2Axis.Equals(UE.Vector3.right) || joint2Axis.Equals(UE.Vector3.left)) { if (joint2Axis.Equals(UE.Vector3.left)) @@ -192,7 +194,7 @@ private static void MakeRevoluteJoint2(this UE.ArticulationBody body, in SDF.Axi } else { - UE.Debug.LogWarning("MakeRevoluteJoint2 - Wrong axis, " + body.transform.parent.name + "::" + body.name + " = " + joint2Axis); + UE.Debug.LogWarning("MakeRevoluteJoint2 - Wrong axis2, " + body.transform.parent.name + "::" + body.name + " = " + joint2Axis); } } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index 71799854..f92b7c9f 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -31,18 +31,17 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec if (linkObjectChild is null) { - Debug.LogWarningFormat("child Link object is NULL!!! {0}", joint.ChildLinkName); + Debug.LogWarning($"child Link object is NULL!!! {joint.ChildLinkName}"); return; } - var articulationBodyChild = linkObjectChild.GetComponent(); - if (linkObjectChild is null || linkObjectParent is null) { - Debug.LogWarningFormat("RigidBody of Link is NULL!!! child({0}) parent({1})", linkObjectChild, linkObjectParent); + Debug.LogWarning($"RigidBody of Link is NULL!!! child({linkObjectChild}) parent({linkObjectParent})"); return; } + var articulationBodyChild = linkObjectChild.GetComponent(); if (articulationBodyChild == null) { Debug.LogWarningFormat("Link Child has NO Articulation Body, will create an articulation body for linking, parent({0}) child({1})", @@ -70,16 +69,16 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec { if (joint.Axis.dynamics != null) { - if (joint.Type.Equals("prismatic")) - axisSpringReference = (float)joint.Axis.dynamics.spring_reference; - else - axisSpringReference = SDF2Unity.CurveOrientation((float)joint.Axis.dynamics.spring_reference); + axisSpringReference = (joint.Type.Equals("prismatic")) ? + (float)joint.Axis.dynamics.spring_reference : + SDF2Unity.CurveOrientation((float)joint.Axis.dynamics.spring_reference); } - +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity if (!double.IsInfinity(joint.Axis.limit.velocity)) { linkHelper.JointAxisLimitVelocity = (float)joint.Axis.limit.velocity; } +#endif } if (joint.Axis2 != null) @@ -88,11 +87,12 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec { axis2SpringReference = SDF2Unity.CurveOrientation((float)joint.Axis2.dynamics.spring_reference); } - +#if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity if (!double.IsInfinity(joint.Axis2.limit.velocity)) { linkHelper.JointAxis2LimitVelocity = (float)joint.Axis2.limit.velocity; } +#endif } linkHelper.SetJointPoseTarget(axisSpringReference, axis2SpringReference); From a93ea292ab6521f75e571d3541656ea39e40fd1a Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 20 Nov 2024 15:30:51 +0900 Subject: [PATCH 06/19] Fix ResetArticulationBody() in PoseControl - reset default target reference on wrong axis --- Assets/Scripts/Tools/SDF/Helper/Base.cs | 7 +- .../Scripts/Tools/SDF/Helper/PoseControl.cs | 85 ++++++++----------- .../Tools/SDF/Implement/Implement.Joint.cs | 1 - .../Scripts/Tools/SDF/Import/Import.Joint.cs | 8 +- 4 files changed, 49 insertions(+), 52 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Helper/Base.cs b/Assets/Scripts/Tools/SDF/Helper/Base.cs index 09818d1a..098e0c31 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Base.cs +++ b/Assets/Scripts/Tools/SDF/Helper/Base.cs @@ -65,11 +65,14 @@ public void ClearPose() } } - public void SetJointPoseTarget(in float targetAxis1, in float targetAxis2, in int targetFrame = 0) + public void SetJointPoseTarget( + in UE.Vector3 axis1xyz, in float targetAxis1, + in UE.Vector3 axis2xyz, in float targetAxis2, + in int targetFrame = 0) { if (_poseControl != null) { - _poseControl.SetJointTarget(targetAxis1, targetAxis2, targetFrame); + _poseControl.SetJointTarget(axis1xyz, targetAxis1, axis2xyz, targetAxis2, targetFrame); } } diff --git a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs index c10ac21d..6b064dbe 100644 --- a/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs +++ b/Assets/Scripts/Tools/SDF/Helper/PoseControl.cs @@ -21,7 +21,9 @@ public class PoseControl struct JointTarget { + public UE.Vector3 axis1xyz; public float axis1; + public UE.Vector3 axis2xyz; public float axis2; } @@ -34,10 +36,15 @@ public PoseControl(in UE.Transform target) _targetTransform = target; } - public void SetJointTarget(in float targetAxis1, in float targetAxis2, in int targetFrame = 0) + public void SetJointTarget( + in UE.Vector3 axis1xyz, in float targetAxis1, + in UE.Vector3 axis2xyz, in float targetAxis2, + in int targetFrame = 0) { var jointTarget = new JointTarget(); + jointTarget.axis1xyz = axis1xyz; jointTarget.axis1 = targetAxis1; + jointTarget.axis2xyz = axis2xyz; jointTarget.axis2 = targetAxis2; if (targetFrame < _jointTargetList.Count) @@ -53,17 +60,26 @@ public void SetJointTarget(in float targetAxis1, in float targetAxis2, in int ta } } - public void GetJointTarget(out float targetAxis1, out float targetAxis2, in int targetFrame = 0) + public void GetJointTarget( + out UE.Vector3 axis1xyz, out float targetAxis1, + out UE.Vector3 axis2xyz, out float targetAxis2, + in int targetFrame = 0) { - targetAxis1 = 0; - targetAxis2 = 0; - if (targetFrame < _jointTargetList.Count) { var jointTarget = _jointTargetList[targetFrame]; + axis1xyz = jointTarget.axis1xyz; targetAxis1 = jointTarget.axis1; + axis2xyz = jointTarget.axis2xyz; targetAxis2 = jointTarget.axis2; } + else + { + axis1xyz = UE.Vector3.zero; + targetAxis1 = 0; + axis2xyz = UE.Vector3.zero; + targetAxis2 = 0; + } } public void Add(in UE.Vector3 newPosition, in UE.Quaternion newRotation) @@ -137,65 +153,38 @@ private void ResetArticulationBody(in int targetFrame = 0) return; } - GetJointTarget(out var targetJoint1, out var targetJoint2, targetFrame); + GetJointTarget(out var joint1Axis, out var targetJoint1, out var joint2Axis, out var targetJoint2, targetFrame); // Debug.Log($"PoseControl {_articulationBody.name} targetJoint1={targetJoint1.ToString("F6")} targetJoint2={targetJoint2.ToString("F6")}"); // Debug.Log($"PoseControl {_articulationBody.name} X({_articulationBody.linearLockX}|{_articulationBody.twistLock}) Y({_articulationBody.linearLockY}|{_articulationBody.swingYLock}) Z({_articulationBody.linearLockZ}|{_articulationBody.swingZLock})"); var xDrive = _articulationBody.xDrive; + var yDrive = _articulationBody.yDrive; + var zDrive = _articulationBody.zDrive; - if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion)) + if (joint1Axis.Equals(UE.Vector3.right) || joint1Axis.Equals(UE.Vector3.left)) { - if (!_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) - { - xDrive.target = targetJoint1; - } - else - { - xDrive.target = targetJoint2; - } + xDrive.target = targetJoint1; } - else + else if (joint1Axis.Equals(UE.Vector3.up) || joint1Axis.Equals(UE.Vector3.down)) { - xDrive.target = 0; + yDrive.target = targetJoint1; } - - var yDrive = _articulationBody.yDrive; - - if (!_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) + else if (joint1Axis.Equals(UE.Vector3.forward) || joint1Axis.Equals(UE.Vector3.back)) { - if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) - { - yDrive.target = targetJoint1; - } - else - { - yDrive.target = targetJoint2; - } + zDrive.target = targetJoint1; } - else + + if (joint2Axis.Equals(UE.Vector3.right) || joint2Axis.Equals(UE.Vector3.left)) { - yDrive.target = 0; + xDrive.target = targetJoint2; } - - var zDrive = _articulationBody.zDrive; - - if (!_articulationBody.swingZLock.Equals(UE.ArticulationDofLock.LockedMotion)) + else if (joint2Axis.Equals(UE.Vector3.up) || joint2Axis.Equals(UE.Vector3.down)) { - if (!_articulationBody.twistLock.Equals(UE.ArticulationDofLock.LockedMotion) || - !_articulationBody.swingYLock.Equals(UE.ArticulationDofLock.LockedMotion)) - { - zDrive.target = targetJoint1; - } - else - { - zDrive.target = targetJoint2; - } + yDrive.target = targetJoint2; } - else + else if (joint2Axis.Equals(UE.Vector3.forward) || joint2Axis.Equals(UE.Vector3.back)) { - zDrive.target = 0; + zDrive.target = targetJoint2; } var isPrismatic = _articulationBody.jointType.Equals(UE.ArticulationJointType.PrismaticJoint); diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index 63105128..8a9b2d6b 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs @@ -97,7 +97,6 @@ public static void MakeRevoluteJoint(this UE.ArticulationBody body, in SDF.Axis drive.stiffness = (float)axis.dynamics.spring_stiffness; drive.target = SDF2Unity.CurveOrientation((float)axis.dynamics.spring_reference); drive.damping = (float)axis.dynamics.damping; - UE.Debug.LogWarning($"{body.name} axis.dynamics.spring_reference=" + drive.target); } body.jointFriction = (axis.dynamics != null) ? (float)axis.dynamics.friction : 0.5f; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index f92b7c9f..3efebcd9 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -58,7 +58,9 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec var linkHelper = linkObjectChild.GetComponent(); if (linkHelper != null) { + var axis1xyz = UE.Vector3.zero; var axisSpringReference = 0f; + var axis2xyz = UE.Vector3.zero; var axis2SpringReference = 0f; linkHelper.JointName = joint.Name; @@ -69,10 +71,12 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec { if (joint.Axis.dynamics != null) { + axis1xyz = SDF2Unity.Axis(joint.Axis.xyz); axisSpringReference = (joint.Type.Equals("prismatic")) ? (float)joint.Axis.dynamics.spring_reference : SDF2Unity.CurveOrientation((float)joint.Axis.dynamics.spring_reference); } + #if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity if (!double.IsInfinity(joint.Axis.limit.velocity)) { @@ -85,8 +89,10 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec { if (joint.Axis2.dynamics != null) { + axis2xyz = SDF2Unity.Axis(joint.Axis2.xyz); axis2SpringReference = SDF2Unity.CurveOrientation((float)joint.Axis2.dynamics.spring_reference); } + #if true // TODO: Candidate to remove due to AriticulationBody.maxJointVelocity if (!double.IsInfinity(joint.Axis2.limit.velocity)) { @@ -95,7 +101,7 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec #endif } - linkHelper.SetJointPoseTarget(axisSpringReference, axis2SpringReference); + linkHelper.SetJointPoseTarget(axis1xyz, axisSpringReference, axis2xyz, axis2SpringReference); } } } From a1a3b5afc3a51c78f5f2279fd916d3f57c5913a8 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 20 Nov 2024 18:39:25 +0900 Subject: [PATCH 07/19] Fix updating information of Bumper sensor Modify MicomSensor --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 30 ++++- Assets/Scripts/Devices/Contact.cs | 121 ++++++------------ Assets/Scripts/Devices/MicomSensor.cs | 37 ++---- .../Tools/SDF/Implement/Implement.Sensor.cs | 3 +- 4 files changed, 72 insertions(+), 119 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index b2c33d79..0510c5d9 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -288,13 +288,26 @@ private void SetMotorPID( if (GetPluginParameters().IsValidNode($"{parameterPrefix}/limit")) { - var limitIntegral = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral"); - var limitOutput = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output"); + if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") && + !GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max")) + { + var limitIntegral = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral"); + integralMin = -Math.Abs(limitIntegral); + integralMax = Math.Abs(limitIntegral); + } - integralMin = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral/min", -Mathf.Abs(limitIntegral)); - integralMax = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral/max", Mathf.Abs(limitIntegral)); - outputMin = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output/min", -Mathf.Abs(limitOutput)); - outputMax = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output/max", Mathf.Abs(limitOutput)); + if (!GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/min") && + !GetPluginParameters().IsValidNode($"{parameterPrefix}/limit/integral/max")) + { + var limitOutput = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output"); + outputMin = -Math.Abs(limitOutput); + outputMax = Math.Abs(limitOutput); + } + + integralMin = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral/min", integralMin); + integralMax = GetPluginParameters().GetValue($"{parameterPrefix}/limit/integral/max", integralMax); + outputMin = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output/min", outputMin); + outputMax = GetPluginParameters().GetValue($"{parameterPrefix}/limit/output/max", outputMax); } SetPID(P, I, D, integralMin, integralMax, outputMin, outputMax); @@ -415,8 +428,11 @@ protected override void HandleCustomRequestMessage(in string requestType, in Any SetEmptyResponse(ref response); break; + case "request_bumper_topic_name": + break; + default: break; } } -} +} \ No newline at end of file diff --git a/Assets/Scripts/Devices/Contact.cs b/Assets/Scripts/Devices/Contact.cs index b9e486bc..914e5382 100644 --- a/Assets/Scripts/Devices/Contact.cs +++ b/Assets/Scripts/Devices/Contact.cs @@ -4,7 +4,6 @@ * SPDX-License-Identifier: MIT */ -using System.Collections.Generic; using System; using UnityEngine; using messages = cloisim.msgs; @@ -13,15 +12,14 @@ namespace SensorDevices { public class ContactTrigger : MonoBehaviour { - public Action collisionEnter = null; public Action collisionStay = null; public Action collisionExit = null; - private void OnCollisionEnter(Collision other) + private void OnCollisionStay(Collision other) { - if (collisionEnter != null) + if (collisionStay != null) { - collisionEnter.Invoke(other); + collisionStay.Invoke(other); } } @@ -32,14 +30,6 @@ private void OnCollisionExit(Collision other) collisionExit.Invoke(other); } } - - private void OnCollisionStay(Collision other) - { - if (collisionStay != null) - { - collisionStay.Invoke(other); - } - } } public class Contact : Device @@ -57,6 +47,7 @@ protected override void OnAwake() protected override void OnStart() { + // Debug.Log("Contact target collision: " + targetCollision); } protected override void InitializeMessages() @@ -68,39 +59,43 @@ protected override void InitializeMessages() protected override void GenerateMessage() { contacts.Time.SetCurrentTime(); - // if (contacts.contact.Count > 0) - // { - // Debug.Log(contacts.contact[0].Depths.Length + " : " + contacts.contact[0].Normals.Count); - // } PushDeviceMessage(contacts); } - public string GetColliderParentName(in Collider collider) + public string GetColliderName(in Collider collider) { - return collider.transform.parent.name; + var collisionHelper = collider.transform.gameObject.GetComponentInParent(); + if (collisionHelper == null) + { + return collider.transform.parent.name + "::" + collider.name; + } + else + { + var linkHelper = collisionHelper.GetComponentInParent(); + return linkHelper.Model.name + "::" + linkHelper.name + "::" + collisionHelper.name; + } } - public string GetColliderName(in Collider collider) + public void CollisionEnter(Collision other) { - var childName = collider.name; - var parentName = GetColliderParentName(collider); - return parentName + "::" + childName; + // Debug.Log("CollisionEnter: " + other.contacts.Length); } - public void CollisionEnter(Collision other) + public void CollisionStay(Collision other) { + // Debug.Log("CollisionStay: " + other.contacts.Length); for (var i = 0; i < other.contacts.Length; i++) { var collisionContact = other.contacts[i]; - var collision1 = GetColliderParentName(collisionContact.thisCollider); + var collision1 = GetColliderName(collisionContact.thisCollider); var collision2 = GetColliderName(collisionContact.otherCollider); - // Debug.Log("CollisionEnter: " + collision1 + " <-> " + collision2); + // Debug.Log($"CollisionStay: {collision1} <-> {collision2}"); - if (string.Equals(collision1, targetCollision)) + if (collision1.EndsWith("::" + targetCollision)) { // find existing collision set - var foundContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2)); - if (foundContact == null) + var existingContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2)); + if (existingContact == null) { var newContact = new messages.Contact(); // newContact.Wrenchs // TODO: Need to be implemented; @@ -109,70 +104,32 @@ public void CollisionEnter(Collision other) newContact.Collision1 = collision1; newContact.Collision2 = collision2; - newContact.Depths = new double[0]; + newContact.Depths = new double[] { collisionContact.separation }; - contacts.contact.Add(newContact); - } - } - } - } + var normal = new messages.Vector3d(); + normal.Set(collisionContact.normal); + newContact.Normals.Add(normal); - public void CollisionStay(Collision other) - { - for (var i = 0; i < other.contacts.Length; i++) - { - var collisionContact = other.contacts[i]; - var collision1 = GetColliderParentName(collisionContact.thisCollider); - if (!string.Equals(collision1, targetCollision)) - { - continue; - } + var position = new messages.Vector3d(); + position.Set(collisionContact.point); + newContact.Positions.Add(position); - var collision2 = GetColliderName(collisionContact.otherCollider); - // Debug.Log("CollsiionStay: " + collision1 + " " + collision2); + newContact.Time.SetCurrentTime(); + // Debug.Log("CollisionStay: " + collision1 + " <-> " + collision2); - // find existing collision set - var existingContact = contacts.contact.Find(x => x.Collision1.Contains(collision1) && x.Collision2.Contains(collision2)); - if (existingContact != null) - { - // Debug.Log("Existing!!"); - var depths = existingContact.Depths; - Array.Resize(ref depths, depths.Length + 1); - depths[depths.Length - 1] = collisionContact.separation; - existingContact.Depths = depths; - - var normal = new messages.Vector3d(); - normal.Set(collisionContact.normal); - existingContact.Normals.Add(normal); - - var position = new messages.Vector3d(); - position.Set(collisionContact.point); - existingContact.Positions.Add(position); - - existingContact.Time.SetCurrentTime(); - // Debug.Log("CollisionStay: " + collision1 + " <-> " + collision2); + contacts.contact.Add(newContact); + } } - // Debug.DrawLine(collisionContact.point, collisionContact.normal, Color.white); } + + // Debug.Log("CollisionStay: " + contacts.contact.Count); } public void CollisionExit(Collision other) { - // Debug.Log("CollisionExit: " + other.contactCount + " ," + other.collider.name); - var collision2 = GetColliderName(other.collider); - var foundContacts = contacts.contact.FindAll(x => x.Collision2.Contains(collision2)); - - foreach (var foundContact in foundContacts) - { - // Debug.Log("CollisionExit: Remove " + foundContact.Collision1 + " <-> " + foundContact.Collision2); - contacts.contact.Remove(foundContact); - } - - // if (contacts.contact.Count == 0) - // { - // Debug.Log("CollisionExit: no contacts"); - // } + // Debug.Log($"CollisionExit: {other.contacts.Length}"); + contacts.contact.Clear(); } public bool IsContacted() diff --git a/Assets/Scripts/Devices/MicomSensor.cs b/Assets/Scripts/Devices/MicomSensor.cs index 2455f5fc..c8800d32 100644 --- a/Assets/Scripts/Devices/MicomSensor.cs +++ b/Assets/Scripts/Devices/MicomSensor.cs @@ -20,9 +20,7 @@ public partial class MicomSensor : Device private List ussSensors = new List(); private List irSensors = new List(); // private List magnetSensors = null; - private List bumperSensors = new List(); - - // private List bumperSensors = new List(); + private List _bumperSensors = new List(); protected override void OnAwake() @@ -103,48 +101,31 @@ public void SetMagnet(in List magnetList) { // TODO: to be implemented } + Debug.Log("Magnet is to be implemented"); } public void SetBumper(in List bumperList) { - // Debug.Log(targetContactName); var contactsInChild = GetComponentsInChildren(); + + var bumperCount = 0; foreach (var bumper in bumperList) { foreach (var contact in contactsInChild) { if (contact.name.Equals(bumper)) { - bumperSensors.Add(contact); + _bumperSensors.Add(contact); + bumperCount++; Debug.Log("Found " + contact.name); + break; } } } - var bumperCount = bumperSensors.Count; micomSensorData.bumper.Bumpeds = new bool[bumperCount]; } -#if false - public void SetBumperSensor(in List bumperJointNameList) - { - if (bumperContact != null) - { - var linkList = GetComponentsInChildren(); - foreach (var link in linkList) - { - foreach (var bumperJointName in bumperJointNameList) - { - // TODO: to be implemented - } - } - - var bumperCount = bumperSensors.Count; - micomSensorData.bumper.Bumpeds = new bool[bumperCount]; - } - } -#endif - public void SetBattery(in SensorDevices.Battery targetBattery) { micomSensorData.Battery = new messages.Battery(); @@ -219,9 +200,9 @@ private void UpdateBumper() return; } - for (var index = 0; index < bumperSensors.Count; index++) + for (var index = 0; index < _bumperSensors.Count; index++) { - var bumperSensor = bumperSensors[index]; + var bumperSensor = _bumperSensors[index]; micomSensorData.bumper.Bumpeds[index] = bumperSensor.IsContacted(); // Debug.Log(micomSensorData.bumper.Bumpeds[index]); } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index a2d2cc3d..a223558e 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -295,9 +295,8 @@ public static Device AddContact(this UE.GameObject targetObject, in SDF.Contact contact.topic = element.topic; var contactTrigger = targetObject.AddComponent(); - contactTrigger.collisionEnter = contact.CollisionEnter; - contactTrigger.collisionExit = contact.CollisionExit; contactTrigger.collisionStay = contact.CollisionStay; + contactTrigger.collisionExit = contact.CollisionExit; return contact; } From b278854532f196720be647fa94e8844c1ec74c8b Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 21 Nov 2024 11:14:51 +0900 Subject: [PATCH 08/19] Upgrade Unity editor version -> 2022.3.53f1 (LTS) --- ProjectSettings/ProjectVersion.txt | 4 +- ProjectSettings/SceneTemplateSettings.json | 85 ++++++++++++++++++++++ 2 files changed, 87 insertions(+), 2 deletions(-) diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index 3b8759d9..ba6dfe72 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.52f1 -m_EditorVersionWithRevision: 2022.3.52f1 (1120fcb54228) +m_EditorVersion: 2022.3.53f1 +m_EditorVersionWithRevision: 2022.3.53f1 (df4e529d20d3) diff --git a/ProjectSettings/SceneTemplateSettings.json b/ProjectSettings/SceneTemplateSettings.json index aed05ef2..5e97f839 100644 --- a/ProjectSettings/SceneTemplateSettings.json +++ b/ProjectSettings/SceneTemplateSettings.json @@ -1,21 +1,91 @@ { "templatePinStates": [], "dependencyTypeInfos": [ + { + "userAdded": false, + "type": "UnityEngine.AnimationClip", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEditor.Animations.AnimatorController", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.AnimatorOverrideController", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEditor.Audio.AudioMixerController", + "defaultInstantiationMode": 0 + }, { "userAdded": false, "type": "UnityEngine.ComputeShader", "defaultInstantiationMode": 1 }, + { + "userAdded": false, + "type": "UnityEngine.Cubemap", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.GameObject", + "defaultInstantiationMode": 0 + }, { "userAdded": false, "type": "UnityEditor.LightingDataAsset", "defaultInstantiationMode": 0 }, + { + "userAdded": false, + "type": "UnityEngine.LightingSettings", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Material", + "defaultInstantiationMode": 0 + }, { "userAdded": false, "type": "UnityEditor.MonoScript", "defaultInstantiationMode": 1 }, + { + "userAdded": false, + "type": "UnityEngine.PhysicMaterial", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.PhysicsMaterial2D", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Rendering.PostProcessing.PostProcessProfile", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Rendering.PostProcessing.PostProcessResources", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Rendering.VolumeProfile", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEditor.SceneAsset", + "defaultInstantiationMode": 1 + }, { "userAdded": false, "type": "UnityEngine.Shader", @@ -25,6 +95,21 @@ "userAdded": false, "type": "UnityEngine.ShaderVariantCollection", "defaultInstantiationMode": 1 + }, + { + "userAdded": false, + "type": "UnityEngine.Texture", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Texture2D", + "defaultInstantiationMode": 0 + }, + { + "userAdded": false, + "type": "UnityEngine.Timeline.TimelineAsset", + "defaultInstantiationMode": 0 } ], "defaultDependencyTypeInfo": { From 422a054662a87d78bfcc28b1b548688bb278d7b2 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Thu, 21 Nov 2024 11:49:14 +0900 Subject: [PATCH 09/19] Update README.md --- README.md | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index b1182b99..bdeefe83 100644 --- a/README.md +++ b/README.md @@ -69,7 +69,8 @@ Here are the list of items that is implemented(marked) or planned to be implemen - [X] Road Plus, [SDF](http://sdformat.org/spec?ver=1.6) file basically targeting and supporting version 1.6 and works on the essential elements such as ``, ``, ``, ``, ``, etc. -It does not support optional elmenets like ``, `