Fix SDF.Import.Util.SpecifyPose()

- Wrong Pose(rotation/position) calculation
lge-ros2 · Nov 21, 2024 · bfd995d · bfd995d
1 parent 95dd296
commit bfd995d
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 9 deletions.
diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Base.cs b/Assets/Scripts/Tools/SDF/Import/Import.Base.cs
@@ -136,7 +136,6 @@ public IEnumerator<World> Start(World world)
 				yield return null;
 
 				ImportActors(world.GetActors());
-
 				yield return null;
 			}
 
@@ -154,7 +153,6 @@ public IEnumerator<Model> Start(Model model)
 				}
 
 				modelObject.SpecifyPose();
-
 				yield return null;
 			}
 		}

diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Util.cs b/Assets/Scripts/Tools/SDF/Import/Import.Util.cs
@@ -108,8 +108,7 @@ public static void SpecifyPose(this Object targetObject)
 					body.enabled = false;
 				}
 
-				var baseHelpers = rootObject.GetComponentsInChildren<SDF.Helper.Base>();
-				foreach (var baseHelper in baseHelpers)
+				foreach (var baseHelper in rootObject.GetComponentsInChildren<SDF.Helper.Base>())
 				{
 					var pose = baseHelper?.Pose;
 					if (pose == null)
@@ -126,12 +125,13 @@ public static void SpecifyPose(this Object targetObject)
 					if (string.IsNullOrEmpty(pose.relative_to))
 					{
 						var rootModelTransform = FindRootParentModel(baseHelper);
-						// UE.Debug.Log($"SpecifyPose {baseHelper.name}: non relative_to baseHelper: {localRotation.eulerAngles.ToString("F5")}");
-						// UE.Debug.LogWarning($"SpecifyPose {baseHelper.name}: {rootModelTransform.localRotation.eulerAngles.ToString("F6")} * {parentObject.localRotation.eulerAngles.ToString("F6")} *  {localRotation.eulerAngles.ToString("F6")}");
-						// UE.Debug.LogWarning($"SpecifyPose {baseHelper.name}: rootModelTransform === {rootModelTransform.name} <-> {parentObject.name}");
+						// UE.Debug.Log($"SpecifyPose {baseHelper.name}: non relative_to baseHelper: {localRotation.eulerAngles.ToString("F5")} rootModelTransform: {rootModelTransform.name}");
+						// UE.Debug.LogWarning($"SpecifyPose {baseHelper.name}: {rootModelTransform.localRotation.eulerAngles.ToString("F6")} * {parentObject.localRotation.eulerAngles.ToString("F6")} * {localRotation.eulerAngles.ToString("F6")}");
+						// UE.Debug.LogWarning($"SpecifyPose {baseHelper.name}: rootModelTransform == {rootModelTransform.name} <-> {parentObject.name}");
 
+						var rotationOffset = (rootModelTransform.Equals(parentObject)) ? UE.Quaternion.identity : parentObject.localRotation;
 						var positionOffset = (rootModelTransform.Equals(parentObject)) ? UE.Vector3.zero : (parentObject.position - rootModelTransform.position);
-						var rotationOffset = (rootModelTransform.Equals(parentObject))? UE.Quaternion.identity : (rootModelTransform.localRotation * parentObject.localRotation);
+						positionOffset = UE.Quaternion.Inverse(rootModelTransform.localRotation) * positionOffset;
 
 						localRotation = rotationOffset * localRotation;
 						localPosition = localPosition - positionOffset;

diff --git a/Assets/Scripts/Tools/SDF/Parser/World.cs b/Assets/Scripts/Tools/SDF/Parser/World.cs
@@ -245,7 +245,7 @@ protected override void ParseElements()
 			lights = new Lights(root);
 			plugins = new Plugins(root);
 
-			var gravityStr = (IsValidNode("gravity")) ? GetValue<string>("gravity") : "0 0 -9.8";
+			var gravityStr = (IsValidNode("gravity")) ? GetValue<string>("gravity") : "0 0 -9.8000000000000007";
 			gravity.FromString(gravityStr);
 
 			// Console.WriteLine("{0}", GetType().Name);