Skip to content

Commit

Permalink
Merge from 'develop' into 'main' for CLOiSim-4.9.4 (#329)
Browse files Browse the repository at this point in the history
  • Loading branch information
@hyunseok-yang
hyunseok-yang authored Dec 10, 2024
2 parents 88dc41a + 674ec1a commit 54bdba7
Show file tree
Hide file tree
Showing 11 changed files with 106 additions and 79 deletions.
12 changes: 6 additions & 6 deletions Assets/Scripts/CLOiSimPlugins/MicomPlugin.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,6 +168,12 @@ private void SetSelfBalancedWheel(in string parameterPrefix)
}
_log.AppendLine($"AutoStart: {autostart}");

var headJoint = GetPluginParameters().GetValue<string>($"{parameterPrefix}/head/joint");
if (!string.IsNullOrEmpty(headJoint))
{
(_motorControl as SelfBalancedDrive).SetHeadJoint(headJoint);
}

var hipJointLeft = GetPluginParameters().GetValue<string>($"{parameterPrefix}/hip/joint[@type='left']");
var hipJointRight = GetPluginParameters().GetValue<string>($"{parameterPrefix}/hip/joint[@type='right']");
if (!string.IsNullOrEmpty(hipJointLeft) && !string.IsNullOrEmpty(hipJointRight))
Expand All @@ -182,12 +188,6 @@ private void SetSelfBalancedWheel(in string parameterPrefix)
(_motorControl as SelfBalancedDrive).SetLegJoints(legJointLeft, legJointRight);
}

var headJoint = GetPluginParameters().GetValue<string>($"{parameterPrefix}/head/joint");
if (!string.IsNullOrEmpty(headJoint))
{
(_motorControl as SelfBalancedDrive).SetHeadJoint(headJoint);
}

var bodyJoint = GetPluginParameters().GetValue<string>($"{parameterPrefix}/body/joint");
if (!string.IsNullOrEmpty(bodyJoint))
{
Expand Down
10 changes: 6 additions & 4 deletions Assets/Scripts/Devices/MicomCommand.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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}");
}
Expand Down
8 changes: 8 additions & 0 deletions Assets/Scripts/Devices/Modules/Articulation.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}

/// <returns>radian for angular and meter for linear</param>
public float GetDriveTarget(in int targetDegree = 0)
{
Expand Down
89 changes: 50 additions & 39 deletions Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SelfBalancedDrive.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
Expand All @@ -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)
{
Expand Down Expand Up @@ -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;
Expand All @@ -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}");
Expand All @@ -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)
Expand All @@ -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));
}
Expand All @@ -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)
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
}
}
1 change: 1 addition & 0 deletions Assets/Scripts/Devices/Modules/Motor/SelfBalanceControl/SlidingModeControl.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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}");
Expand Down
17 changes: 11 additions & 6 deletions Assets/Scripts/Tools/Mesh/Assimp.Common.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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))
{
Expand All @@ -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);
Expand Down
12 changes: 1 addition & 11 deletions Assets/Scripts/Tools/Mesh/Assimp.Mesh.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -368,11 +368,6 @@ private static MeshMaterialList LoadMeshes(in List<Assimp.Mesh> 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,
Expand Down Expand Up @@ -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 ||
Expand Down
25 changes: 16 additions & 9 deletions Assets/Scripts/Tools/SDF/Import/Import.Link.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand All @@ -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;
}
Expand Down Expand Up @@ -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;
Expand Down
Loading

0 comments on commit 54bdba7

Please sign in to comment.