// adds a joint via URDFJoint
public bool AddJoint(URDFJoint urdfj)
{
if (!joints.ContainsKey(urdfj.name))
{
joints.Add(urdfj.name, urdfj);
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;
}
// Set the angle of a joint
public void SetAngle(string name, float angle) {
URDFJoint joint = joints[name];
joint.setAngle(angle);
}
