private static IDictionary <UrdfLink, UrdfJoint> FindLinkParents(UrdfRobot robot)
{
var linkToParent = new Dictionary <UrdfLink, UrdfJoint>();
foreach (UrdfLink link in robot.Links.Values)
{
UrdfJoint parentJoint = robot.Joints.FirstOrDefault(joint => joint.Child == link);
linkToParent[link] = parentJoint;
}
return(linkToParent);
}
private void LoadJoint(UrdfJoint joint)
{
RigidBody childRigidBody;
if (!_linkToRigidBody.TryGetValue(joint.Child.Name, out childRigidBody))
{
return;
}
RigidBody parentRigidBody;
if (joint.Parent.Collisions.Any())
{
if (!_linkToRigidBody.TryGetValue(joint.Parent.Name, out parentRigidBody))
{
return;
}
}
else
{
parentRigidBody = TypedConstraint.GetFixedBody();
}
TypedConstraint constraint;
if (joint is UrdfContinuousJoint)
{
constraint = CreateRevoluteJoint(childRigidBody, parentRigidBody);
}
else if (joint is UrdfFixedJoint)
{
Matrix childFrame = ParseInertia(joint.Child.Inertial.Inertia);
childFrame = ParsePose(joint.Origin);
constraint = CreateFixedJoint(childRigidBody, parentRigidBody, childFrame);
}
else
{
//throw new NotImplementedException();
return;
}
World.AddConstraint(constraint, true);
}
private static UrdfRobot ParseRobot(XmlElement element)
{
var robot = new UrdfRobot
{
Name = element.GetAttribute("name")
};
foreach (XmlElement linkElement in element.SelectNodes("link"))
{
UrdfLink link = ParseLink(linkElement);
robot.Links[link.Name] = link;
}
foreach (XmlElement jointElement in element.SelectNodes("joint"))
{
UrdfJoint joint = ParseJoint(jointElement, robot);
robot.Joints.Add(joint);
}
return(robot);
}
