From 2aa219c9381d19a619bb678a12483d1391bd2f0d Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Dec 2024 13:59:48 +0900 Subject: [PATCH 1/8] Revert "Fix scale direction in ToUnityMeshObject() of MeshLoader" This reverts commit 9c51732ca113763e91fc6d4c555e4175e76f01b9. --- Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs b/Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs index 67c67d06..58e4ea8a 100644 --- a/Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs +++ b/Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs @@ -368,11 +368,6 @@ private static MeshMaterialList LoadMeshes(in List sceneMeshes) return meshMatList; } - private static Vector3 ToUnityScale(in Vector3 value) - { - return new Vector3(value.z, value.x, value.y); - } - private static GameObject ToUnityMeshObject( this Assimp.Node node, in MeshMaterialList meshMatList, @@ -405,14 +400,9 @@ private static GameObject ToUnityMeshObject( // Convert Assimp transfrom into Unity transform var nodeTransformMatrix = node.Transform.ToUnity(); - nodeObject.transform.localPosition = nodeTransformMatrix.GetPosition(); nodeObject.transform.localRotation = nodeTransformMatrix.rotation; - nodeObject.transform.localScale = ToUnityScale(nodeTransformMatrix.lossyScale); - - // Debug.Log("ToUnityMeshObject : " + node.Name + ", " + nodeTransformMatrix.GetPosition() ); - // Debug.Log("ToUnityMeshObject : " + node.Name + ", " + nodeTransformMatrix.rotation ); - // Debug.Log("ToUnityMeshObject : " + node.Name + ", " + nodeTransformMatrix.lossyScale ); + nodeObject.transform.localScale = nodeTransformMatrix.lossyScale; doFlip = (nodeObject.transform.localScale.x < 0 || nodeObject.transform.localScale.y < 0 || From 93291354177e9742c2a50f66cb37d3b36a8c75a3 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Dec 2024 14:00:12 +0900 Subject: [PATCH 2/8] Modify MeshLoader.ToUnity() in Assimp.Common - scaling problem reported with dae collada mesh --- Assets/Scripts/Tools/Mesh/Assimp.Common.cs | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/Assets/Scripts/Tools/Mesh/Assimp.Common.cs b/Assets/Scripts/Tools/Mesh/Assimp.Common.cs index 1d532e84..dace477c 100644 --- a/Assets/Scripts/Tools/Mesh/Assimp.Common.cs +++ b/Assets/Scripts/Tools/Mesh/Assimp.Common.cs @@ -170,11 +170,6 @@ private static Matrix4x4 ToUnity(this Assimp.Matrix4x4 assimpMatrix) var newScale = new Vector3(scale.y, scale.z, scale.x); scale = newScale; } - else if (!isRotZeroX && !isRotZeroY && isRotZeroZ) - { - var newScale = new Vector3(scale.z, scale.x, scale.y); - scale = newScale; - } else if (isRotZeroX && !isRotZeroY && isRotZeroZ && !Mathf.Approximately(rot.eulerAngles.y, 180f)) { @@ -187,7 +182,17 @@ private static Matrix4x4 ToUnity(this Assimp.Matrix4x4 assimpMatrix) var newScale = new Vector3(scale.y, scale.x, scale.z); scale = newScale; } - // Debug.Log($"new scaling={scale.x} {scale.y} {scale.z} rot={rot.eulerAngles}"); + else if (!isRotZeroX && isRotZeroY && isRotZeroZ) + { + var newScale = new Vector3(scale.x, scale.z, scale.y); + scale = newScale; + } + else if (!isRotZeroX && !isRotZeroY && isRotZeroZ) + { + var newScale = new Vector3(scale.z, scale.x, scale.y); + scale = newScale; + } + // Debug.Log($"new isRotZero={isRotZeroX}/{isRotZeroY}/{isRotZeroZ} scaling={scale.x} {scale.y} {scale.z} rot={rot.eulerAngles}"); #endregion return Matrix4x4.TRS(pos, rot, scale); From c795488df518f1b708483fcacfe15437362e6008 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Dec 2024 14:16:52 +0900 Subject: [PATCH 3/8] Modify GetInertiaTensor() in SDF.Import.Loader - set inertiaTensor rotation which calculated automatically by articulation body. --- .../Scripts/Tools/SDF/Import/Import.Link.cs | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index 5e177f37..8e8b70f5 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs @@ -15,7 +15,7 @@ public partial class Loader : Base { private static readonly float MinimumInertiaTensor = 1e-6f; - private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia) + private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia, in UE.ArticulationBody tempArticulationBodyForCalculation) { /** * Inertia Tensor @@ -27,13 +27,6 @@ private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia) var inertiaMomentum = UE.Pose.identity; var inertiaVector = SDF2Unity.Scalar((float)inertia?.ixx, (float)inertia?.iyy, (float)inertia?.izz); - /* - * Unity’s ArticulationBody does not directly expose off-diagonal components of the inertia tensor(Ixy, Ixz, Iyz). - * If these are needed, you might have to approximate them by adjusting inertiaTensorRotation, - * which changes the orientation of the inertia tensor. - */ - // var inertiaRotationVector = SDF2Unity.Scalar((float)inertia?.ixy, (float)inertia?.iyz, (float)inertia?.ixz); - for (var index = 0; index < 3; index++) { if (inertiaVector[index] <= MinimumInertiaTensor) @@ -42,9 +35,23 @@ private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia) } } + /* + * Unity’s ArticulationBody does not directly expose off-diagonal components of the inertia tensor(Ixy, Ixz, Iyz). + * If these are needed, you might have to approximate them by adjusting inertiaTensorRotation, + * which changes the orientation of the inertia tensor. + */ + // var inertiaRotationVector = SDF2Unity.Scalar((float)inertia?.ixy, (float)inertia?.iyz, (float)inertia?.ixz); + inertiaMomentum.position = inertiaVector; // inertiaMomentum.rotation = UE.Quaternion.Euler(inertiaRotationVector.x, inertiaRotationVector.y, inertiaRotationVector.z); + #region Temporary Code for intertia tensor rotation + tempArticulationBodyForCalculation.automaticInertiaTensor = true; + // UE.Debug.LogWarning($"{tempArticulationBodyForCalculation.name} Inertia Tensor: {tempArticulationBodyForCalculation.inertiaTensor}, {tempArticulationBodyForCalculation.inertiaTensorRotation.eulerAngles}"); + inertiaMomentum.rotation = tempArticulationBodyForCalculation.inertiaTensorRotation; + tempArticulationBodyForCalculation.automaticInertiaTensor = false; + #endregion + // Debug.Log("Inertia Tensor: " + inertiaMomentum.position + ", " + inertiaMomentum.rotation.eulerAngles); return inertiaMomentum; } @@ -175,7 +182,7 @@ private static UE.ArticulationBody CreateArticulationBody(in UE.GameObject linkO articulationBody.ResetInertiaTensor(); if (inertial?.inertia != null) { - var momentum = GetInertiaTensor(inertial?.inertia); + var momentum = GetInertiaTensor(inertial?.inertia, articulationBody); articulationBody.inertiaTensor = momentum.position; articulationBody.inertiaTensorRotation = momentum.rotation; articulationBody.automaticInertiaTensor = false; From d0f0231fecb49e9a47083e997dd36c202ac6a297 Mon Sep 17 00:00:00 2001 From: "Hyunseok Yang (YG)" Date: Wed, 4 Dec 2024 18:55:50 +0900 Subject: [PATCH 4/8] Modify ControlHipAndLeg() in SelfBalanceControl class - hip angle is double of leg angle Modify MicomCommand - adjust height control angle --- Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs | 12 ++-- Assets/Scripts/Devices/MicomCommand.cs | 6 +- .../Scripts/Devices/Modules/Articulation.cs | 8 +++ .../SelfBalanceControl/SelfBalancedDrive.cs | 69 ++++++++++--------- .../SelfBalanceControl/SlidingModeControl.cs | 1 + 5 files changed, 56 insertions(+), 40 deletions(-) diff --git a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs index bed2177d..06bd8788 100644 --- a/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs +++ b/Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs @@ -168,6 +168,12 @@ private void SetSelfBalancedWheel(in string parameterPrefix) } _log.AppendLine($"AutoStart: {autostart}"); + var headJoint = GetPluginParameters().GetValue($"{parameterPrefix}/head/joint"); + if (!string.IsNullOrEmpty(headJoint)) + { + (_motorControl as SelfBalancedDrive).SetHeadJoint(headJoint); + } + var hipJointLeft = GetPluginParameters().GetValue($"{parameterPrefix}/hip/joint[@type='left']"); var hipJointRight = GetPluginParameters().GetValue($"{parameterPrefix}/hip/joint[@type='right']"); if (!string.IsNullOrEmpty(hipJointLeft) && !string.IsNullOrEmpty(hipJointRight)) @@ -182,12 +188,6 @@ private void SetSelfBalancedWheel(in string parameterPrefix) (_motorControl as SelfBalancedDrive).SetLegJoints(legJointLeft, legJointRight); } - var headJoint = GetPluginParameters().GetValue($"{parameterPrefix}/head/joint"); - if (!string.IsNullOrEmpty(headJoint)) - { - (_motorControl as SelfBalancedDrive).SetHeadJoint(headJoint); - } - var bodyJoint = GetPluginParameters().GetValue($"{parameterPrefix}/body/joint"); if (!string.IsNullOrEmpty(bodyJoint)) { diff --git a/Assets/Scripts/Devices/MicomCommand.cs b/Assets/Scripts/Devices/MicomCommand.cs index 6b44bc92..de87056e 100644 --- a/Assets/Scripts/Devices/MicomCommand.cs +++ b/Assets/Scripts/Devices/MicomCommand.cs @@ -218,6 +218,8 @@ private void ControlMowing(in string target, in cloisim.msgs.Any value) #if UNITY_EDITOR #region Constant for SelfBalancedDrive private const float HeadsetRotationUnit = 1f; // deg + private const float RollRotationUnitKeyboard = 0.01f; // deg + private const float HeightMovementUnitKeyboard = 0.25f; // deg #endregion void LateUpdate() @@ -263,11 +265,11 @@ void LateUpdate() if (Input.GetKey(KeyCode.UpArrow)) { - balancedDrive.HeightTarget -= HeightMovementUnit; + balancedDrive.HeightTarget -= HeightMovementUnitKeyboard; } else if (Input.GetKey(KeyCode.DownArrow)) { - balancedDrive.HeightTarget += HeightMovementUnit; + balancedDrive.HeightTarget += HeightMovementUnitKeyboard; } // Debug.Log($"HeightTarget={balancedDrive.HeightTarget}"); } diff --git a/Assets/Scripts/Devices/Modules/Articulation.cs b/Assets/Scripts/Devices/Modules/Articulation.cs index 3004f902..98fce316 100644 --- a/Assets/Scripts/Devices/Modules/Articulation.cs +++ b/Assets/Scripts/Devices/Modules/Articulation.cs @@ -199,6 +199,14 @@ public void SetJointForce(in float force, in int targetDegree = 0) } } + public void SetJointFriction(in float friction) + { + if (_jointBody != null) + { + _jointBody.jointFriction = friction; + } + } + /// radian for angular and meter for linear public float GetDriveTarget(in int targetDegree = 0) { diff --git a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs index 7c8e9873..30870343 100644 --- a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs +++ b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs @@ -200,18 +200,18 @@ public void SetBodyJoint(in string targetJointName) ChangeDriveType(Location.BODY, ArticulationDriveType.Target); } - public void SetHipJoints(in string hipJointLeft, in string hipJointright) + public void SetHipJoints(in string targetLeft, in string targetRight) { - AttachMotor(Location.HIP_LEFT, hipJointLeft); - AttachMotor(Location.HIP_RIGHT, hipJointright); + AttachMotor(Location.HIP_LEFT, targetLeft); + AttachMotor(Location.HIP_RIGHT, targetRight); ChangeDriveType(Location.HIP_LEFT, ArticulationDriveType.Target); ChangeDriveType(Location.HIP_RIGHT, ArticulationDriveType.Target); } - public void SetLegJoints(in string legJointLeft, in string legJointright) + public void SetLegJoints(in string targetLeft, in string targetRight) { - AttachMotor(Location.LEG_LEFT, legJointLeft); - AttachMotor(Location.LEG_RIGHT, legJointright); + AttachMotor(Location.LEG_LEFT, targetLeft); + AttachMotor(Location.LEG_RIGHT, targetRight); ChangeDriveType(Location.LEG_LEFT, ArticulationDriveType.Target); ChangeDriveType(Location.LEG_RIGHT, ArticulationDriveType.Target); } @@ -224,8 +224,9 @@ public void ChangeWheelDriveType() public override void Drive(in float linearVelocity, in float angularVelocity) { + const float BoostAngularSpeed = 3.0f; _commandTwistLinear = linearVelocity; - _commandTwistAngular = SDF2Unity.CurveOrientationAngle(angularVelocity); + _commandTwistAngular = SDF2Unity.CurveOrientationAngle(angularVelocity) * BoostAngularSpeed; if (Math.Abs(_commandTwistLinear) < float.Epsilon || Math.Abs(_commandTwistAngular) < float.Epsilon) { @@ -290,16 +291,14 @@ private void AdjustHeadsetByPitch(in double currentPitch, in float duration) // Debug.LogWarning("Adjusting head by pitch"); } - private void ControlHipAndLeg(in double currentRoll) + private void ControlHipAndLeg() { - const float BodyUpperGain = 0.9f; - const float BodyLowerGain = 1.4f; + var hipTarget = _commandTargetHeight * 0.5; + + _commandHipTarget.x = hipTarget; + _commandHipTarget.y = hipTarget; - _commandTargetBody = _commandTargetHeight * ((_commandTargetHeight >= 0) ? BodyUpperGain : BodyLowerGain); - // Debug.Log($"_commandTargetHeight: {_commandTargetHeight} _commandTargetBody: {_commandTargetBody}"); - - _commandHipTarget.x = _commandTargetHeight; - _commandHipTarget.y = _commandTargetHeight; + _commandTargetBody = _commandTargetHeight; _commandLegTarget.x = _commandTargetHeight; _commandLegTarget.y = _commandTargetHeight; @@ -350,7 +349,7 @@ private Vector3 GetOrientation(SensorDevices.IMU imuSensor) private void SetWheelEfforts(in Vector2d efforts) { - // Debug.Log($"Effort: {efforts}"); + // Debug.Log($"Effort: {efforts}"); _motorList[Location.FRONT_WHEEL_LEFT]?.SetJointForce((float)Unity2SDF.Direction.Curve(efforts.x)); _motorList[Location.FRONT_WHEEL_RIGHT]?.SetJointForce((float)Unity2SDF.Direction.Curve(efforts.y)); } @@ -376,6 +375,9 @@ private void SetJoints(in VectorXd targets) _motorList[Location.LEG_LEFT]?.Drive(targetPosition: (float)targets[4]); _motorList[Location.LEG_RIGHT]?.Drive(targetPosition: (float)targets[5]); } + + _motorList[Location.FRONT_WHEEL_LEFT]?.Drive(targetPosition: (float)Unity2SDF.Direction.Curve(targets[2])); + _motorList[Location.FRONT_WHEEL_RIGHT]?.Drive(targetPosition: (float)Unity2SDF.Direction.Curve(targets[3])); } private VectorXd GetTargetReferences(in float duration) @@ -462,6 +464,20 @@ private void ProcessBalancing(in Vector2 wheelVelocity, in float duration, Vecto _resetPose= false; } + #region Body Pose Control + if (Math.Abs(_commandTargetRollByDrive) > float.Epsilon) + { + ControlSmoothRollTarget(duration); + } + else if ((_doControlRollHeightByCommandTimeout -= duration) < float.Epsilon) + { + RestoreHipAndLegZero(duration); + } + + ControlHipAndLeg(); + #endregion + + #region Self-Balanced Control var wipStates = _kinematics.ComputeStates(wheelVelocityLeft, wheelVelocityRight, yaw, pitch, roll, duration); if (_doUpdatePitchProfiler) @@ -489,32 +505,21 @@ private void ProcessBalancing(in Vector2 wheelVelocity, in float duration, Vecto } } - if (Math.Abs(_commandTargetRollByDrive) > float.Epsilon) - { - ControlSmoothRollTarget(duration); - } - else if ((_doControlRollHeightByCommandTimeout -= duration) < float.Epsilon) - { - RestoreHipAndLegZero(duration); - } - - ControlHipAndLeg(roll); - if ((_doControlHeadsetByCommandTimeout -= duration) < float.Epsilon) { AdjustHeadsetByPitch(wipStates[3], duration); } - var wipEfforts = _smc.ComputeControl(wipStates, wipReferences, duration); - - SetWheelEfforts(wipEfforts); - var jointTargets = new VectorXd(new double[] { _commandTargetHeadset, _commandTargetBody, _commandHipTarget.x, _commandHipTarget.y, - _commandLegTarget.x, _commandLegTarget.y, + _commandLegTarget.x, _commandLegTarget.y }); SetJoints(jointTargets); + + var wipEfforts = _smc.ComputeControl(wipStates, wipReferences, duration); + SetWheelEfforts(wipEfforts); + #endregion } } \ No newline at end of file diff --git a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SlidingModeControl.cs b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SlidingModeControl.cs index 3c03ec2c..9bdcf409 100644 --- a/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SlidingModeControl.cs +++ b/Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SlidingModeControl.cs @@ -168,6 +168,7 @@ public Vector2d ComputeControl(in VectorXd states, in VectorXd references, in do _uEQ = _uLQ - (_nominalModel.SxB).Inverse * _nominalModel.S * _f; // UnityEngine.Debug.Log($"states: {states.ToString("F4")} | references: {references.ToString("F4")} | Delta: {delta.ToString("F4")} | K: {_nominalModel.K.ToString("F4")} | uLQ({_uLQ.ToString("F4")})"); + // UnityEngine.Debug.Log($"Delta: {delta.ToString("F4")}"); // UnityEngine.Debug.Log($"states: {states.ToString("F4")} | references: {references.ToString("F4")} | Delta: {delta.ToString("F4")} | uLQ({_uLQ.ToString("F4")})"); // UnityEngine.Debug.Log($"K: {_nominalModel.K} | f: {_f} | uLQ({_uLQ}) | uEQ({_uEQ})"); // UnityEngine.Debug.Log($"uLQ({_uLQ}) | K: {_nominalModel.K} | Delta: {delta}"); From 887ad4ce8400de8e6b56ec801f2252006c9d536d Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Mon, 9 Dec 2024 23:21:26 +0900 Subject: [PATCH 5/8] Upgrade Unity editor version -> 2022.3.54f1 (LTS) --- ProjectSettings/ProjectVersion.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index ba6dfe72..2b775c52 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.53f1 -m_EditorVersionWithRevision: 2022.3.53f1 (df4e529d20d3) +m_EditorVersion: 2022.3.54f1 +m_EditorVersionWithRevision: 2022.3.54f1 (129125d4e700) From db09438221fdc0e2027ecd1b1235d71712208818 Mon Sep 17 00:00:00 2001 From: Hyunseok Yang Date: Mon, 9 Dec 2024 23:23:05 +0900 Subject: [PATCH 6/8] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index bdeefe83..3c138283 100644 --- a/README.md +++ b/README.md @@ -151,7 +151,7 @@ if `` element of `