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;
}
