public static void Create(GameObject linkObject, Link.Inertial inertial = null) { UrdfInertial urdfInertial = linkObject.AddComponent <UrdfInertial>(); #if UNITY_2020_1_OR_NEWER ArticulationBody robotLink = urdfInertial.GetComponent <ArticulationBody>(); #else Rigidbody robotLink = urdfInertial.GetComponent <Rigidbody>(); #endif if (inertial != null) { robotLink.mass = ((float)inertial.mass > 0)?((float)inertial.mass):minMass; if (inertial.origin != null) { robotLink.centerOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin); } else { robotLink.centerOfMass = Vector3.zero; } urdfInertial.ImportInertiaData(inertial); urdfInertial.useUrdfData = true; } urdfInertial.displayInertiaGizmo = false; }
/// <summary> /// Compares inertial information of two links /// </summary> /// <param name="source">First link's inertial information to be compared</param> /// <param name="exported">Second link's inertial information to be compared</param> /// <param name="indent">Indent level in the log file</param> /// <returns></returns> private bool CompareInertial(Link.Inertial source, Link.Inertial exported, int indent) { if (source == null && exported == null) { linkLog.AppendLine(String.Format("{0}Inertia Attributes Null:{1,6}", Indent(indent), "True")); return(true); } if ((source == null && exported != null) || (source != null && exported == null)) { linkLog.AppendLine(String.Format("{0}Inertia Attributes Null:{1,6}", Indent(indent), "False")); return(false); } bool massEqual = source.mass.EqualsDelta(exported.mass, .05); linkLog.AppendLine(String.Format("{0}Mass:", Indent(indent))); linkLog.AppendLine(String.Format("{0}Equal:{1,6}", Indent(indent), massEqual)); if (!massEqual) { return(false); } if (!CompareOrigin(source.origin, exported.origin, indent + 1)) { return(false); } return(true); }
private void ImportInertiaData(Link.Inertial inertial) { Vector3 eigenvalues; Vector3[] eigenvectors; Matrix3x3 rotationMatrix = ToMatrix3x3(inertial.inertia); rotationMatrix.DiagonalizeRealSymmetric(out eigenvalues, out eigenvectors); #if UNITY_2020_1_OR_NEWER ArticulationBody robotLink = GetComponent <ArticulationBody>(); #else Rigidbody robotLink = GetComponent <Rigidbody>(); #endif Vector3 inertiaEulerAngles; if (inertial.origin != null) { inertiaEulerAngles = UrdfOrigin.GetRotationFromUrdf(inertial.origin); } else { inertiaEulerAngles = new Vector3(0, 0, 0); } this.inertialAxisRotation.eulerAngles = inertiaEulerAngles; robotLink.inertiaTensor = ToUnityInertiaTensor(FixMinInertia(eigenvalues)); robotLink.inertiaTensorRotation = ToQuaternion(eigenvectors[0], eigenvectors[1], eigenvectors[2]).Ros2Unity() * this.inertialAxisRotation; this.centerOfMass = robotLink.centerOfMass; this.inertiaTensor = robotLink.inertiaTensor; this.inertiaTensorRotation = robotLink.inertiaTensorRotation; }