private void LoadLink(UrdfLink link, string baseDirectory)
        {
            float        mass     = 0;
            UrdfInertial inertial = link.Inertial;

            if (inertial != null)
            {
                mass = (float)inertial.Mass;
            }

            UrdfCollision collision = link.Collision;

            if (collision != null)
            {
                Matrix       origin   = ParsePose(collision.Origin);
                UrdfGeometry geometry = collision.Geometry;
                switch (geometry.Type)
                {
                case UrdfGeometryType.Box:
                    var     box      = geometry as UrdfBox;
                    Vector3 size     = ParseVector3(box.Size);
                    var     boxShape = new BoxShape(size * 0.5f);
                    PhysicsHelper.CreateBody(mass, origin, boxShape, World);
                    break;

                case UrdfGeometryType.Cylinder:
                    var   cylinder      = geometry as UrdfCylinder;
                    float radius        = (float)cylinder.Radius * 0.5f;
                    float length        = (float)cylinder.Length * 0.5f;
                    var   cylinderShape = new CylinderShape(radius, length, radius);
                    PhysicsHelper.CreateBody(mass, origin, cylinderShape, World);
                    break;

                case UrdfGeometryType.Mesh:
                    var mesh = geometry as UrdfMesh;
                    LoadFile(mesh.FileName, baseDirectory, origin);
                    break;

                case UrdfGeometryType.Sphere:
                    var sphere      = geometry as UrdfSphere;
                    var sphereShape = new SphereShape((float)sphere.Radius);
                    PhysicsHelper.CreateBody(mass, origin, sphereShape, World);
                    break;
                }
            }
        }
        private void LoadCollisions(UrdfLink link, string baseDirectory, Matrix parentTransform)
        {
            if (link.Collisions.Length == 0)
            {
                return;
            }

            UrdfInertial inertial = link.Inertial;
            float        mass     = inertial != null
                ? (float)inertial.Mass
                : 0;

            Matrix origin = ParsePose(link.Inertial.Origin);

            //Matrix inertia = ParseInertia(link.Inertial.Inertia);
            parentTransform = parentTransform * origin;

            CollisionShape shape;
            Matrix         pose;

            if (link.Collisions.Length == 1)
            {
                var collision = link.Collisions[0];
                shape = CreateShapeFromGeometry(collision.Geometry, mass, baseDirectory);
                pose  = parentTransform * ParsePose(collision.Origin);
            }
            else
            {
                shape = LoadCompoundCollisionShape(link.Collisions, baseDirectory, mass, parentTransform);
                pose  = parentTransform;
            }

            RigidBody body;

            if (mass == 0)
            {
                body = PhysicsHelper.CreateStaticBody(pose, shape, World);
            }
            else
            {
                body = PhysicsHelper.CreateBody(mass, pose, shape, World);
            }

            _linkToRigidBody[link.Name] = body;
        }
        private void LoadLink(UrdfLink link, Matrix parentTransform, string baseDirectory)
        {
            float        mass     = 0;
            UrdfInertial inertial = link.Inertial;

            if (inertial != null)
            {
                mass = (float)inertial.Mass;
            }

            Matrix worldTransform = parentTransform;

            UrdfCollision collision = link.Collision;

            if (collision != null)
            {
                Matrix origin = ParsePose(collision.Origin);
                worldTransform = worldTransform * origin;
                UrdfGeometry   geometry = collision.Geometry;
                CollisionShape shape    = CreateShapeFromGeometry(baseDirectory, mass, geometry);
                RigidBody      body;
                if (mass == 0)
                {
                    body = PhysicsHelper.CreateStaticBody(worldTransform, shape, World);
                }
                else
                {
                    body = PhysicsHelper.CreateBody(mass, worldTransform, shape, World);
                }

                _linkToRigidBody[link.Name] = body;
            }

            var children = _linkToParentJoint.Where(l => l.Value?.Parent == link);

            foreach (KeyValuePair <UrdfLink, UrdfJoint> child in children)
            {
                LoadLink(child.Key, worldTransform, baseDirectory);
                LoadJoint(child.Value);
            }
        }
示例#4
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);
        }
        private void LoadLink(UrdfLink link, Matrix parentTransform, string baseDirectory)
        {
            LoadCollisions(link, baseDirectory, parentTransform);

            Matrix    worldTransform;
            RigidBody body;

            if (_linkToRigidBody.TryGetValue(link.Name, out body))
            {
                worldTransform = body.WorldTransform;
            }
            else
            {
                worldTransform = Matrix.Identity;
            }

            var children = _linkToParentJoint.Where(l => l.Value?.Parent == link);

            foreach (KeyValuePair <UrdfLink, UrdfJoint> child in children)
            {
                LoadLink(child.Key, worldTransform, baseDirectory);
                LoadJoint(child.Value);
            }
        }