From 8b9b617601ef18dd5b1b111d4fb0a7bad7a55779 Mon Sep 17 00:00:00 2001 From: Hyunseok Date: Wed, 10 Mar 2021 02:43:27 +0900 Subject: [PATCH] CLOiSim-2.0.1 [Major Changes] * Update app version info in Project settings - 2.0.0 -> 2.0.1 * Upgrade Unity editor version - 2020.2.7f1 * Update package version - Core RP Library: 10.3.1 -> 10.3.2 * Apply pose of Joint(SDF) when apply anchor position for articulation body * Update Dynamic parameters for physics - solver Iterations: 10 - solver velocity iteration: 5 * Changes in Motor module - change to retrieving motor velocity using jointPosition - remove getting averaged joint velocity * (GUI) Print error meessage on simulation display during SDF parsing [Minor Changes] * Add protection code in Lidar Data generation * Parameter name changed in ImportJoint Method * refactoring for SDF sensor ray range element * Change GUI properties [Bug fix] * Bug fix in Lidar Sensor device - Set sensor position and rotation for lidar - fix rotation for lidar visualization * Fix in Motor module - set false in rapid direction control after stop * Bug Fix in Lidar Sensor device - calculate angle step - deg to rad for vertical angle --- Assets/Scenes/MainScene.unity | 91 ++++++++++++++----- .../Scripts/Devices/Lidar.LaserCameraData.cs | 3 + Assets/Scripts/Devices/Lidar.cs | 39 +++++--- Assets/Scripts/Devices/Modules/Motor.cs | 56 ++++++------ Assets/Scripts/Main.cs | 6 +- .../Tools/SDF/Implement/Implement.Sensor.cs | 8 +- .../Scripts/Tools/SDF/Import/Import.Joint.cs | 5 + .../Scripts/Tools/SDF/Import/Import.Sensor.cs | 55 +++++------ Assets/Scripts/Tools/SDF/Parser/SDF.Model.cs | 2 + Assets/Scripts/Tools/SDF/Parser/SDF.Root.cs | 11 ++- .../Tools/SDF/Parser/SDF.Sensor.Parse.cs | 9 +- .../Tools/SDF/Parser/SDF.SensorType.cs | 12 ++- Assets/Scripts/UI/ObjectSpawning.cs | 20 ++-- Assets/Scripts/UI/SimulationDisplay.cs | 39 +++++--- Packages/manifest.json | 2 +- Packages/packages-lock.json | 2 +- ProjectSettings/DynamicsManager.asset | 4 +- ProjectSettings/ProjectSettings.asset | 2 +- ProjectSettings/ProjectVersion.txt | 4 +- 19 files changed, 246 insertions(+), 124 deletions(-) diff --git a/Assets/Scenes/MainScene.unity b/Assets/Scenes/MainScene.unity index 49008710..1791395b 100644 --- a/Assets/Scenes/MainScene.unity +++ b/Assets/Scenes/MainScene.unity @@ -218,8 +218,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!1 &25597963 @@ -316,8 +316,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!1 &34803206 @@ -827,8 +827,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isTopModel: 1 isStatic: 0 --- !u!1 &249819585 @@ -1297,8 +1297,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!135 &402915479 @@ -1961,8 +1961,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!171741748 &635764732 @@ -2548,8 +2548,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isTopModel: 1 isStatic: 0 --- !u!1 &779420288 @@ -2617,6 +2617,53 @@ GameObject: m_StaticEditorFlags: 0 m_IsActive: 1 --- !u!4 &789896500 +Transform: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 789896499} + m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} + m_LocalPosition: {x: 0, y: 0, z: 0} + m_LocalScale: {x: 0.5, y: 0.1, z: 0.5} + m_Children: [] + m_Father: {fileID: 25597964} + m_RootOrder: 0 + m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} +--- !u!64 &779420291 +MeshCollider: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 779420288} + m_Material: {fileID: 13400000, guid: 9cf0822921b89b82fb8239bf69165329, type: 2} + m_IsTrigger: 0 + m_Enabled: 1 + serializedVersion: 4 + m_Convex: 1 + m_CookingOptions: 20 + m_Mesh: {fileID: 10206, guid: 0000000000000000e000000000000000, type: 0} +--- !u!1 &789896499 +GameObject: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + serializedVersion: 6 + m_Component: + - component: {fileID: 789896500} + - component: {fileID: 789896503} + - component: {fileID: 789896502} + - component: {fileID: 789896501} + m_Layer: 0 + m_Name: GameObject + m_TagString: Untagged + m_Icon: {fileID: 0} + m_NavMeshLayer: 0 + m_StaticEditorFlags: 0 + m_IsActive: 1 +--- !u!4 &789896500 Transform: m_ObjectHideFlags: 0 m_CorrespondingSourceObject: {fileID: 0} @@ -2932,8 +2979,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!1 &887438904 @@ -3232,8 +3279,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 1c0ad0e8e55c95c4e931911c4b943a64, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: space: 1 transformType: 0 pivot: 0 @@ -5451,8 +5498,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isTopModel: 0 isStatic: 0 --- !u!1 &1750713662 @@ -5816,8 +5863,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 5f9694630fd757ed38508aa2d402926b, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isSelfCollide: 0 drawInertia: 0 --- !u!1 &1835497263 @@ -6297,8 +6344,8 @@ MonoBehaviour: m_Enabled: 1 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: cb1f25bcbdf8dbeb2910354dd18778b2, type: 3} - m_Name: - m_EditorClassIdentifier: + m_Name: + m_EditorClassIdentifier: isTopModel: 1 isStatic: 0 --- !u!1 &2133470735 diff --git a/Assets/Scripts/Devices/Lidar.LaserCameraData.cs b/Assets/Scripts/Devices/Lidar.LaserCameraData.cs index b084fcbc..0be69d42 100644 --- a/Assets/Scripts/Devices/Lidar.LaserCameraData.cs +++ b/Assets/Scripts/Devices/Lidar.LaserCameraData.cs @@ -35,7 +35,10 @@ public void AllocateBuffer(in int width, in int height) { imageWidth = width; imageHeight = height; + output = new double[imageWidth]; + // TODO: vertical ray + // output = new double[imageWidth * imageHeight]; } public void SetBufferData(in NativeArray buffer) diff --git a/Assets/Scripts/Devices/Lidar.cs b/Assets/Scripts/Devices/Lidar.cs index 42266684..e8a87dfa 100644 --- a/Assets/Scripts/Devices/Lidar.cs +++ b/Assets/Scripts/Devices/Lidar.cs @@ -45,6 +45,7 @@ public partial class Lidar : Device public Color rayColor = new Color(1, 0.1f, 0.1f, 0.15f); private Transform lidarLink = null; + private Pose lidarSensorInitPose = new Pose(); private ShadowQuality originalShadowSettings_; private UnityEngine.Camera laserCam = null; private Material depthMaterial = null; @@ -87,10 +88,9 @@ private void OnPostRender() QualitySettings.shadows = originalShadowSettings_; } - private double GetAngleStep(in double minAngle, in double maxAngle, in uint totalSamples) { - return (maxAngle - minAngle) / (resolution * (totalSamples - 1)); + return (maxAngle - minAngle) / (resolution * totalSamples); } protected override void OnAwake() @@ -107,6 +107,9 @@ protected override void OnStart() { if (laserCam) { + lidarSensorInitPose.position = transform.localPosition; + lidarSensorInitPose.rotation = transform.localRotation; + DoParseFilter(); SetupLaserCamera(); @@ -131,16 +134,15 @@ protected override void InitializeMessages() laserScan.AngleMin = angleMin * Mathf.Deg2Rad; laserScan.AngleMax = angleMax * Mathf.Deg2Rad; laserScan.AngleStep = GetAngleStep(laserScan.AngleMin, laserScan.AngleMax, samples); + laserScan.Count = samples; + laserScan.RangeMin = rangeMin; laserScan.RangeMax = rangeMax; - laserScan.Count = samples; laserScan.VerticalAngleMin = verticalAngleMin * Mathf.Deg2Rad; laserScan.VerticalAngleMax = verticalAngleMax * Mathf.Deg2Rad; + laserScan.VerticalAngleStep = (verticalAngleMin == 0 && verticalAngleMax == 0) ? 1 : GetAngleStep(laserScan.VerticalAngleMin, laserScan.VerticalAngleMax, verticalSamples); laserScan.VerticalCount = verticalSamples; - laserScan.VerticalAngleStep - = (verticalAngleMin == 0 && verticalAngleMax == 0) ? - 1 : GetAngleStep(laserScan.VerticalAngleMin, laserScan.VerticalAngleMax, verticalSamples); laserScan.Ranges = new double[samples]; laserScan.Intensities = new double[samples]; @@ -236,7 +238,7 @@ private IEnumerator LaserCameraWorker() var data = laserCamData[dataIndex]; axisRotation.y = data.centerAngle; - laserCam.transform.localRotation = Quaternion.Euler(axisRotation); + laserCam.transform.localRotation = lidarSensorInitPose.rotation * Quaternion.Euler(axisRotation); laserCam.enabled = true; @@ -296,8 +298,8 @@ protected override IEnumerator MainDeviceWorker() protected override void GenerateMessage() { - var lidarPosition = lidarLink.position + transform.localPosition; - var lidarRotation = lidarLink.rotation; + var lidarPosition = lidarLink.position + lidarSensorInitPose.position; + var lidarRotation = lidarLink.rotation * lidarSensorInitPose.rotation; var laserScan = laserScanStamped.Scan; @@ -331,6 +333,11 @@ protected override void GenerateMessage() var srcLengthratio = Mathf.Abs((dataStartAngle - laserStartAngle) / dataTotalAngle); copyLength = outputBufferLength - Mathf.FloorToInt(outputBufferLength * srcLengthratio); dstBufferOffset = (int)samples - copyLength; + + if (copyLength < 0 || dstBufferOffset < 0) + { + doCopy = false; + } } // middle of laser angle else if (dataStartAngle >= laserStartAngle && dataEndAngle < laserEndAngle) @@ -338,6 +345,11 @@ protected override void GenerateMessage() srcBufferOffset = 0; copyLength = outputBufferLength; dstBufferOffset = (int)samples - (Mathf.CeilToInt(samples * ((dataStartAngle - laserStartAngle) / laserTotalAngle)) + copyLength); + + if (copyLength < 0 || dstBufferOffset < 0) + { + doCopy = false; + } } // end side of laser angle else if (dataEndAngle >= laserEndAngle) @@ -346,6 +358,11 @@ protected override void GenerateMessage() copyLength = Mathf.CeilToInt(outputBufferLength * srcLengthRatio); srcBufferOffset = outputBufferLength - copyLength; dstBufferOffset = 0; + + if (copyLength < 0 || srcBufferOffset < 0) + { + doCopy = false; + } } else { @@ -379,13 +396,13 @@ protected override IEnumerator OnVisualize() { yield return waitForEndOfFrame; - var lidarSensorWorldPosition = lidarLink.position + transform.localPosition; + var lidarSensorWorldPosition = lidarLink.position + lidarSensorInitPose.position; var rangeData = GetRangeData(); for (var hScanIndex = 0; hScanIndex < rangeData.Length; hScanIndex++) { var rayAngleH = ((laserHAngleResolution * hScanIndex)) + startAngle; - var rayRotation = Quaternion.AngleAxis((float)(rayAngleH), lidarLink.up) * lidarLink.forward; + var rayRotation = (Quaternion.AngleAxis((float)rayAngleH, transform.up)) * lidarLink.forward; var rayStart = (rayRotation * (float)rangeMin) + lidarSensorWorldPosition; var ccwIndex = (uint)(rangeData.Length - hScanIndex - 1); diff --git a/Assets/Scripts/Devices/Modules/Motor.cs b/Assets/Scripts/Devices/Modules/Motor.cs index a01f45ac..f97c59b6 100644 --- a/Assets/Scripts/Devices/Modules/Motor.cs +++ b/Assets/Scripts/Devices/Modules/Motor.cs @@ -4,7 +4,6 @@ * SPDX-License-Identifier: MIT */ -using System.Collections.Generic; using UnityEngine; public class Motor : MonoBehaviour @@ -120,8 +119,6 @@ public float Compensate() public float _targetTorque = 0; public float _currentMotorVelocity; - public const float compensatingRatio = 1.20f; // compensting target velocity - public string GetMotorName() { return (_motorBody == null)? string.Empty:_motorBody.transform.parent.name; @@ -135,6 +132,7 @@ public void SetTargetJoint(in GameObject target) public void SetTargetJoint(in ArticulationBody body) { + if (body.jointType.Equals(ArticulationJointType.RevoluteJoint) || body.jointType.Equals(ArticulationJointType.SphericalJoint)) { _motorBody = body; @@ -166,25 +164,29 @@ public float GetCurrentVelocity() /// degree per second public void SetVelocityTarget(in float targetAngularVelocity) { + var compensatingVelocityRatio = 0f; + if (Mathf.Abs(targetAngularVelocity) < float.Epsilon || targetAngularVelocity == 0) { _enableMotor = false; - _targetAngularVelocity = 0; } else { _enableMotor = true; - if (_targetAngularVelocity != 0 && _feedback.IsMotionRotating) + if (_feedback.IsMotionRotating) { var directionSwitch = (Mathf.Sign(_targetAngularVelocity) == Mathf.Sign(targetAngularVelocity)) ? false : true; _rapidControl.SetDirectionSwitched(directionSwitch); } const float compensateThreshold = 10.0f; + const float compensatingRatio = 1.20f; - _targetAngularVelocity = targetAngularVelocity * ((Mathf.Abs(targetAngularVelocity) < compensateThreshold) ? compensatingRatio : 1.0f); + compensatingVelocityRatio = ((Mathf.Abs(targetAngularVelocity) < compensateThreshold) ? compensatingRatio : 1.0f); } + + _targetAngularVelocity = targetAngularVelocity * compensatingVelocityRatio; } void FixedUpdate() @@ -204,14 +206,14 @@ void FixedUpdate() // Debug.LogFormat("joint vel({0}) accel({1}) force({2}) friction({3}) pos({4})", // _motorBody.jointVelocity[0], _motorBody.jointAcceleration[0], _motorBody.jointForce[0], _motorBody.jointFriction, _motorBody.jointPosition[0]); - // Compensate target angular velocity - var targetAngularVelocityCompensation = (_targetAngularVelocity != 0) ? (Mathf.Sign(_targetAngularVelocity) * _feedback.Compensate()) : 0; - - var compensatedTargetAngularVelocity = _targetAngularVelocity + targetAngularVelocityCompensation; - // do stop motion of motor when motor disabled if (_enableMotor) { + // Compensate target angular velocity + var targetAngularVelocityCompensation = (_targetAngularVelocity != 0) ? (Mathf.Sign(_targetAngularVelocity) * _feedback.Compensate()) : 0; + + var compensatedTargetAngularVelocity = _targetAngularVelocity + targetAngularVelocityCompensation; + _targetTorque = Mathf.Abs(_pidControl.Update(compensatedTargetAngularVelocity, _currentMotorVelocity, Time.fixedDeltaTime)); // Debug.Log(GetMotorName() + ", " + _targetAngularVelocity + " <=> " + _currentMotorVelocity); @@ -234,15 +236,16 @@ void FixedUpdate() public void Stop() { - _motorBody.velocity = Vector3.zero; - _motorBody.angularVelocity = Vector3.zero; - _targetTorque = 0; SetJointVelocity(0); SetTargetVelocityAndForce(0, 0); _pidControl.Reset(); + _rapidControl.SetDirectionSwitched(false); + + _motorBody.velocity = Vector3.zero; + _motorBody.angularVelocity = Vector3.zero; } private ArticulationDrive GetDrive() @@ -293,6 +296,10 @@ private void SetDrive(in ArticulationDrive drive) _motorBody.zDrive = drive; } } + else + { + Stop(); + } } private void SetTargetVelocityAndForce(in float targetVelocity, in float targetForce) @@ -316,23 +323,20 @@ private void SetJointVelocity(in float velocity) } } - private const int _maxQueueForPrevJointVelocities = 3; - private float _prevJointVelocitySum = 0; - private Queue _prevJointVelocities = new Queue(_maxQueueForPrevJointVelocities); + public float _prevJointPosition = 0; private float GetJointVelocity() { - var currentJointVelocity = _motorBody.jointVelocity[0] * Mathf.Rad2Deg; - _prevJointVelocities.Enqueue(currentJointVelocity); - _prevJointVelocitySum += currentJointVelocity; - - var averageJointVelocity = 0f; - if (_prevJointVelocities.Count == _maxQueueForPrevJointVelocities) + if (_motorBody == null) { - averageJointVelocity = _prevJointVelocitySum/(float)_maxQueueForPrevJointVelocities; - _prevJointVelocitySum -= _prevJointVelocities.Dequeue(); + return 0; } - return (_motorBody) ? (averageJointVelocity) : 0; + // calculate velocity using joint position is more accurate than joint velocity + var jointPosition = _motorBody.jointPosition[0] * Mathf.Rad2Deg; + var jointVelocity = (Mathf.DeltaAngle(_prevJointPosition, jointPosition) / Time.fixedDeltaTime); + _prevJointPosition = jointPosition; + + return jointVelocity; } } \ No newline at end of file diff --git a/Assets/Scripts/Main.cs b/Assets/Scripts/Main.cs index 4a8070fc..ca5c7f6f 100644 --- a/Assets/Scripts/Main.cs +++ b/Assets/Scripts/Main.cs @@ -138,6 +138,7 @@ private IEnumerator LoadWorld() // Debug.Log("World: " + worldFileName); var sdf = new SDF.Root(); + sdf.SetTargetLogOutput(simulationDisplay); sdf.SetWorldFileName(worldFileName); sdf.fileDefaultPath = filesRootDirectory; sdf.modelDefaultPaths.AddRange(modelRootDirectories); @@ -157,8 +158,9 @@ private IEnumerator LoadWorld() } else { - Debug.LogError("Parsing failed!!"); - simulationDisplay?.SetEventMessage("Failed to load world file: " + worldFileName); + var errorMessage = "Parsing failed!!!, failed to load world file: " + worldFileName; + Debug.LogError(errorMessage); + simulationDisplay?.SetEventMessage(errorMessage); } yield return null; diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs index a944e2e2..98f40e53 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Implement/Implement.Sensor.cs @@ -63,14 +63,14 @@ public static Device AddLidar(in SDF.Ray element, in GameObject targetObject) lidar.deviceName = GetFrameName(newSensorObject); lidar.samples = element.horizontal.samples; - lidar.rangeMin = element.range_min; - lidar.rangeMax = element.range_max; + lidar.rangeMin = element.range.min; + lidar.rangeMax = element.range.max; lidar.angleMin = element.horizontal.min_angle * Mathf.Rad2Deg; lidar.angleMax = element.horizontal.max_angle * Mathf.Rad2Deg; lidar.resolution = element.horizontal.resolution; lidar.verticalSamples = element.vertical.samples; - lidar.verticalAngleMin = element.vertical.min_angle; - lidar.verticalAngleMax = element.vertical.max_angle; + lidar.verticalAngleMin = element.vertical.min_angle * Mathf.Rad2Deg; + lidar.verticalAngleMax = element.vertical.max_angle * Mathf.Rad2Deg; // lidar.hideFlags = HideFlags.NotEditable; return lidar; diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index f3b9af60..6e0a1fb3 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs @@ -117,6 +117,11 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec anchorPose.rotation = modelTransformChild.localRotation; } + var jointPosition = SDF2Unity.GetPosition(joint.Pose.Pos); + var jointRotation = SDF2Unity.GetRotation(joint.Pose.Rot); + anchorPose.position += jointPosition; + anchorPose.rotation *= jointRotation; + Implement.Joint.SetArticulationBodyAnchor(articulationBodyChild, anchorPose); } diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs index aa3cfe5c..3b3e9d83 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Sensor.cs @@ -16,57 +16,57 @@ namespace Import { public partial class Loader : Base { - protected override System.Object ImportSensor(in SDF.Sensor item, in System.Object parentObject) + protected override System.Object ImportSensor(in SDF.Sensor sensor, in System.Object parentObject) { // Console.WriteLine("[Sensor] {0}", item.Name); var targetObject = (parentObject as UE.GameObject); - Device sensor = null; + Device device = null; - var sensorType = item.Type; + var sensorType = sensor.Type; switch (sensorType) { case "lidar": case "ray": case "gpu_ray": - var ray = item.GetSensor() as SDF.Ray; - sensor = Implement.Sensor.AddLidar(ray, targetObject); + var ray = sensor.GetSensor() as SDF.Ray; + device = Implement.Sensor.AddLidar(ray, targetObject); break; case "depth": - var depthCamera = item.GetSensor() as SDF.Camera; - sensor = Implement.Sensor.AddDepthCamera(depthCamera, targetObject); + var depthCamera = sensor.GetSensor() as SDF.Camera; + device = Implement.Sensor.AddDepthCamera(depthCamera, targetObject); break; case "camera": - var camera = item.GetSensor() as SDF.Camera; - sensor = Implement.Sensor.AddCamera(camera, targetObject); + var camera = sensor.GetSensor() as SDF.Camera; + device = Implement.Sensor.AddCamera(camera, targetObject); break; case "multicamera": - var cameras = item.GetSensor() as SDF.Cameras; - sensor = Implement.Sensor.AddMultiCamera(cameras, targetObject); + var cameras = sensor.GetSensor() as SDF.Cameras; + device = Implement.Sensor.AddMultiCamera(cameras, targetObject); break; case "imu": - var imu = item.GetSensor() as SDF.IMU; - sensor = Implement.Sensor.AddImu(imu, targetObject); + var imu = sensor.GetSensor() as SDF.IMU; + device = Implement.Sensor.AddImu(imu, targetObject); break; case "sonar": - var sonar = item.GetSensor() as SDF.Sonar; - sensor = Implement.Sensor.AddSonar(sonar, targetObject); + var sonar = sensor.GetSensor() as SDF.Sonar; + device = Implement.Sensor.AddSonar(sonar, targetObject); break; case "gps": - var gps = item.GetSensor() as SDF.GPS; - sensor = Implement.Sensor.AddGps(gps, targetObject); + var gps = sensor.GetSensor() as SDF.GPS; + device = Implement.Sensor.AddGps(gps, targetObject); break; case "contact": - var contact = item.GetSensor() as SDF.Contact; - sensor = Implement.Sensor.AddContact(contact, targetObject); + var contact = sensor.GetSensor() as SDF.Contact; + device = Implement.Sensor.AddContact(contact, targetObject); break; case "air_pressure": @@ -77,7 +77,7 @@ protected override System.Object ImportSensor(in SDF.Sensor item, in System.Obje case "rfidtag": case "rfid": case "transceiver": - Debug.LogWarningFormat("[Sensor] Not supported sensor name({0}) type({1})!!!!!", item.Name, sensorType); + Debug.LogWarningFormat("[Sensor] Not supported sensor name({0}) type({1})!!!!!", sensor.Name, sensorType); break; default: @@ -85,18 +85,19 @@ protected override System.Object ImportSensor(in SDF.Sensor item, in System.Obje break; } - if (sensor) + if (device) { - sensor.SetUpdateRate((float)item.UpdateRate()); - sensor.EnableVisualize = item.Visualize(); - var newSensorObject = sensor.gameObject; + device.SetUpdateRate((float)sensor.UpdateRate()); + device.EnableVisualize = sensor.Visualize(); + + var newSensorObject = device.gameObject; if (newSensorObject != null) { newSensorObject.tag = "Sensor"; - newSensorObject.name = item.Name; - newSensorObject.transform.localPosition += SDF2Unity.GetPosition(item.Pose.Pos); - newSensorObject.transform.localRotation *= SDF2Unity.GetRotation(item.Pose.Rot); + newSensorObject.name = sensor.Name; + newSensorObject.transform.localPosition += SDF2Unity.GetPosition(sensor.Pose.Pos); + newSensorObject.transform.localRotation *= SDF2Unity.GetRotation(sensor.Pose.Rot); #if UNITY_EDITOR SceneVisibilityManager.instance.ToggleVisibility(newSensorObject, true); SceneVisibilityManager.instance.DisablePicking(newSensorObject, true); diff --git a/Assets/Scripts/Tools/SDF/Parser/SDF.Model.cs b/Assets/Scripts/Tools/SDF/Parser/SDF.Model.cs index e7bf7702..bf6ca8a5 100644 --- a/Assets/Scripts/Tools/SDF/Parser/SDF.Model.cs +++ b/Assets/Scripts/Tools/SDF/Parser/SDF.Model.cs @@ -70,10 +70,12 @@ public List GetModels() { return models.GetData(); } + public List GetLinks() { return links.GetData(); } + public List GetJoints() { return joints.GetData(); diff --git a/Assets/Scripts/Tools/SDF/Parser/SDF.Root.cs b/Assets/Scripts/Tools/SDF/Parser/SDF.Root.cs index 1a6b916c..1f90dae6 100644 --- a/Assets/Scripts/Tools/SDF/Parser/SDF.Root.cs +++ b/Assets/Scripts/Tools/SDF/Parser/SDF.Root.cs @@ -34,6 +34,8 @@ public class Root private string worldFileName = string.Empty; + private SimulationDisplay simulationDisplay = null; + public Root() : this("") { @@ -46,6 +48,11 @@ public Root(string filename) SetWorldFileName(filename); } + public void SetTargetLogOutput(in SimulationDisplay target) + { + simulationDisplay = target; + } + public void SetWorldFileName(in string filename) { if (!string.IsNullOrEmpty(filename)) @@ -319,7 +326,9 @@ private XmlNode GetIncludedModel(XmlNode _node) } catch (XmlException e) { - Console.WriteLine("Failed to Load included model(" + modelName + ") file - " + e.Message); + var errorMessage = "Failed to Load included model(" + modelName + ") file - " + e.Message; + Console.WriteLine(errorMessage); + simulationDisplay?.SetEventMessage(errorMessage); return null; } diff --git a/Assets/Scripts/Tools/SDF/Parser/SDF.Sensor.Parse.cs b/Assets/Scripts/Tools/SDF/Parser/SDF.Sensor.Parse.cs index 9aca59ed..6e9baa77 100644 --- a/Assets/Scripts/Tools/SDF/Parser/SDF.Sensor.Parse.cs +++ b/Assets/Scripts/Tools/SDF/Parser/SDF.Sensor.Parse.cs @@ -45,9 +45,12 @@ private Ray ParseRay() } } - ray.range_min = GetValue("ray/range/min"); - ray.range_max = GetValue("ray/range/max"); ; - ray.range_resolution = GetValue("ray/range/resolution"); ; + if (IsValidNode("ray/range")) + { + ray.range.min = GetValue("ray/range/min"); + ray.range.max = GetValue("ray/range/max"); ; + ray.range.resolution = GetValue("ray/range/resolution"); + } if (IsValidNode("ray/noise")) { diff --git a/Assets/Scripts/Tools/SDF/Parser/SDF.SensorType.cs b/Assets/Scripts/Tools/SDF/Parser/SDF.SensorType.cs index 64599f25..1e36ed8f 100644 --- a/Assets/Scripts/Tools/SDF/Parser/SDF.SensorType.cs +++ b/Assets/Scripts/Tools/SDF/Parser/SDF.SensorType.cs @@ -204,12 +204,16 @@ public Scan(uint samples = 640) public double max_angle = 0.0; } + public class Range + { + public double min = 0; + public double max = 0; + public double resolution = 0; + } + public Scan horizontal = new Scan(640); public Scan vertical = new Scan(1); - - public double range_min = 0.0; - public double range_max = 0.0; - public double range_resolution = 0.0; + public Range range = new Range(); public string noise_type = "gaussian"; public double noise_mean = 0.0; diff --git a/Assets/Scripts/UI/ObjectSpawning.cs b/Assets/Scripts/UI/ObjectSpawning.cs index 52ef09d1..a9a12bef 100644 --- a/Assets/Scripts/UI/ObjectSpawning.cs +++ b/Assets/Scripts/UI/ObjectSpawning.cs @@ -189,11 +189,15 @@ private Transform GetTransformOnClick() void DrawShadow(in Rect rect, in string value) { - GUI.color = new Color(0, 0, 0, 0.64f); + var prevColor = GUI.skin.label.normal.textColor; + + GUI.skin.label.normal.textColor = new Color(0, 0, 0, 0.64f); var rectShadow = rect; rectShadow.x += 1; rectShadow.y += 1; GUI.Label(rectShadow, value); + + GUI.skin.label.normal.textColor = prevColor; } private string prevScaleFactorString; @@ -201,14 +205,16 @@ void DrawShadow(in Rect rect, in string value) private bool doCheckScaleFactorValue = false; void OnGUI() { + var originLabelColor = GUI.skin.label.normal.textColor; + GUI.skin.label.alignment = TextAnchor.MiddleRight; GUI.skin.label.fontSize = labelFontSize; - GUI.skin.textField.alignment = TextAnchor.MiddleCenter; + GUI.skin.label.alignment = TextAnchor.MiddleCenter; var centerPointX = Screen.width / 2; var rectToolbar = new Rect(centerPointX - toolbarWidth / 2, topMargin, toolbarWidth, guiHeight); - GUI.color = Color.white; + GUI.skin.label.normal.textColor = Color.white; toolbarSelected = GUI.Toolbar(rectToolbar, toolbarSelected, toolbarStrings); var rectToolbarLabel = rectToolbar; @@ -216,20 +222,20 @@ void OnGUI() rectToolbarLabel.width = 45; DrawShadow(rectToolbarLabel, "Props: "); - GUI.color = Color.white; + GUI.skin.label.normal.textColor = Color.white; GUI.Label(rectToolbarLabel, "Props: "); var rectScaleLabel = rectToolbar; rectScaleLabel.x += (toolbarWidth + 5); rectScaleLabel.width = 50; DrawShadow(rectScaleLabel, "Scale: "); - GUI.color = Color.white; + GUI.skin.label.normal.textColor = Color.white; GUI.Label(rectScaleLabel, "Scale: "); var rectScale = rectScaleLabel; rectScale.x += 50; rectScale.width = 40; - GUI.color = Color.white; + GUI.skin.label.normal.textColor = Color.white; GUI.SetNextControlName("ScaleField"); scaleFactorString = GUI.TextField(rectScale, scaleFactorString, 5); @@ -273,5 +279,7 @@ void OnGUI() } doCheckScaleFactorValue = false; } + + GUI.skin.label.normal.textColor = originLabelColor; } } \ No newline at end of file diff --git a/Assets/Scripts/UI/SimulationDisplay.cs b/Assets/Scripts/UI/SimulationDisplay.cs index dd2a8df9..bb04249b 100644 --- a/Assets/Scripts/UI/SimulationDisplay.cs +++ b/Assets/Scripts/UI/SimulationDisplay.cs @@ -30,11 +30,13 @@ public class SimulationDisplay : MonoBehaviour private const int textWidthFps = 80; private const int textWidthVersion = 50; private const int textWidthSimulation = 600; + private const int textWidthEvent = 600; [Header("Rect")] private Rect rectVersion = new Rect(textLeftMargin, textTopMargin, textWidthVersion, textHeight); private Rect rectFps = new Rect(Screen.width - textWidthFps - textLeftMargin, textTopMargin, textWidthFps, textHeight); private Rect rectSimulationinfo = new Rect(textLeftMargin, Screen.height - textHeight - textTopMargin, textWidthSimulation, textHeight); + private Rect rectEventMessage = new Rect(textLeftMargin, Screen.height - (textHeight*2) - textTopMargin, textWidthEvent, textHeight); // Start is called before the first frame update void Awake() @@ -95,40 +97,55 @@ private string GetBoldText(in string value) void DrawShadow(in Rect rect, in string value) { - GUI.color = new Color(0, 0, 0, 0.34f); + var prevColor = GUI.skin.label.normal.textColor; + + GUI.skin.label.normal.textColor = new Color(0, 0, 0, 0.34f); var rectShadow = rect; rectShadow.x += 1; rectShadow.y += 1; GUI.Label(rectShadow, value); + + GUI.skin.label.normal.textColor = prevColor; } void OnGUI() { + var originLabelColor = GUI.skin.label.normal.textColor; + GUI.skin.label.alignment = TextAnchor.MiddleLeft; GUI.skin.label.fontSize = labelFontSize; + GUI.skin.label.wordWrap = true; // version info var versionString = GetBoldText(Application.version); DrawShadow(rectVersion, versionString); - GUI.color = Color.green; + GUI.skin.label.normal.textColor = Color.green; GUI.Label(rectVersion, versionString); - // Simulation time info or event message - rectSimulationinfo.y = Screen.height - textHeight - textTopMargin; - - var simulationInfo = (string.IsNullOrEmpty(eventMessage)) ? GetTimeInfoString() : eventMessage; + // Simulation time info + var simulationInfo = GetTimeInfoString(); DrawShadow(rectSimulationinfo, simulationInfo); - GUI.color = Color.black; + GUI.skin.label.normal.textColor = Color.black; GUI.Label(rectSimulationinfo, simulationInfo); - // fps info - GUI.skin.label.alignment = TextAnchor.MiddleRight; + // error message or event message + var originLabelSkin = GUI.skin.label; + + GUI.skin.label.wordWrap = false; + GUI.skin.label.clipping = TextClipping.Overflow; + DrawShadow(rectEventMessage, eventMessage); + GUI.skin.label.normal.textColor = Color.red; + GUI.Label(rectEventMessage, eventMessage); - rectFps.x = Screen.width - textWidthFps - textLeftMargin; + GUI.skin.label = originLabelSkin; + // fps info + GUI.skin.label.alignment = TextAnchor.MiddleRight; var fpsString = "FPS [" + GetBoldText(Mathf.Round(fps).ToString("F1")) + "]"; DrawShadow(rectFps, fpsString); - GUI.color = Color.cyan; + GUI.skin.label.normal.textColor = Color.cyan; GUI.Label(rectFps, fpsString); + + GUI.skin.label.normal.textColor = originLabelColor; } } \ No newline at end of file diff --git a/Packages/manifest.json b/Packages/manifest.json index affd69dd..332651b9 100644 --- a/Packages/manifest.json +++ b/Packages/manifest.json @@ -2,7 +2,7 @@ "dependencies": { "com.unity.ide.vscode": "1.2.3", "com.unity.mathematics": "1.2.1", - "com.unity.render-pipelines.core": "10.3.1", + "com.unity.render-pipelines.core": "10.3.2", "com.unity.searcher": "4.3.1", "com.unity.textmeshpro": "3.0.4", "com.unity.toolchain.linux-x86_64": "0.1.18-preview", diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 78466a5d..7faf0015 100644 --- a/Packages/packages-lock.json +++ b/Packages/packages-lock.json @@ -15,7 +15,7 @@ "url": "https://packages.unity.com" }, "com.unity.render-pipelines.core": { - "version": "10.3.1", + "version": "10.3.2", "depth": 0, "source": "registry", "dependencies": { diff --git a/ProjectSettings/DynamicsManager.asset b/ProjectSettings/DynamicsManager.asset index 3c0e6c1a..0d071498 100644 --- a/ProjectSettings/DynamicsManager.asset +++ b/ProjectSettings/DynamicsManager.asset @@ -10,8 +10,8 @@ PhysicsManager: m_DefaultMaxDepenetrationVelocity: 10 m_SleepThreshold: 0.005 m_DefaultContactOffset: 0.001 - m_DefaultSolverIterations: 15 - m_DefaultSolverVelocityIterations: 10 + m_DefaultSolverIterations: 10 + m_DefaultSolverVelocityIterations: 5 m_QueriesHitBackfaces: 0 m_QueriesHitTriggers: 1 m_EnableAdaptiveForce: 1 diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 64cd5ee3..4232a294 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -127,7 +127,7 @@ PlayerSettings: 16:10: 1 16:9: 1 Others: 1 - bundleVersion: 2.0.0 + bundleVersion: 2.0.1 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0 diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index aa2fd82e..3bdba153 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2020.2.6f1 -m_EditorVersionWithRevision: 2020.2.6f1 (8a2143876886) +m_EditorVersion: 2020.2.7f1 +m_EditorVersionWithRevision: 2020.2.7f1 (c53830e277f1)