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