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..2506aff2 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.15f; // deg + private const float HeightMovementUnitKeyboard = 0.15f; // deg #endregion void LateUpdate() @@ -253,21 +255,21 @@ void LateUpdate() { if (Input.GetKey(KeyCode.LeftArrow)) { - balancedDrive.RollTarget -= RollRotationUnit; + balancedDrive.RollTarget -= RollRotationUnitKeyboard; } else if (Input.GetKey(KeyCode.RightArrow)) { - balancedDrive.RollTarget += RollRotationUnit; + balancedDrive.RollTarget += RollRotationUnitKeyboard; } // Debug.Log($"RollTarget={balancedDrive.RollTarget}"); 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..c577179e 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,18 @@ private void AdjustHeadsetByPitch(in double currentPitch, in float duration) // Debug.LogWarning("Adjusting head by pitch"); } - private void ControlHipAndLeg(in double currentRoll) + private double adjustBody = 1.84; + + private VectorXd ControlHipAndLeg(in double currentPitch) { - const float BodyUpperGain = 0.9f; - const float BodyLowerGain = 1.4f; + var hipTarget = _commandTargetHeight * 0.5; - _commandTargetBody = _commandTargetHeight * ((_commandTargetHeight >= 0) ? BodyUpperGain : BodyLowerGain); - // Debug.Log($"_commandTargetHeight: {_commandTargetHeight} _commandTargetBody: {_commandTargetBody}"); + // Debug.Log($"{currentPitch} {hipTarget} | {hipTarget * 0.5} | {hipTarget * 0.8} | {hipTarget * 1.1} | {hipTarget * 1.3} | {hipTarget * 1.5} "); + _commandHipTarget.x = hipTarget; + _commandHipTarget.y = hipTarget; - _commandHipTarget.x = _commandTargetHeight; - _commandHipTarget.y = _commandTargetHeight; + _commandTargetBody = hipTarget * adjustBody; + // Debug.Log($"{hipTarget} {_commandTargetBody} "); _commandLegTarget.x = _commandTargetHeight; _commandLegTarget.y = _commandTargetHeight; @@ -309,12 +312,19 @@ private void ControlHipAndLeg(in double currentRoll) _commandLegTarget.x += -_commandTargetRoll; _commandLegTarget.y += _commandTargetRoll; + + return new VectorXd(new double[] { + _commandTargetHeadset, + _commandTargetBody, + _commandHipTarget.x, _commandHipTarget.y, + _commandLegTarget.x, _commandLegTarget.y + }); } private float _smoothControlTime = 0; private double _prevCommandTargetRollByDrive = 0; - private void ControlSmoothRollTarget(in float duration) + private void ControlSmoothRollTarget() { const float smoothTimeDiff = 0.00005f; // Debug.Log($"_commandTwistLinear: {_commandTwistLinear} _kinematics.OdomTranslationalVelocity: {_kinematics.OdomTranslationalVelocity}"); @@ -333,10 +343,11 @@ private void ControlSmoothRollTarget(in float duration) } } - private void RestoreHipAndLegZero(in float duration) + private void RestoreHipAndLegZero() { - _commandTargetRoll = Mathf.Lerp((float)_commandTargetRoll, 0, duration); - _commandTargetHeight = Mathf.Lerp((float)_commandTargetHeight, 0, duration); + const float smoothLerpTime = 0.008f; + _commandTargetRoll = Mathf.Lerp((float)_commandTargetRoll, 0, smoothLerpTime); + _commandTargetHeight = Mathf.Lerp((float)_commandTargetHeight, 0, smoothLerpTime); } private Vector3 GetOrientation(SensorDevices.IMU imuSensor) @@ -350,7 +361,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 +387,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 +476,20 @@ private void ProcessBalancing(in Vector2 wheelVelocity, in float duration, Vecto _resetPose= false; } + #region Body Pose Control + if (Math.Abs(_commandTargetRollByDrive) > float.Epsilon) + { + ControlSmoothRollTarget(); + } + else if ((_doControlRollHeightByCommandTimeout -= duration) < float.Epsilon) + { + RestoreHipAndLegZero(); + } + + var jointTargets = ControlHipAndLeg(pitch); + #endregion + + #region Self-Balanced Control var wipStates = _kinematics.ComputeStates(wheelVelocityLeft, wheelVelocityRight, yaw, pitch, roll, duration); if (_doUpdatePitchProfiler) @@ -489,32 +517,15 @@ 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); + SetJoints(jointTargets); + 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, - }); - SetJoints(jointTargets); + #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}"); 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); 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 || 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; diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 2923d86b..07b50378 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -140,7 +140,7 @@ PlayerSettings: loadStoreDebugModeEnabled: 0 visionOSBundleVersion: 1.0 tvOSBundleVersion: 1.0 - bundleVersion: 4.9.3 + bundleVersion: 4.9.4 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0 @@ -186,8 +186,10 @@ PlayerSettings: strictShaderVariantMatching: 0 VertexChannelCompressionMask: -1 iPhoneSdkVersion: 988 + iOSSimulatorArchitecture: 0 iOSTargetOSVersionString: 12.0 tvOSSdkVersion: 0 + tvOSSimulatorArchitecture: 0 tvOSRequireExtendedGameController: 0 tvOSTargetOSVersionString: 12.0 VisionOSSdkVersion: 0 @@ -519,6 +521,7 @@ PlayerSettings: switchSocketBufferEfficiency: 4 switchSocketInitializeEnabled: 1 switchNetworkInterfaceManagerInitializeEnabled: 1 + switchDisableHTCSPlayerConnection: 0 switchUseNewStyleFilepaths: 0 switchUseLegacyFmodPriorities: 1 switchUseMicroSleepForYield: 1 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) 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 `