// Adds the URDFLink to the list
public bool AddLink(URDFLink urdfl)
{
if (!links.ContainsKey(urdfl.name))
{
links.Add(urdfl.name, urdfl);
return(true);
}
return(false);
}
// Adds the URDFLink to the list
public bool AddLink(URDFLink link) {
if (!links.ContainsKey(link.name)) {
links.Add(link.name, link);
return true;
}
return false;
}
// Validates the consistency of the URDF structure
public bool IsConsistent(ref string errorMsg) {
errorMsg = "";
// verify that
// * every joint's name matches its key
// * every joint specifies a joint type
// * both parent and child match
foreach( KeyValuePair<string, URDFJoint> kv in joints ) {
URDFJoint j = kv.Value;
if (j.name != kv.Key) {
errorMsg = string.Format("Joint \"{0}'s\" name does not match key \"{1}\"", j.name, kv.Key);
return false;
}
if (j.type == "") {
errorMsg = string.Format("Joint \"{0}'s\" type is not set", j.name);
return false;
}
if (j.parentLink == null) {
errorMsg = string.Format("Joint \"{0}\" does not have a parent link", j.name);
return false;
}
if (!j.parentLink.children.Contains(j)) {
errorMsg = string.Format("Joint \"{0}'s\" parent link \"{1}\" does not contain it as a child", j.name, j.parentLink.name);
return false;
}
if (j.childLink == null) {
errorMsg = string.Format("Joint \"{0}\" does not have a child link", j.name);
return false;
}
if (j.childLink.parent != j) {
errorMsg = string.Format("Joint \"{0}'s\" child link \"{1}\" does not have it as a parent", j.name, j.childLink.name);
return false;
}
}
// verify that
// * every link's name matches it key
// * every parent and child matches
foreach (KeyValuePair<string, URDFLink> kv in links) {
URDFLink l = kv.Value;
if (l.name != kv.Key) {
errorMsg = string.Format("Link \"{0}'s\" name does not match key \"{1}\"", l.name, kv.Key);
return false;
}
if (l.parent != null && l.parent.childLink != l) {
errorMsg = string.Format("Link \"{0}'s\" parent joint \"{1}\" does not have it as a child", l.name, l.parent.name);
return false;
}
foreach (URDFJoint j in l.children) {
if (j.parentLink != l) {
errorMsg = string.Format("Link \"{0}'s\" child joint \"{1}\" does not have it as a parent", l.name, j.name);
return false;
}
}
}
return true;
}
