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);
        }
Esempio n. 3
0
        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);
        }