private static Matrix3x3 ToMatrix3x3(Link.Inertial.Inertia inertia)
{
return(new Matrix3x3(
new[] { (float)inertia.ixx, (float)inertia.ixy, (float)inertia.ixz,
(float)inertia.iyy, (float)inertia.iyz,
(float)inertia.izz }));
}
private static Link.Inertial.Inertia ToRosCoordinates(Link.Inertial.Inertia unityInertia)
{
return(new Link.Inertial.Inertia(0, 0, 0, 0, 0, 0)
{
ixx = unityInertia.izz,
iyy = unityInertia.ixx,
izz = unityInertia.iyy,
ixy = -unityInertia.ixz,
ixz = unityInertia.iyz,
iyz = -unityInertia.ixy
});
}
public Link.Inertial ExportInertialData()
{
#if UNITY_2020_1_OR_NEWER
ArticulationBody robotLink = GetComponent <ArticulationBody>();
#else
Rigidbody robotLink = GetComponent <Rigidbody>();
#endif
if (robotLink == null)
{
return(null);
}
UpdateLinkData();
Vector3 originAngles = inertialAxisRotation.eulerAngles;
Origin inertialOrigin = new Origin(robotLink.centerOfMass.Unity2Ros().ToRoundedDoubleArray(), new double[] { (double)originAngles.x, (double)originAngles.y, (double)originAngles.z });
Link.Inertial.Inertia inertia = ExportInertiaData();
return(new Link.Inertial(Math.Round(robotLink.mass, RoundDigits), inertialOrigin, inertia));
}
