Merge from 'develop' into 'main' for CLOiSim-4.5.6 (#288)

Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs

@@ -71,7 +71,7 @@ private void LoadJoints()
 				if (jointState.AddTargetJoint(jointName, out var targetLink, out var isStatic))
 				{
 					var parentFrameId = GetPluginParameters().GetAttributeInPath<string>("joints/joint[text()='" + jointName + "']", "parent_frame_id");
-					var jointParentLinkName = (parentFrameId == null) ? targetLink.JointParentLinkName : parentFrameId;
+					var jointParentLinkName = (string.IsNullOrEmpty(parentFrameId)) ? targetLink.JointParentLinkName : parentFrameId;
 					var tf = new TF(targetLink, targetLink.JointChildLinkName, jointParentLinkName);
 					if (isStatic)
 					{

Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs

@@ -39,20 +39,23 @@ protected void PublishTfThread(System.Object threadObject)
 				{
 					var tf = tfList[i];
 
-					tfMessage.Header.StrId = tf.parentFrameId;
-					tfMessage.Transform.Name = tf.childFrameId;
+					tfMessage.Header.StrId = tf.ParentFrameID;
+					tfMessage.Transform.Name = tf.ChildFrameID;
 
 					var tfPose = tf.GetPose();
 					DeviceHelper.SetCurrentTime(tfMessage.Header.Stamp);
 					DeviceHelper.SetVector3d(tfMessage.Transform.Position, tfPose.position);
 					DeviceHelper.SetQuaternion(tfMessage.Transform.Orientation, tfPose.rotation);
 
 					deviceMessage.SetMessage<messages.TransformStamped>(tfMessage);
-					// Debug.Log(tfMessage.Header.Stamp.Sec + "." + tfMessage.Header.Stamp.Nsec + ": " + tfMessage.Header.StrId + ", " + tfMessage.Transform.Name) ;
 					if (publisher.Publish(deviceMessage) == false)
 					{
 						Debug.Log(tfMessage.Header.StrId + ", " + tfMessage.Transform.Name + " error to send TF!!");
 					}
+					else
+					{
+						// Debug.Log(tfMessage.Header.Stamp.Sec + "." + tfMessage.Header.Stamp.Nsec + ": " + tfMessage.Header.StrId + ", " + tfMessage.Transform.Name);
+					}
 					CLOiSimPluginThread.Sleep(updatePeriodPerEachTf);
 				}
 			}
@@ -204,17 +207,16 @@ private void SetStaticTransformsResponse(ref DeviceMessage msRos2Info)
 		{
 			var ros2StaticTransformLink = new messages.Param();
 			ros2StaticTransformLink.Name = "parent_frame_id";
-			ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.parentFrameId };
+			ros2StaticTransformLink.Value = new Any { Type = Any.ValueType.String, StringValue = tf.ParentFrameID };
 
 			{
 				var tfPose = tf.GetPose();
-				// Debug.Log(tf.parentFrameId + " <= " + tf.childFrameId + " = " + tf.link.JointAxis + ", " + tfPose);
 
 				var poseMessage = new messages.Pose();
 				poseMessage.Position = new messages.Vector3d();
 				poseMessage.Orientation = new messages.Quaternion();
 
-				poseMessage.Name = tf.childFrameId;
+				poseMessage.Name = tf.ChildFrameID;
 				DeviceHelper.SetVector3d(poseMessage.Position, tfPose.position);
 				DeviceHelper.SetQuaternion(poseMessage.Orientation, tfPose.rotation);
 

Assets/Scripts/CLOiSimPlugins/Modules/Base/TF.cs

@@ -8,42 +8,40 @@
 
 public class TF
 {
-	private const int targetPoseFrame = 1;
+	private const int TargetPoseFrame = 1;
 
-	public string parentFrameId = string.Empty;
-	public string childFrameId = string.Empty;
-	public SDF.Helper.Link link = null;
+	public string _parentFrameId = string.Empty;
+	public string _childFrameId = string.Empty;
+	private SDF.Helper.Link _link = null;
 
-	public TF(in SDF.Helper.Link link, in string childFrameId, in string parentFrameId = "base_link")
+	public string ParentFrameID => _parentFrameId;
+	public string ChildFrameID => _childFrameId;
+
+
+	public TF(in SDF.Helper.Link link, in string childFrameId, in string parentFrameId)
 	{
-		this.parentFrameId = parentFrameId.Replace("::", "_");
-		this.childFrameId = childFrameId.Replace("::", "_");
-		this.link = link;
+		this._parentFrameId = parentFrameId.Replace("::", "_");
+		this._childFrameId = childFrameId.Replace("::", "_");
+		this._link = link;
 		// Debug.LogFormat("{0} <- {1}", parentFrameId, childFrameId);
 	}
 
 	public Pose GetPose()
 	{
-		var tfLink = this.link;
-		var tfPose = tfLink.GetPose(targetPoseFrame);
-		var modelPose = tfLink.Model.GetPose(targetPoseFrame);
+		var tfPose = Pose.identity;
+		var modelPose = _link.Model.GetPose(TargetPoseFrame);
+		var linkJointPose = _link.LinkJointPose;
+		var linkPoseMoving = _link.GetPose(TargetPoseFrame);
 
-		// Debug.Log(parentFrameId + " <= " + childFrameId + "(" + tfLink.JointAxis + ")" + tfPose);
-		if (!tfLink.Model.Equals(tfLink.RootModel))
-		{
-			tfPose.rotation = modelPose.rotation * tfPose.rotation;
-
-			if (tfLink.Model != null)
-			{
-				tfPose.position += modelPose.position;
-			}
-		}
-		else
+		if (!_link.Model.Equals(_link.RootModel))
 		{
+			tfPose.position += modelPose.position;
 			tfPose.rotation *= modelPose.rotation;
-			// Debug.Log("is Root Model TF");
 		}
-		// Debug.Log(parentFrameId + "::" + childFrameI + "(" + tfLink.JointAxis + ") = tf rot" + tfPose.rotation.eulerAngles);
+
+		tfPose.position += linkJointPose.position;
+		tfPose.rotation *= linkPoseMoving.rotation;
+
 		return tfPose;
 	}
 }

Assets/Scripts/Tools/SDF/Helper/Link.cs

@@ -13,9 +13,13 @@ namespace Helper
 	{
 		public class Link : Base
 		{
-			private Model rootModel = null;
-			private Model parentModelHelper = null;
+			private Model _rootModel = null;
+			private Model _parentModelHelper = null;
 			private UE.ArticulationBody _artBody = null;
+			private UE.ArticulationBody _parentArtBody = null;
+			private Link _parentLink = null;
+			private UE.Pose _jointPose = UE.Pose.identity;
+			private bool _isParentLinkModel = false;
 
 			[UE.Header("Properties")]
 			public bool drawInertia = false;
@@ -25,21 +29,20 @@ public class Link : Base
 			public bool isSelfCollide = false;
 			public bool useGravity = false;
 
+			[UE.Header("Joint related")]
 			private string jointName = string.Empty;
 			private string jointParentLinkName = string.Empty;
 			private string jointChildLinkName = string.Empty;
 
-			private UE.Vector3 jointAxis = UE.Vector3.zero;
-			private UE.Vector3 jointAxis2 = UE.Vector3.zero;
+			private UE.Pose _jointAnchorPose = new UE.Pose();
 
 			private float _jointAxisLimitVelocity = float.NaN;
-
 			private float _jointAxis2LimitVelocity = float.NaN;
 
 			private List<UE.ContactPoint> collisionContacts = new List<UE.ContactPoint>();
 
-			private SensorDevices.Battery battery = null;
-			public SensorDevices.Battery Battery => battery;
+			private SensorDevices.Battery _battery = null;
+			public SensorDevices.Battery Battery => _battery;
 
 			public string JointName
 			{
@@ -71,35 +74,41 @@ public float JointAxis2LimitVelocity
 				set => this._jointAxis2LimitVelocity = value;
 			}
 
-			public UE.Vector3 JointAxis
-			{
-				get => this.jointAxis;
-				set => this.jointAxis = value;
-			}
-
-			public UE.Vector3 JointAxis2
-			{
-				get => this.jointAxis2;
-				set => this.jointAxis2 = value;
-			}
-
-			public Model RootModel => rootModel;
+			public UE.Pose LinkJointPose => _jointPose;
 
-			public Model Model => parentModelHelper;
+			public Model RootModel => _rootModel;
+			public Model Model => _parentModelHelper;
 
 			new protected void Awake()
 			{
 				base.Awake();
-				parentModelHelper = transform.parent?.GetComponent<Model>();
+				_parentModelHelper = transform.parent?.GetComponent<Model>();
+				_jointAnchorPose = UE.Pose.identity;
 			}
 
 			// Start is called before the first frame update
 			void Start()
 			{
 				var modelHelpers = GetComponentsInParent(typeof(Model));
-				rootModel = (Model)modelHelpers[modelHelpers.Length - 1];
+				_rootModel = (Model)modelHelpers[modelHelpers.Length - 1];
 
-				_artBody = GetComponent<UE.ArticulationBody>();
+				var parentArtBodies = GetComponentsInParent<UE.ArticulationBody>();
+
+				if (parentArtBodies.Length > 0)
+				{
+					_artBody = parentArtBodies[0];
+
+					if (parentArtBodies.Length > 1)
+					{
+						_parentArtBody = parentArtBodies[1];
+						_parentLink = _parentArtBody.gameObject.GetComponent<Link>();
+					}
+				}
+
+				if (transform.parent.CompareTag("Link"))
+				{
+					_isParentLinkModel = true;
+				}
 
 				// Handle self collision
 				if (!isSelfCollide)
@@ -111,13 +120,27 @@ void Start()
 			void LateUpdate()
 			{
 				SetPose(transform.localPosition, transform.localRotation, 1);
-			}
+
+				if (_artBody != null)
+				{
+					_jointAnchorPose.position = _artBody.anchorPosition;
+					_jointAnchorPose.rotation = _artBody.anchorRotation;
+				}
+
+				_jointPose.position = transform.localPosition + _jointAnchorPose.position;
+				_jointPose.rotation = transform.localRotation * _jointAnchorPose.rotation;
+
+				if (_parentLink != null && _isParentLinkModel == false)
+				{
+					_jointPose.position -= _parentLink._jointPose.position;
+				}
+		}
 
 			void OnDrawGizmos()
 			{
 				if (_artBody && drawInertia)
 				{
-					UE.Gizmos.color = new UE.Color(0.35f, 0.0f, 0.1f, 0.1f);
+					UE.Gizmos.color = new UE.Color(0.45f, 0.1f, 0.15f, 0.3f);
 
 					var region = _artBody.inertiaTensor;
 					if (region.x < 1f && region.y < 1f && region.z < 1f)
@@ -170,12 +193,12 @@ private UE.Collider[] GetCollidersInChildren()
 
 			private void IgnoreSelfCollision()
 			{
-				if (rootModel == null)
+				if (_rootModel == null)
 				{
 					return;
 				}
 
-				var otherLinkPlugins = rootModel.GetComponentsInChildren<Link>();
+				var otherLinkPlugins = _rootModel.GetComponentsInChildren<Link>();
 				var thisColliders = GetCollidersInChildren();
 
 				foreach (var otherLinkPlugin in otherLinkPlugins)
@@ -199,12 +222,12 @@ private void IgnoreSelfCollision()
 
 			public void AttachBattery(in string name, in float initVoltage)
 			{
-				if (battery == null)
+				if (_battery == null)
 				{
-					battery = new SensorDevices.Battery(name);
+					_battery = new SensorDevices.Battery(name);
 				}
 
-				battery.SetMax(initVoltage);
+				_battery.SetMax(initVoltage);
 			}
 		}
 	}

Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs

@@ -291,7 +291,7 @@ public static void MakePrismatic(in UE.ArticulationBody body, in SDF.Axis axis,
 
 			private static void ReverseArticulationBodyAxis(in UE.ArticulationBody body, in UE.Vector3 euler)
 			{
-				body.anchorRotation *= UE.Quaternion.Euler(euler * 180);
+				body.anchorRotation *= UE.Quaternion.Euler(euler * 180f);
 				// body.parentAnchorRotation *= UE.Quaternion.Euler(euler * 180);  // TODO: matchAnchors is set to true
 			}
 

Assets/Scripts/Tools/SDF/Implement/Implement.Material.cs

@@ -5,10 +5,8 @@
  */
 
 using System.Collections.Generic;
-using System.Linq;
 using System.IO;
 using UE = UnityEngine;
-using Debug = UnityEngine.Debug;
 
 namespace SDF
 {

Assets/Scripts/Tools/SDF/Import/Import.Joint.cs

@@ -159,8 +159,6 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec
 
 					if (joint.Axis != null)
 					{
-						// articulationBodyChild
-						linkHelper.JointAxis = SDF2Unity.Axis(joint.Axis.xyz);
 						if (joint.Axis.dynamics != null)
 						{
 							if (joint.Type.Equals("prismatic"))
@@ -177,7 +175,6 @@ protected override void ImportJoint(in Joint joint, in System.Object parentObjec
 
 					if (joint.Axis2 != null)
 					{
-						linkHelper.JointAxis2 = SDF2Unity.Axis(joint.Axis2.xyz);
 						if (joint.Axis2.dynamics != null)
 						{
 							axis2SpringReference = SDF2Unity.CurveOrientation((float)joint.Axis2.dynamics.spring_reference);

Assets/Scripts/Tools/SDF/Import/Import.Link.cs

@@ -17,6 +17,11 @@ public partial class Loader : Base
 
 			private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia)
 			{
+				/**
+				 *  | Ixx  Ixy  Ixz |
+				 *  | Ixy  Iyy  Iyz |
+				 *  | Ixz  Iyz  Izz |
+				 */
 				var inertiaMomentum = UE.Pose.identity;
 				var inertiaVector = SDF2Unity.Scalar((float)inertia?.ixx, (float)inertia?.iyy, (float)inertia?.izz);
 				var inertiaRotationVector = SDF2Unity.Scalar((float)inertia?.ixy, (float)inertia?.iyz, (float)inertia?.ixz);
@@ -27,11 +32,6 @@ private static UE.Pose GetInertiaTensor(in SDF.Inertial.Inertia inertia)
 					{
 						inertiaVector[index] = MinimumInertiaTensor;
 					}
-
-					if (inertiaRotationVector[index] <= MinimumInertiaTensor)
-					{
-						inertiaRotationVector[index] = MinimumInertiaTensor;
-					}
 				}
 
 				inertiaMomentum.position = inertiaVector;

Assets/Scripts/Tools/SDF/Parser/Link.cs

@@ -14,15 +14,15 @@ public class Inertial
 	{
 		public class Inertia
 		{
-			public double ixx;
-			public double ixy;
-			public double ixz;
-			public double iyy;
-			public double iyz;
-			public double izz;
+			public double ixx = 0;
+			public double ixy = 0;
+			public double ixz = 0;
+			public double iyy = 0;
+			public double iyz = 0;
+			public double izz = 0;
 		}
 
-		public double mass;
+		public double mass = 0;
 
 		public Inertia inertia;
 

Assets/Scripts/Tools/SDF/Util/SDF2Unity.cs

@@ -29,7 +29,11 @@ public static Color Color(in string value)
 
 	public static Vector3 Scalar(in double x, in double y, in double z)
 	{
-		return new Vector3(Mathf.Abs((float)y), Mathf.Abs((float)z), Mathf.Abs((float)x));
+		var scalarVector = Position(x, y, z);
+		scalarVector.x = Mathf.Abs(scalarVector.x);
+		scalarVector.y = Mathf.Abs(scalarVector.y);
+		scalarVector.z = Mathf.Abs(scalarVector.z);
+		return scalarVector;
 	}
 
 	/// <param name="x">right handed system x</param>

Assets/Scripts/UI/ModelImporter.cs

@@ -139,7 +139,7 @@ private void UpdateInitPose()
 		var modelHelper = _targetObject.GetComponent<SDF.Helper.Model>();
 		if (modelHelper != null)
 		{
-			modelHelper.SetPose(_targetObject.position + modelDeployOffset, _targetObject.rotation);
+			modelHelper.SetPose(_targetObject.localPosition + modelDeployOffset, _targetObject.localRotation);
 		}
 	}
 

Packages/packages-lock.json

@@ -10,7 +10,7 @@
       "url": "https://packages.unity.com"
     },
     "com.unity.burst": {
-      "version": "1.8.13",
+      "version": "1.8.15",
       "depth": 1,
       "source": "registry",
       "dependencies": {