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;
}
private void ImportLinkData(Link link, Joint joint)
{
if (link.inertial == null && joint == null)
{
isBaseLink = true;
}
gameObject.name = link.name;
if (joint?.origin != null)
{
UrdfOrigin.ImportOriginData(transform, joint.origin);
}
if (link.inertial != null)
{
UrdfInertial.Create(gameObject, link.inertial);
if (joint != null)
{
UrdfJoint.Create(gameObject, UrdfJoint.GetJointType(joint.type), joint);
}
}
else if (joint != null)
{
Debug.LogWarning("No Joint Component will be created in GameObject \"" + gameObject.name + "\" as it has no Rigidbody Component.\n"
+ "Please define an Inertial for Link \"" + link.name + "\" in the URDF file to create a Rigidbody Component.\n", gameObject);
}
foreach (Joint childJoint in link.joints)
{
Link child = childJoint.ChildLink;
UrdfLink.Create(transform, child, childJoint);
}
}
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 Joint ExportDefaultJoint(Transform transform)
{
return new Joint(
transform.parent.name + "_" + transform.name + "_joint",
JointTypes.Fixed.ToString().ToLower(),
transform.parent.name,
transform.name,
UrdfOrigin.ExportOriginData(transform));
}
public Link.Collision ExportCollisionData()
{
CheckForUrdfCompatibility();
Link.Geometry geometry = UrdfGeometry.ExportGeometryData(geometryType, transform, true);
string collisionName = gameObject.name == "unnamed" ? null : gameObject.name;
return(new Link.Collision(geometry, collisionName, UrdfOrigin.ExportOriginData(transform)));
}
public static void Create(Transform parent, Link.Collision collision)
{
GameObject collisionObject = new GameObject("unnamed");
collisionObject.transform.SetParentAndAlign(parent);
UrdfCollision urdfCollision = collisionObject.AddComponent <UrdfCollision>();
urdfCollision.geometryType = UrdfGeometry.GetGeometryType(collision.geometry);
UrdfGeometryCollision.Create(collisionObject.transform, urdfCollision.geometryType, collision.geometry);
UrdfOrigin.ImportOriginData(collisionObject.transform, collision.origin);
}
public static void Create(Transform parent, Link.Visual visual)
{
GameObject visualObject = new GameObject(visual.name ?? "unnamed");
visualObject.transform.SetParentAndAlign(parent);
UrdfVisual urdfVisual = visualObject.AddComponent <UrdfVisual>();
urdfVisual.geometryType = UrdfGeometry.GetGeometryType(visual.geometry);
UrdfGeometryVisual.Create(visualObject.transform, urdfVisual.geometryType, visual.geometry);
UrdfMaterial.SetUrdfMaterial(visualObject, visual.material);
UrdfOrigin.ImportOriginData(visualObject.transform, visual.origin);
}
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;
}
public Link.Visual ExportVisualData()
{
CheckForUrdfCompatibility();
Link.Geometry geometry = UrdfGeometry.ExportGeometryData(geometryType, transform);
Link.Visual.Material material = null;
if (!(geometry.mesh != null && geometry.mesh.filename.ToLower().EndsWith(".dae"))) //Collada files contain their own materials
{
material = UrdfMaterial.ExportMaterialData(gameObject.GetComponentInChildren <MeshRenderer>().sharedMaterial);
}
string visualName = gameObject.name == "unnamed" ? null : gameObject.name;
return(new Link.Visual(geometry, visualName, UrdfOrigin.ExportOriginData(transform), material));
}
public Joint ExportJointData()
{
UnityJoint = GetComponent<UnityEngine.Joint>();
CheckForUrdfCompatibility();
//Data common to all joints
Joint joint = new Joint(
JointName,
JointType.ToString().ToLower(),
gameObject.transform.parent.name,
gameObject.name,
UrdfOrigin.ExportOriginData(transform));
joint.limit = ExportLimitData();
return ExportSpecificJointData(joint);
}
public Joint ExportJointData()
{
#if UNITY_2020_1_OR_NEWER
unityJoint = GetComponent <UnityEngine.ArticulationBody>();
#else
unityJoint = GetComponent <UnityEngine.Joint>();
#endif
CheckForUrdfCompatibility();
//Data common to all joints
Joint joint = new Joint(
jointName,
JointType.ToString().ToLower(),
gameObject.transform.parent.name,
gameObject.name,
UrdfOrigin.ExportOriginData(transform));
joint.limit = ExportLimitData();
return(ExportSpecificJointData(joint));
}
