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