public static void Create(GameObject linkObject, Link.Inertial inertial = null)
{
UrdfInertial urdfInertial = linkObject.AddComponent <UrdfInertial>();
Rigidbody _rigidbody = urdfInertial.GetComponent <Rigidbody>();
if (inertial != null)
{
_rigidbody.mass = (float)inertial.mass;
if (inertial.origin != null)
{
_rigidbody.centerOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
}
urdfInertial.ImportInertiaData(inertial.inertia);
urdfInertial.UseUrdfData = true;
}
urdfInertial.DisplayInertiaGizmo = false;
//Save original rigidbody data from URDF
urdfInertial.CenterOfMass = _rigidbody.centerOfMass;
urdfInertial.InertiaTensor = _rigidbody.inertiaTensor;
urdfInertial.InertiaTensorRotation = _rigidbody.inertiaTensorRotation;
}
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;
if (inertial.origin != null)
{
robotLink.centerOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
}
else
{
robotLink.centerOfMass = Vector3.zero;
}
urdfInertial.ImportInertiaData(inertial);
urdfInertial.useUrdfData = true;
}
urdfInertial.displayInertiaGizmo = false;
}
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;
}
public static void Create(GameObject linkObject, Link.Inertial inertial)
{
UrdfInertial urdfInertial = linkObject.AddComponent <UrdfInertial>();
urdfInertial.UrdfMass = (float)inertial.mass;
if (inertial.origin != null)
{
urdfInertial.UrdfCenterOfMass = UrdfOrigin.GetPositionFromUrdf(inertial.origin);
}
urdfInertial.ImportInertiaData(inertial.inertia);
urdfInertial.Initialize();
urdfInertial.isCreated = true;
}
