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); } }
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); } }