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 static UrdfCollision[] ParseCollisions(XmlElement element)
{
XmlNodeList collisionNodes = element.GetElementsByTagName("collision");
if (collisionNodes.Count == 0)
{
return(Array.Empty <UrdfCollision>());
}
var collisions = new UrdfCollision[collisionNodes.Count];
for (int i = 0; i < collisionNodes.Count; i++)
{
collisions[i] = ParseCollision(collisionNodes[i]);
}
return(collisions);
}
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);
}
}
