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 void ImportJointData_SpecificLimit_Succeeds()
{
var joint = new Joint(
name: "custom_joint", type: "planar", parent: "base", child: "link",
limit: new Joint.Limit(4, 5, 6, 7),
dynamics: new Joint.Dynamics(8, 9));
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
TestUrdfJointPlanar urdfJoint = linkObject.AddComponent <TestUrdfJointPlanar>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
urdfJoint.TestImportJointData(joint);
Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockX);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockY);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockZ);
Assert.AreEqual(4, articulationBody.xDrive.lowerLimit);
Assert.AreEqual(4, articulationBody.yDrive.lowerLimit);
Assert.AreEqual(4, articulationBody.zDrive.lowerLimit);
Assert.AreEqual(5, articulationBody.xDrive.upperLimit);
Assert.AreEqual(5, articulationBody.yDrive.upperLimit);
Assert.AreEqual(5, articulationBody.zDrive.upperLimit);
Assert.AreEqual(6, articulationBody.xDrive.forceLimit);
Assert.AreEqual(6, articulationBody.yDrive.forceLimit);
Assert.AreEqual(6, articulationBody.zDrive.forceLimit);
Assert.AreEqual(7, articulationBody.maxLinearVelocity);
Object.DestroyImmediate(baseObject);
}
/// <summary>
/// Initializes RobotLinks and sets a boolean to its Visuals dictionary,
/// telling whether the model of individual visual was already imported (is type of box, cylinder, capsule)
/// or not yet (is mesh - is going to be continually imported from ColladaImporter).
/// </summary>
public void LoadLinks()
{
// Get all UrdfLink components in builded Robot
foreach (UrdfLink link in RobotModelGameObject.GetComponentsInChildren <UrdfLink>())
{
// Get all UrdfVisuals of each UrdfLink
GameObject visualsGameObject = link.gameObject.GetComponentInChildren <UrdfVisuals>().gameObject;
Dictionary <UrdfVisual, bool> visuals = new Dictionary <UrdfVisual, bool>();
// Traverse each UrdfVisual and set a boolean indicating whether its visual is already loaded (is of some basic type - box, cylinder, capsule)
// or is going to be loaded by ColladaImporter (in case its type of mesh)
foreach (UrdfVisual visual in visualsGameObject.GetComponentsInChildren <UrdfVisual>())
{
visuals.Add(visual, visual.GeometryType == GeometryTypes.Mesh ? false : true);
// hide visual if it is mesh.. mesh will be displayed when fully loaded
visual.gameObject.SetActive(visual.GeometryType == GeometryTypes.Mesh ? false : true);
}
UrdfJoint urdfJoint = link.GetComponent <UrdfJoint>();
JointStateWriter jointWriter = null;
if (urdfJoint != null)
{
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
jointWriter = urdfJoint.transform.AddComponentIfNotExists <JointStateWriter>();
Joints.Add(urdfJoint.JointName, link.gameObject.name);
}
}
Links.Add(link.gameObject.name, new RobotLink(link.gameObject.name, urdfJoint, jointWriter, visuals, is_base_link: link.IsBaseLink));
}
}
public void DrawGhost(Drawing3d drawing, JointPlacement[] placements, Color color)
{
foreach (JointPlacement jointPlacement in placements)
{
UrdfJoint joint = jointPlacement.Joint;
int numChildren = joint.transform.childCount;
for (int Idx = 0; Idx < numChildren; ++Idx)
{
Transform child = joint.transform.GetChild(Idx);
if (child.GetComponent <UrdfJoint>()) // don't want to draw the other joints with this jointPlacement
{
continue;
}
foreach (MeshFilter mfilter in child.GetComponentsInChildren <MeshFilter>())
{
Vector3 localMeshOffset = jointPlacement.Rotation * joint.transform.InverseTransformPoint(mfilter.transform.position);
Quaternion localMeshRotation = Quaternion.Inverse(joint.transform.rotation) * mfilter.transform.rotation;
drawing.DrawMesh(
mfilter.sharedMesh,
jointPlacement.Position + localMeshOffset,
jointPlacement.Rotation * localMeshRotation,
Vector3.one,
color
);
}
}
}
}
public void ExportDefaultJointData_DefaultJoint_Succeeds()
{
Vector3 position = new Vector3(1, 2, 3);
Quaternion rotation = Quaternion.Euler(4, 5, 6);
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
linkObject.transform.position = position;
linkObject.transform.rotation = rotation;
Joint joint = UrdfJoint.ExportDefaultJoint(linkObject.transform);
Assert.AreEqual("base_link_joint", joint.name);
Assert.AreEqual("fixed", joint.type);
Assert.AreEqual(baseObject.name, joint.parent);
Assert.AreEqual(linkObject.name, joint.child);
Assert.AreEqual(new double[] { position[2], -position[0], position[1] }, joint.origin.Xyz);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[2] * Mathf.Deg2Rad, (float)joint.origin.Rpy[0]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(rotation.eulerAngles[0] * Mathf.Deg2Rad, (float)joint.origin.Rpy[1]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[1] * Mathf.Deg2Rad, (float)joint.origin.Rpy[2]);
Object.DestroyImmediate(baseObject);
Object.DestroyImmediate(linkObject);
}
public void ExportJointData_ArbitraryJointData_Succeeds()
{
Vector3 position = new Vector3(1, 2, 3);
Quaternion rotation = Quaternion.Euler(4, 5, 6);
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
linkObject.transform.position = position;
linkObject.transform.rotation = rotation;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Revolute);
var joint = linkObject.GetComponent <UrdfJoint>().ExportJointData();
Assert.IsNull(joint.name);
Assert.AreEqual("revolute", joint.type);
Assert.AreEqual(baseObject.name, joint.parent);
Assert.AreEqual(linkObject.name, joint.child);
Assert.AreEqual(new double[] { position[2], -position[0], position[1] }, joint.origin.Xyz);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[2] * Mathf.Deg2Rad, (float)joint.origin.Rpy[0]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(rotation.eulerAngles[0] * Mathf.Deg2Rad, (float)joint.origin.Rpy[1]);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(-rotation.eulerAngles[1] * Mathf.Deg2Rad, (float)joint.origin.Rpy[2]);
Object.DestroyImmediate(baseObject);
Object.DestroyImmediate(linkObject);
}
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 void Start()
{
jointStateWriter = GetComponent <JointStateWriter>();
urdfJoint = GetComponent <UrdfJoint>();
hingeJointLimitsManager = GetComponent <HingeJointLimitsManager>();
prismaticJointLimitsManager = GetComponent <PrismaticJointLimitsManager>();
useLimits = (hingeJointLimitsManager != null || prismaticJointLimitsManager != null);
}
public RobotLink(string link_name, UrdfJoint urdf_joint, JointStateWriter joint_writer, Dictionary <UrdfVisual, bool> visuals_gameObject = null, bool is_base_link = false)
{
LinkName = link_name;
UrdfJoint = urdf_joint;
jointWriter = joint_writer;
Visuals = visuals_gameObject ?? new Dictionary <UrdfVisual, bool>();
IsBaseLink = is_base_link;
}
public void GetJointType_AllJointTypes_Succeeds()
{
Assert.AreEqual(UrdfJoint.JointTypes.Fixed, UrdfJoint.GetJointType("fixed"));
Assert.AreEqual(UrdfJoint.JointTypes.Continuous, UrdfJoint.GetJointType("continuous"));
Assert.AreEqual(UrdfJoint.JointTypes.Revolute, UrdfJoint.GetJointType("revolute"));
Assert.AreEqual(UrdfJoint.JointTypes.Floating, UrdfJoint.GetJointType("floating"));
Assert.AreEqual(UrdfJoint.JointTypes.Prismatic, UrdfJoint.GetJointType("prismatic"));
Assert.AreEqual(UrdfJoint.JointTypes.Planar, UrdfJoint.GetJointType("planar"));
Assert.AreEqual(UrdfJoint.JointTypes.Fixed, UrdfJoint.GetJointType("unknown"));
}
public RobotLink(string link_name, UrdfJoint urdf_joint, JointStateWriter joint_writer, JointStateReader joint_reader, Dictionary <UrdfVisual, bool> visuals_gameObject = null, Dictionary <UrdfCollision, bool> collisions_gameObject = null, bool is_base_link = false, float scale = 1f)
{
LinkName = link_name;
UrdfJoint = urdf_joint;
jointWriter = joint_writer;
jointReader = joint_reader;
Visuals = visuals_gameObject ?? new Dictionary <UrdfVisual, bool>();
Collisions = collisions_gameObject ?? new Dictionary <UrdfCollision, bool>();
IsBaseLink = is_base_link;
LinkScale = scale;
}
public void Create_UrdfJointRevolute_Succeeds()
{
GameObject linkObject = new GameObject("link");
UrdfJoint joint = UrdfJointRevolute.Create(linkObject);
UrdfJoint urdfJoint = linkObject.GetComponent <UrdfJoint>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.IsTrue(joint is UrdfJointRevolute);
Assert.AreEqual(joint, urdfJoint);
Assert.AreEqual(ArticulationJointType.RevoluteJoint, articulationBody.jointType);
Object.DestroyImmediate(linkObject);
}
public void Create_UrdfJoint_Succeeds(UrdfJoint.JointTypes urdfJointType, ArticulationJointType articulationJointType)
{
GameObject linkObject = new GameObject("link");
UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, urdfJointType);
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.IsNotNull(urdfJoint);
Assert.IsNotNull(articulationBody);
Assert.AreEqual(urdfJointType, urdfJoint.JointType);
Assert.AreEqual(articulationJointType, articulationBody.jointType);
Object.DestroyImmediate(linkObject);
}
public void Create_WithOtherTypeOfJointData_FixedArticulationBody()
{
Joint joint = new Joint(
name: "reference", type: "prismatic", parent: null, child: null);
GameObject linkObject = new GameObject("link");
UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Fixed, joint);
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.IsNotNull(urdfJoint);
Assert.IsNotNull(articulationBody);
Assert.AreEqual(UrdfJoint.JointTypes.Fixed, urdfJoint.JointType);
Assert.AreEqual(ArticulationJointType.FixedJoint, articulationBody.jointType);
}
public void AreLimitsCorrect_Fails()
{
var joint = new Joint(
name: "custom_joint", type: "planar", parent: "base", child: "link",
limit: new Joint.Limit(5, 4, 6, 7),
dynamics: new Joint.Dynamics(8, 9));
GameObject linkObject = new GameObject("link");
UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Revolute, joint);
Assert.IsFalse(urdfJoint.AreLimitsCorrect());
Object.DestroyImmediate(linkObject);
}
public void AreLimitsCorrect_Succeeds()
{
var joint = new Joint(
name: "custom_joint", type: "planar", parent: "base", child: "link",
limit: new Joint.Limit(4, 5, 6, 7),
dynamics: new Joint.Dynamics(8, 9));
GameObject linkObject = new GameObject("link");
UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Prismatic, joint);
Assert.IsTrue(urdfJoint.AreLimitsCorrect());
Object.DestroyImmediate(linkObject);
}
public void GetPosition_Succeeds()
{
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
linkObject.transform.localPosition = new Vector3(1, 2, 3);
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
UrdfJoint joint = UrdfJointPlanar.Create(linkObject);
Assert.AreEqual(linkObject.transform.localPosition.magnitude, joint.GetPosition());
Object.DestroyImmediate(baseObject);
}
public void Create_WithJointData_Succeeds()
{
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
var joint = new Joint("custom_name", "revolute", "base", "link");
UrdfJoint urdfJoint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Prismatic, joint);
Assert.AreEqual("custom_name", urdfJoint.jointName);
Object.DestroyImmediate(baseObject);
Object.DestroyImmediate(linkObject);
}
public void GenerateUniqueJointName_UniqueName_Succeeds()
{
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
var joint = UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Revolute);
Assert.IsNull(joint.jointName);
joint.GenerateUniqueJointName();
Assert.NotNull(joint.jointName);
Object.DestroyImmediate(baseObject);
Object.DestroyImmediate(linkObject);
}
public override void OnInspectorGUI()
{
GUILayout.Space(5);
urdfLink.IsBaseLink = EditorGUILayout.Toggle("Is Base Link", urdfLink.IsBaseLink);
GUILayout.Space(5);
EditorGUILayout.BeginVertical("HelpBox");
jointType = (UrdfJoint.JointTypes)EditorGUILayout.EnumPopup(
"Child Joint Type", jointType);
if (GUILayout.Button("Add child link (with joint)"))
{
UrdfLink childLink = UrdfLinkExtensions.Create(urdfLink.transform);
UrdfJoint.Create(childLink.gameObject, jointType);
}
EditorGUILayout.EndVertical();
}
public void UpdateJointState_Succeeds()
{
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
UrdfJoint joint = UrdfJointRevolute.Create(linkObject);
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.AreEqual(0, articulationBody.xDrive.target);
joint.UpdateJointState(1);
Assert.AreEqual(1 * Mathf.Rad2Deg, articulationBody.xDrive.target);
Object.DestroyImmediate(baseObject);
}
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 void GetPositionVelocityEffort_Succeeds()
{
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
UrdfJoint joint = UrdfJointPrismatic.Create(linkObject);
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
articulationBody.jointPosition = new ArticulationReducedSpace(1, 2, 3);
articulationBody.jointVelocity = new ArticulationReducedSpace(4, 5, 6);
articulationBody.jointForce = new ArticulationReducedSpace(7, 8, 9);
Assert.AreEqual(1, joint.GetPosition());
Assert.AreEqual(4, joint.GetVelocity());
Assert.AreEqual(7, joint.GetEffort());
Object.DestroyImmediate(baseObject);
}
private static Robot ExportRobotData(this UrdfRobot urdfRobot)
{
Robot robot = new Robot();
robot.ConstructFromFile(urdfRobot.FilePath, urdfRobot.gameObject.name);
List <string> linkNames = new List <string>();
foreach (UrdfLink urdfLink in urdfRobot.GetComponentsInChildren <UrdfLink>())
{
//Link export
if (linkNames.Contains(urdfLink.name))
{
EditorUtility.DisplayDialog("URDF Export Error",
"URDF export failed. There are several links with the name " +
urdfLink.name + ". Make sure all link names are unique before exporting this robot.",
"Ok");
return(null);
}
robot.links.Add(urdfLink.ExportLinkData());
linkNames.Add(urdfLink.name);
//Joint export
UrdfJoint urdfJoint = urdfLink.gameObject.GetComponent <UrdfJoint>();
if (urdfJoint != null)
{
robot.joints.Add(urdfJoint.ExportJointData());
}
else if (!urdfLink.IsBaseLink)
{
//Make sure that links with no rigidbodies are still connected to the robot by a default joint
robot.joints.Add(UrdfJoint.ExportDefaultJoint(urdfLink.transform));
}
}
robot.materials = UrdfMaterial.Materials.Values.ToList();
robot.plugins = urdfRobot.GetComponentInChildren <UrdfPlugins>().ExportPluginsData();
return(robot);
}
public void ImportJointData_SpecificAixs_Succeeds()
{
var joint = new Joint(
name: "custom_joint", type: "prismatic", parent: "base", child: "link",
axis: new Joint.Axis(new double[] { 1, 2, 3 }),
limit: new Joint.Limit(4, 5, 6, 7),
dynamics: new Joint.Dynamics(8, 9));
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
TestUrdfJointRevolute urdfJoint = linkObject.AddComponent <TestUrdfJointRevolute>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
urdfJoint.TestImportJointData(joint);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.linearLockX);
Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockY);
Assert.AreEqual(ArticulationDofLock.LockedMotion, articulationBody.linearLockZ);
Assert.AreEqual(ArticulationDofLock.LimitedMotion, articulationBody.twistLock);
Quaternion expectedAnchorRotation = new Quaternion();
expectedAnchorRotation.SetFromToRotation(new Vector3(1, 0, 0), -new Vector3(-2, 3, 1));
Assert.AreEqual(expectedAnchorRotation, articulationBody.anchorRotation);
Assert.AreEqual(4 * Mathf.Rad2Deg, articulationBody.xDrive.lowerLimit);
Assert.AreEqual(5 * Mathf.Rad2Deg, articulationBody.xDrive.upperLimit);
Assert.AreEqual(6, articulationBody.xDrive.forceLimit);
Assert.AreEqual(7, articulationBody.maxAngularVelocity);
Assert.AreEqual(8, articulationBody.linearDamping);
Assert.AreEqual(8, articulationBody.angularDamping);
Assert.AreEqual(9, articulationBody.jointFriction);
Object.DestroyImmediate(baseObject);
}
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 void ChangeJointType_FromFixedToRevolute_Succeeds()
{
GameObject baseLink = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseLink.transform;
UrdfJoint.Create(baseLink, UrdfJoint.JointTypes.Fixed);
UrdfJoint.Create(linkObject, UrdfJoint.JointTypes.Fixed);
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.AreEqual(ArticulationJointType.FixedJoint, articulationBody.jointType);
Assert.AreEqual(0, articulationBody.dofCount);
UrdfJoint.ChangeJointType(linkObject, UrdfJoint.JointTypes.Revolute);
articulationBody = linkObject.GetComponent <ArticulationBody>();
Assert.AreEqual(ArticulationJointType.RevoluteJoint, articulationBody.jointType);
Assert.AreEqual(1, articulationBody.dofCount);
Object.DestroyImmediate(linkObject);
}
public void ImportJointData_DefaultAxis_Succeeds(Joint.Axis axis, Quaternion expectedAnchorRotation)
{
var joint = new Joint(
name: "custom_joint", type: "continuous", parent: "base", child: "link",
axis: axis);
GameObject baseObject = new GameObject("base");
GameObject linkObject = new GameObject("link");
linkObject.transform.parent = baseObject.transform;
UrdfJoint.Create(baseObject, UrdfJoint.JointTypes.Fixed);
TestUrdfJointPlanar urdfJoint = linkObject.AddComponent <TestUrdfJointPlanar>();
ArticulationBody articulationBody = linkObject.GetComponent <ArticulationBody>();
urdfJoint.TestImportJointData(joint);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.w, articulationBody.anchorRotation.w);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.x, articulationBody.anchorRotation.x);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.y, articulationBody.anchorRotation.y);
UnityEngine.Assertions.Assert.AreApproximatelyEqual(expectedAnchorRotation.z, articulationBody.anchorRotation.z);
Object.DestroyImmediate(baseObject);
}
public override void OnInspectorGUI()
{
urdfJoint = (UrdfJoint)target;
GUILayout.Space(5);
UrdfJoint.JointTypes newJointType = urdfJoint.JointType;
EditorGUILayout.BeginVertical("HelpBox");
newJointType = (UrdfJoint.JointTypes)EditorGUILayout.EnumPopup(
"Type of joint", newJointType);
if (newJointType != urdfJoint.JointType)
{
if (EditorUtility.DisplayDialog("Confirm joint type change",
"Are you sure you want to change the joint type? This will erase all information currently stored in the joint.",
"Continue", "Cancel"))
{
UrdfJoint.ChangeJointType(urdfJoint.gameObject, newJointType);
}
}
EditorGUILayout.EndVertical();
showDetails = EditorGUILayout.Foldout(showDetails, "Joint URDF Configuration", true);
if (showDetails)
{
urdfJoint.JointName = EditorGUILayout.TextField("Name", urdfJoint.JointName);
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
GUILayout.BeginVertical("HelpBox");
}
switch (urdfJoint.JointType)
{
case UrdfJoint.JointTypes.Fixed:
break;
case UrdfJoint.JointTypes.Continuous:
DisplayDynamicsMessage("HingeJoint > Spring > Damper (for damping) and Spring (for friction)");
DisplayAxisMessage("HingeJoint > Axis");
break;
case UrdfJoint.JointTypes.Revolute:
DisplayDynamicsMessage("HingeJoint > Spring > Damper (for damping) and Spring (for friction)");
DisplayAxisMessage("HingeJoint > Axis");
DisplayRequiredLimitMessage("Hinge Joint Limits Manager > Large Angle Limit / Max");
break;
case UrdfJoint.JointTypes.Floating:
DisplayDynamicsMessage("ConfigurableJoint > xDrive > Position Damper (for Damping) and Position Spring (for friction)");
break;
case UrdfJoint.JointTypes.Prismatic:
DisplayDynamicsMessage("ConfigurableJoint > xDrive > Position Damper (for Damping) and Position Spring (for friction)");
DisplayAxisMessage("ConfigurableJoint > Axis");
DisplayRequiredLimitMessage("Prismatic Joint Limits Manager > Position Limit Min / Max");
break;
case UrdfJoint.JointTypes.Planar:
DisplayDynamicsMessage("ConfigurableJoint > xDrive > Position Damper (for Damping) and Position Spring (for friction)");
DisplayAxisMessage("ConfigurableJoint > Axis and Secondary Axis");
DisplayRequiredLimitMessage("ConfigurableJoint > Linear Limit > Limit");
break;
}
if (urdfJoint.JointType != UrdfJoint.JointTypes.Fixed)
{
GUILayout.EndVertical();
}
}
}
private void Start()
{
urdfJoint = GetComponent <UrdfJoint>();
}
