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 }));
}
public Link.Inertial ExportInertialData()
{
// should we read the data from this clas instead of _rigidbody?
Origin inertialOrigin = new Origin(_rigidbody.centerOfMass.Unity2Ros().ToRoundedDoubleArray(), new double[] { 0, 0, 0 });
Link.Inertial.Inertia inertia = ExportInertiaData(_rigidbody);
return(new Link.Inertial(Math.Round(_rigidbody.mass, RoundDigits), inertialOrigin, inertia));
}
private void ImportInertiaData(Link.Inertial.Inertia inertia)
{
Vector3 eigenvalues;
Vector3[] eigenvectors;
Matrix3x3 rotationMatrix = ToMatrix3x3(inertia);
rotationMatrix.DiagonalizeRealSymmetric(out eigenvalues, out eigenvectors);
UrdfInertiaTensor = ToUnityInertiaTensor(FixMinInertia(eigenvalues));
UrdfInertiaTensorRotation = ToQuaternion(eigenvectors[0], eigenvectors[1], eigenvectors[2]).Ros2Unity();
}
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()
{
Rigidbody _rigidbody = GetComponent <Rigidbody>();
if (_rigidbody == null)
{
return(null);
}
UpdateRigidBodyData();
Origin inertialOrigin = new Origin(_rigidbody.centerOfMass.Unity2Ros().ToRoundedDoubleArray(), new double[] { 0, 0, 0 });
Link.Inertial.Inertia inertia = ExportInertiaData(_rigidbody);
return(new Link.Inertial(Math.Round(_rigidbody.mass, RoundDigits), inertialOrigin, inertia));
}
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));
}