diff --git a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs b/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs index e8939d8c..e42c24f0 100644 --- a/Assets/Scripts/CLOiSimPlugins/JointControlPlugin.cs +++ b/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("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) { diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs index fd5e32e8..6d7d2609 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs +++ b/Assets/Scripts/CLOiSimPlugins/Modules/Base/CLOiSimPlugin.CommonMethod.cs @@ -39,8 +39,8 @@ 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); @@ -48,11 +48,14 @@ protected void PublishTfThread(System.Object threadObject) DeviceHelper.SetQuaternion(tfMessage.Transform.Orientation, tfPose.rotation); deviceMessage.SetMessage(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); diff --git a/Assets/Scripts/CLOiSimPlugins/Modules/Base/TF.cs b/Assets/Scripts/CLOiSimPlugins/Modules/Base/TF.cs index 3e3ae8fb..a26c1686 100644 --- a/Assets/Scripts/CLOiSimPlugins/Modules/Base/TF.cs +++ b/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; } } \ No newline at end of file diff --git a/Assets/Scripts/Tools/SDF/Helper/Link.cs b/Assets/Scripts/Tools/SDF/Helper/Link.cs index 504ff52d..b77ab15f 100644 --- a/Assets/Scripts/Tools/SDF/Helper/Link.cs +++ b/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 collisionContacts = new List(); - 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(); + _parentModelHelper = transform.parent?.GetComponent(); + _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(); + var parentArtBodies = GetComponentsInParent(); + + if (parentArtBodies.Length > 0) + { + _artBody = parentArtBodies[0]; + + if (parentArtBodies.Length > 1) + { + _parentArtBody = parentArtBodies[1]; + _parentLink = _parentArtBody.gameObject.GetComponent(); + } + } + + 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(); + var otherLinkPlugins = _rootModel.GetComponentsInChildren(); 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); } } } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs index 166fb151..4cc84300 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Joint.cs +++ b/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 } diff --git a/Assets/Scripts/Tools/SDF/Implement/Implement.Material.cs b/Assets/Scripts/Tools/SDF/Implement/Implement.Material.cs index f6f124b9..87ce81e6 100644 --- a/Assets/Scripts/Tools/SDF/Implement/Implement.Material.cs +++ b/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 { diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs b/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs index 6d7ea60f..7ad07500 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Joint.cs +++ b/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); diff --git a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs b/Assets/Scripts/Tools/SDF/Import/Import.Link.cs index d80c751b..630577dd 100644 --- a/Assets/Scripts/Tools/SDF/Import/Import.Link.cs +++ b/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; diff --git a/Assets/Scripts/Tools/SDF/Parser/Link.cs b/Assets/Scripts/Tools/SDF/Parser/Link.cs index 6b34f721..a935362b 100644 --- a/Assets/Scripts/Tools/SDF/Parser/Link.cs +++ b/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; diff --git a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.cs b/Assets/Scripts/Tools/SDF/Util/SDF2Unity.cs index f56c7e94..6826d0a7 100644 --- a/Assets/Scripts/Tools/SDF/Util/SDF2Unity.cs +++ b/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; } /// right handed system x diff --git a/Assets/Scripts/UI/ModelImporter.cs b/Assets/Scripts/UI/ModelImporter.cs index b4a55d78..ae3e480d 100644 --- a/Assets/Scripts/UI/ModelImporter.cs +++ b/Assets/Scripts/UI/ModelImporter.cs @@ -139,7 +139,7 @@ private void UpdateInitPose() var modelHelper = _targetObject.GetComponent(); if (modelHelper != null) { - modelHelper.SetPose(_targetObject.position + modelDeployOffset, _targetObject.rotation); + modelHelper.SetPose(_targetObject.localPosition + modelDeployOffset, _targetObject.localRotation); } } diff --git a/Packages/packages-lock.json b/Packages/packages-lock.json index 9ea479a3..4049039c 100644 --- a/Packages/packages-lock.json +++ b/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": { diff --git a/ProjectSettings/ProjectSettings.asset b/ProjectSettings/ProjectSettings.asset index 180266fb..e7055b98 100644 --- a/ProjectSettings/ProjectSettings.asset +++ b/ProjectSettings/ProjectSettings.asset @@ -139,7 +139,7 @@ PlayerSettings: loadStoreDebugModeEnabled: 0 visionOSBundleVersion: 1.0 tvOSBundleVersion: 1.0 - bundleVersion: 4.5.5 + bundleVersion: 4.5.6 preloadedAssets: [] metroInputSource: 0 wsaTransparentSwapchain: 0 diff --git a/ProjectSettings/ProjectVersion.txt b/ProjectSettings/ProjectVersion.txt index d8a33589..f2e63c60 100644 --- a/ProjectSettings/ProjectVersion.txt +++ b/ProjectSettings/ProjectVersion.txt @@ -1,2 +1,2 @@ -m_EditorVersion: 2022.3.30f1 -m_EditorVersionWithRevision: 2022.3.30f1 (70558241b701) +m_EditorVersion: 2022.3.31f1 +m_EditorVersionWithRevision: 2022.3.31f1 (4ede2d13e8b4) diff --git a/README.md b/README.md index f1d0c1ee..17f680f6 100644 --- a/README.md +++ b/README.md @@ -34,7 +34,7 @@ Here are the list of items that is implemented(marked) or planned to be implemen - [X] Joint models - [X] 2-Wheeled Motor driving - [X] Joint control - - [ ] Joint Pose + - [X] Joint Pose - [X] Sensor models - [X] LiDAR Sensor - [X] 2D @@ -77,6 +77,8 @@ There is problem with `` in `` since introduction of articulation b Currently, geometry mesh type is supporting only 'Wavefront(.obj) with material', 'Collada(.dae) including animation' and 'STL(.stl)'. `` elements in `` and ambient properies in mesh files are not support in CLOiSim. +If you're trying to connect `` of ``, it needs to specify unique name in link name in `joint/parent` or `joint/child` within same root model. + ![cloisim_lidar_ros](https://user-images.githubusercontent.com/21001946/107105540-42b65600-686a-11eb-8797-7d937b108c11.gif) [video link](https://user-images.githubusercontent.com/21001946/103972179-d0415000-51af-11eb-824b-3d77051664d5.mp4) @@ -145,10 +147,10 @@ if `` element of `