private static void ImportLinkData(this UrdfLink urdfLink, Link link, Joint joint)
{
if (link.inertial == null && joint == null)
{
urdfLink.IsBaseLink = true;
}
urdfLink.gameObject.name = link.name;
if (joint?.origin != null)
{
UrdfOrigin.ImportOriginData(urdfLink.transform, joint.origin);
}
if (link.inertial != null)
{
UrdfInertial.Create(urdfLink.gameObject, link.inertial);
if (joint != null)
{
UrdfJoint.Create(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint);
}
}
else if (joint != null)
{
Debug.LogWarning("No Joint Component will be created in GameObject \"" + urdfLink.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", urdfLink.gameObject);
}
foreach (Joint childJoint in link.joints)
{
Link child = childJoint.ChildLink;
UrdfLinkExtensions.Create(urdfLink.transform, child, childJoint);
}
}
private static void ImportLinkData(this UrdfLink urdfLink, Link link, Joint joint)
{
if (link.inertial == null && joint == null)
{
urdfLink.IsBaseLink = true;
}
urdfLink.gameObject.name = link.name;
if (joint?.origin != null)
{
UrdfOrigin.ImportOriginData(urdfLink.transform, joint.origin);
}
if (link.inertial != null)
{
UrdfInertial.Create(urdfLink.gameObject, link.inertial);
if (joint != null)
{
UrdfJoint.Create(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint);
}
}
else if (joint != null)
{
UrdfJoint.Create(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint);
}
foreach (Joint childJoint in link.joints)
{
Link child = childJoint.ChildLink;
UrdfLinkExtensions.Create(urdfLink.transform, child, childJoint);
}
}
public static Link ExportLinkData(this UrdfLink urdfLink)
{
if (urdfLink.transform.localScale != Vector3.one)
{
Debug.LogWarning("Only visuals should be scaled. Scale on link \"" + urdfLink.gameObject.name + "\" cannot be saved to the URDF file.", urdfLink.gameObject);
}
UrdfInertial urdfInertial = urdfLink.gameObject.GetComponent <UrdfInertial>();
Link link = new Link(urdfLink.gameObject.name)
{
visuals = urdfLink.GetComponentInChildren <UrdfVisuals>().ExportVisualsData(),
collisions = urdfLink.GetComponentInChildren <UrdfCollisions>().ExportCollisionsData(),
inertial = urdfInertial == null ? null : urdfInertial.ExportInertialData()
};
return(link);
}
private static void ImportLinkData(this UrdfLink urdfLink, Link link, Joint joint)
{
if (link.inertial == null && joint == null)
{
urdfLink.IsBaseLink = true;
}
urdfLink.gameObject.name = link.name;
if (joint?.origin != null)
{
UrdfOrigin.ImportOriginData(urdfLink.transform, joint.origin);
}
if (link.inertial != null)
{
UrdfInertial.Synchronize(urdfLink.gameObject, link.inertial);
if (joint != null)
{
UrdfJoint.Synchronize(urdfLink.gameObject, UrdfJoint.GetJointType(joint.type), joint);
}
}
else if (joint != null)
{
Debug.LogWarning("No Joint Component will be created in GameObject \"" + urdfLink.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", urdfLink.gameObject);
}
foreach (Joint childJoint in link.joints.Where(x => x.ChildLink != null))
{
Link child = childJoint.ChildLink;
UrdfLinkExtensions.Synchronize(urdfLink.transform, child, childJoint);
}
var linkChildren = Utils.GetComponentsInDirectChildrenFromGameobject <UrdfLink>(urdfLink.gameObject);
List <string> wantedObjectNames = link.joints
.Where(x => x.ChildLink != null)
.Select(x => String.IsNullOrEmpty(x.child) ? Utils.GenerateNonReferenceID(x.ChildLink) : x.child)
.ToList();
linkChildren.RemoveAll(x => wantedObjectNames.Contains(x.name));
Utils.DestroyAll(linkChildren.Select(x => x.gameObject));
}
public static UrdfLink Synchronize(Transform parent, Link link = null, Joint joint = null)
{
parent.FindChildOrCreateWithComponent <UrdfLink>(link != null ? link.name : Utils.GenerateNonReferenceID(link), out GameObject linkObject, out UrdfLink urdfLink);
UrdfVisualsExtensions.Synchronize(linkObject.transform, link?.visuals);
UrdfCollisionsExtensions.Synchronize(linkObject.transform, link?.collisions);
if (link != null)
{
urdfLink.ImportLinkData(link, joint);
}
else
{
UrdfInertial.Synchronize(linkObject);
}
if (link != null)
{
foreach (var attachedComponent in link.attachableComponents)
{
if (AttachedDataSynchronizer.Instance.ShouldCreateComponent(attachedComponent))
{
AttachedDataSynchronizer.Instance.HandleAttachedComponent(linkObject, attachedComponent, link);
}
}
}
//Remove Attached Values that are too much
var attachedValueChildren = linkObject.GetComponentsInDirectChildrenFromGameobject <AttachedValue>();
attachedValueChildren.RemoveAll(x => link.attachableComponents.Any(y => y.component.name == x.name));
foreach (var attachedComponent in attachedValueChildren)
{
AttachedDataSynchronizer.Instance.RemoveAttachedComponent(attachedComponent.AttachedComponent);
}
Utils.DestroyAll(attachedValueChildren.Select(x => x.gameObject));
return(urdfLink);
}
public static UrdfLink Create(Transform parent, Link link = null, Joint joint = null)
{
GameObject linkObject = new GameObject("link");
linkObject.transform.SetParentAndAlign(parent);
UrdfLink urdfLink = linkObject.AddComponent <UrdfLink>();
UrdfVisualsExtensions.Create(linkObject.transform, link?.visuals);
UrdfCollisionsExtensions.Create(linkObject.transform, link?.collisions);
if (link != null)
{
urdfLink.ImportLinkData(link, joint);
}
else
{
UrdfInertial.Create(linkObject);
UnityEditor.EditorGUIUtility.PingObject(linkObject);
}
return(urdfLink);
}
private static void ImportSimulationLinkData(this GameObject linkObject, Link link, Joint joint)
{
linkObject.gameObject.name = link.name;
if (joint?.origin != null)
{
UrdfOrigin.ImportOriginData(linkObject.transform, joint.origin);
}
if (link?.inertial != null)
{
UrdfInertial.Create(linkObject, link.inertial);
// TODO(sam): figure out if possible to only let base_footprint be kinematic
linkObject.GetComponent <Rigidbody>().isKinematic = true;
if (joint != null)
{
UrdfSimulatedJoint.Create(linkObject, UrdfJoint.GetJointType(joint.type), joint);
}
}
else if (linkObject.name == "base_footprint")
{
Rigidbody baseFootprintRigidbody = linkObject.AddComponent <Rigidbody>();
baseFootprintRigidbody.useGravity = false;
baseFootprintRigidbody.isKinematic = true;
}
else if (joint != null)
{
Debug.LogWarning("No Joint Component will be created in GameObject \"" + linkObject.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", linkObject);
}
foreach (Joint childJoint in link.joints)
{
Link child = childJoint.ChildLink;
Create(linkObject.transform, child, childJoint);
}
}
public override void OnInspectorGUI()
{
UrdfInertial urdfInertial = (UrdfInertial)target;
GUILayout.Space(5);
urdfInertial.displayInertiaGizmo =
EditorGUILayout.ToggleLeft("Display Inertia Gizmo", urdfInertial.displayInertiaGizmo);
GUILayout.Space(5);
bool newValue = EditorGUILayout.BeginToggleGroup("Use URDF Data", urdfInertial.useUrdfData);
EditorGUILayout.Vector3Field("URDF Center of Mass", urdfInertial.centerOfMass);
EditorGUILayout.Vector3Field("URDF Inertia Tensor", urdfInertial.inertiaTensor);
EditorGUILayout.Vector3Field("URDF Inertia Tensor Rotation",
urdfInertial.inertiaTensorRotation.eulerAngles);
EditorGUILayout.EndToggleGroup();
if (newValue != urdfInertial.useUrdfData)
{
urdfInertial.useUrdfData = newValue;
urdfInertial.UpdateLinkData();
}
}
