Basic URDF loader

BulletSharpPInvoke/demos/DemoFramework/DemoFramework.csproj

@@ -60,6 +60,8 @@
     <Reference Include="System.XML" />
   </ItemGroup>
   <ItemGroup>
+    <Compile Include="FileLoaders\UrdfLoader.cs" />
+    <Compile Include="FileLoaders\UrdfToBullet.cs" />
     <Compile Include="IUpdateReceiver.cs" />
     <Compile Include="Simulation\BoxShooter.cs" />
     <Compile Include="Clock.cs" />

BulletSharpPInvoke/demos/DemoFramework/FileLoaders/UrdfLoader.cs

@@ -0,0 +1,206 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.Xml;
+
+namespace DemoFramework.FileLoaders
+{
+    public static class UrdfLoader
+    {
+        public static UrdfRobot FromFile(string filename)
+        {
+            var document = new XmlDocument();
+            document.Load(filename);
+            return ParseRobot(document["robot"]);
+        }
+
+        private static UrdfRobot ParseRobot(XmlElement element)
+        {
+            var robot = new UrdfRobot
+            {
+                Name = element.GetAttribute("name")
+            };
+
+            foreach (XmlElement linkElement in element.SelectNodes("link"))
+            {
+                var link = ParseLink(linkElement);
+                robot.Links.Add(link);
+            }
+
+            return robot;
+        }
+
+        private static UrdfLink ParseLink(XmlElement element)
+        {
+            return new UrdfLink
+            {
+                Name = element.GetAttribute("name"),
+                Collision = ParseCollision(element["collision"]),
+                Inertial = ParseInertial(element["inertial"]),
+            };
+        }
+
+        private static UrdfCollision ParseCollision(XmlElement element)
+        {
+            if (element == null)
+            {
+                return null;
+            }
+            return new UrdfCollision
+            {
+                Geometry = ParseGeometry(element["geometry"]),
+                Origin = ParseOrigin(element["origin"])
+            };
+        }
+
+        private static UrdfInertial ParseInertial(XmlElement element)
+        {
+            if (element == null)
+            {
+                return null;
+            }
+            return new UrdfInertial
+            {
+                Mass = ParseMass(element["mass"])
+            };
+        }
+
+        private static double ParseMass(XmlElement element)
+        {
+            return double.Parse(element.Attributes["value"].Value, CultureInfo.InvariantCulture);
+        }
+
+        private static UrdfGeometry ParseGeometry(XmlElement element)
+        {
+            var shapeElement = element.SelectSingleNode("box|cylinder|mesh|sphere");
+            switch (shapeElement.Name)
+            {
+                case "box":
+                    return new UrdfBox
+                    {
+                        Size = shapeElement.Attributes["size"].Value
+                    };
+                case "cylinder":
+                    return new UrdfCylinder
+                    {
+                        Radius = double.Parse(
+                            shapeElement.Attributes["radius"].Value,
+                            CultureInfo.InvariantCulture),
+                        Length = double.Parse(
+                            shapeElement.Attributes["length"].Value,
+                            CultureInfo.InvariantCulture)
+                    };
+                case "mesh":
+                    return new UrdfMesh
+                    {
+                        FileName = shapeElement.Attributes["filename"].Value,
+                        Scale = shapeElement.Attributes["scale"]?.Value
+                    };
+                case "sphere":
+                    return new UrdfSphere
+                    {
+                        Radius = double.Parse(
+                            shapeElement.Attributes["radius"].Value,
+                            CultureInfo.InvariantCulture)
+                    };
+            }
+            throw new NotSupportedException();
+        }
+
+        private static UrdfPose ParseOrigin(XmlElement element)
+        {
+            if (element == null)
+            {
+                return null;
+            }
+            return new UrdfPose
+            {
+                Position = element.Attributes["xyz"].Value,
+                RollPitchYaw = element.Attributes["rpy"].Value
+            };
+        }
+    }
+
+    public class UrdfRobot
+    {
+        public string Name { get; set; }
+        public List<UrdfLink> Links { get; } = new List<UrdfLink>();
+
+        public override string ToString()
+        {
+            return Name;
+        }
+    }
+
+    public class UrdfLink
+    {
+        public string Name { get; set; }
+        public UrdfCollision Collision { get; set; }
+        public UrdfInertial Inertial { get; set; }
+
+        public override string ToString()
+        {
+            return Name;
+        }
+    }
+
+    public class UrdfCollision
+    {
+        public UrdfGeometry Geometry { get; set; }
+        public UrdfPose Origin { get; set; }
+    }
+
+    public class UrdfInertial
+    {
+        public double Mass { get; set; }
+    }
+
+    public enum UrdfGeometryType
+    {
+        Box,
+        Cylinder,
+        Mesh,
+        Sphere
+    }
+
+    public abstract class UrdfGeometry
+    {
+        public abstract UrdfGeometryType Type { get; }
+    }
+
+    public class UrdfBox : UrdfGeometry
+    {
+        public override UrdfGeometryType Type => UrdfGeometryType.Box;
+
+        public string Size { get; set; }
+    }
+
+    public class UrdfCylinder : UrdfGeometry
+    {
+        public override UrdfGeometryType Type => UrdfGeometryType.Cylinder;
+
+        public double Length { get; set; }
+        public double Radius { get; set; }
+    }
+
+    public class UrdfMesh : UrdfGeometry
+    {
+        public override UrdfGeometryType Type => UrdfGeometryType.Mesh;
+
+        public string FileName { get; set; }
+        public string Scale { get; set; }
+    }
+
+    public class UrdfSphere : UrdfGeometry
+    {
+        public override UrdfGeometryType Type => UrdfGeometryType.Sphere;
+
+        public double Radius { get; set; }
+    }
+
+    public class UrdfPose
+    {
+        public string Position { get; set; }
+        public string RollPitchYaw { get; set; }
+    }
+}

BulletSharpPInvoke/demos/DemoFramework/FileLoaders/UrdfToBullet.cs

@@ -0,0 +1,136 @@
+using BulletSharp;
+using BulletSharp.Math;
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+using System.IO;
+
+namespace DemoFramework.FileLoaders
+{
+    public class UrdfToBullet
+    {
+        private static char[] _spaceSeparator = new[] { ' ' };
+
+        public UrdfToBullet(DiscreteDynamicsWorld world)
+        {
+            World = world;
+        }
+
+        public DiscreteDynamicsWorld World { get; private set; }
+
+        public void Convert(UrdfRobot robot, string baseDirectory)
+        {
+            foreach (UrdfLink link in robot.Links)
+            {
+                LoadLink(link, baseDirectory);
+            }
+        }
+
+        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 Matrix ParsePose(UrdfPose pose)
+        {
+            if (pose == null)
+            {
+                return Matrix.Identity;
+            }
+            Vector3 rpy = ParseVector3(pose.RollPitchYaw);
+            Matrix matrix = Matrix.RotationYawPitchRoll(rpy.Z, rpy.Y, rpy.X);
+            matrix.Origin = ParseVector3(pose.Position);
+            return matrix;
+        }
+
+        private static Vector3 ParseVector3(string vector)
+        {
+            string[] components = vector.Split(_spaceSeparator, StringSplitOptions.RemoveEmptyEntries);
+            return new Vector3(
+                float.Parse(components[0], CultureInfo.InvariantCulture),
+                float.Parse(components[1], CultureInfo.InvariantCulture),
+                float.Parse(components[2], CultureInfo.InvariantCulture));
+        }
+
+        private void LoadFile(string fileName, string baseDirectory, Matrix transform)
+        {
+            string fullPath = Path.Combine(baseDirectory, fileName);
+            string extension = Path.GetExtension(fullPath);
+            switch (extension)
+            {
+                case ".obj":
+                    WavefrontObj obj = WavefrontObj.Load(fullPath);
+                    var mesh = CreateTriangleMesh(obj.Indices, obj.Vertices, Vector3.One);
+                    CreateMeshBody(mesh, transform);
+                    break;
+                default:
+                    throw new NotSupportedException();
+            }
+        }
+
+        private static TriangleMesh CreateTriangleMesh(List<int> indices, List<Vector3> vertices, Vector3 localScaling)
+        {
+            var triangleMesh = new TriangleMesh();
+
+            int triangleCount = indices.Count / 3;
+            for (int i = 0; i < triangleCount; i++)
+            {
+                int index0 = indices[i * 3];
+                int index1 = indices[i * 3 + 1];
+                int index2 = indices[i * 3 + 2];
+
+                Vector3 vertex0 = vertices[index0] * localScaling;
+                Vector3 vertex1 = vertices[index1] * localScaling;
+                Vector3 vertex2 = vertices[index2] * localScaling;
+
+                triangleMesh.AddTriangleRef(ref vertex0, ref vertex1, ref vertex2);
+            }
+
+            return triangleMesh;
+        }
+
+        private void CreateMeshBody(TriangleMesh mesh, Matrix transform)
+        {
+            const bool useQuantization = true;
+            var concaveShape = new BvhTriangleMeshShape(mesh, useQuantization);
+            PhysicsHelper.CreateStaticBody(transform, concaveShape, World);
+        }
+    }
+}

BulletSharpPInvoke/demos/DemoFramework/FileLoaders/WavefrontObj.cs

@@ -12,6 +12,7 @@ namespace DemoFramework.FileLoaders
         private readonly char[] _lineSplitChars = { ' ' };
 
         private List<Vector3> _vertices = new List<Vector3>();
+        private Dictionary<Vector3, int> _vertexMap = new Dictionary<Vector3, int>();
 
         private WavefrontObj(string filename)
         {
@@ -58,6 +59,11 @@ namespace DemoFramework.FileLoaders
                     break;
                 case "f":
                     int numVertices = parts.Length - 1;
+                    if (numVertices < 3 || numVertices > 4)
+                    {
+                        break;
+                    }
+
                     int[] face = new int[numVertices];
 
                     face[0] = GetVertex(parts[1].Split(_faceSplitSchars, StringSplitOptions.RemoveEmptyEntries));
@@ -98,18 +104,16 @@ namespace DemoFramework.FileLoaders
             }
             Vector3 position = _vertices[vertexIndex - 1];
 
-            // Search for a duplicate
-            for (int i = 0; i < Vertices.Count; i++)
+            int existingIndex;
+            if (_vertexMap.TryGetValue(position, out existingIndex))
             {
-                if (Vertices[i].Equals(position))
-                {
-                    Indices.Add(i);
-                    return i;
-                }
+                Indices.Add(existingIndex);
+                return existingIndex;
             }
 
             int newIndex = Vertices.Count;
             Vertices.Add(position);
+            _vertexMap.Add(position, newIndex);
             Indices.Add(newIndex);
             return newIndex;
         }

BulletSharpPInvoke/demos/Demos.sln

@@ -165,6 +165,13 @@ Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "PendulumDemo", "PendulumDem
 		{67DF42D4-C318-4E52-9017-43A2EFD7790B} = {67DF42D4-C318-4E52-9017-43A2EFD7790B}
 	EndProjectSection
 EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "UrdfDemo", "UrdfDemo\UrdfDemo.csproj", "{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}"
+	ProjectSection(ProjectDependencies) = postProject
+		{540E0C60-3FBD-4166-933D-D80284D1CA0E} = {540E0C60-3FBD-4166-933D-D80284D1CA0E}
+		{3CD13788-CD50-451F-8267-AF7578403621} = {3CD13788-CD50-451F-8267-AF7578403621}
+		{67DF42D4-C318-4E52-9017-43A2EFD7790B} = {67DF42D4-C318-4E52-9017-43A2EFD7790B}
+	EndProjectSection
+EndProject
 Global
 	GlobalSection(SolutionConfigurationPlatforms) = preSolution
 		Debug|x86 = Debug|x86
@@ -273,6 +280,10 @@ Global
 		{56363BE1-F548-45A7-9DC1-7DCF7B5A97CF}.Debug|x86.Build.0 = Debug|x86
 		{56363BE1-F548-45A7-9DC1-7DCF7B5A97CF}.Release|x86.ActiveCfg = Release|x86
 		{56363BE1-F548-45A7-9DC1-7DCF7B5A97CF}.Release|x86.Build.0 = Release|x86
+		{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}.Debug|x86.ActiveCfg = Debug|x86
+		{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}.Debug|x86.Build.0 = Debug|x86
+		{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}.Release|x86.ActiveCfg = Release|x86
+		{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}.Release|x86.Build.0 = Release|x86
 	EndGlobalSection
 	GlobalSection(SolutionProperties) = preSolution
 		HideSolutionNode = FALSE

BulletSharpPInvoke/demos/UrdfDemo/UrdfDemo.cs

@@ -0,0 +1,82 @@
+using BulletSharp;
+using BulletSharp.Math;
+using DemoFramework;
+using DemoFramework.FileLoaders;
+using System;
+using System.IO;
+
+namespace UrdfDemo
+{
+    internal static class Program
+    {
+        [STAThread]
+        static void Main()
+        {
+            DemoRunner.Run<UrdfDemo>();
+        }
+    }
+
+    internal sealed class UrdfDemo : IDemoConfiguration
+    {
+        public ISimulation CreateSimulation(Demo demo)
+        {
+            demo.FreeLook.Eye = new Vector3(1, 2, 1);
+            demo.FreeLook.Target = new Vector3(0, 0, 0);
+            demo.Graphics.WindowTitle = "BulletSharp - URDF Demo";
+            return new UrdfDemoSimulation();
+        }
+    }
+
+    internal sealed class UrdfDemoSimulation : ISimulation
+    {
+        private const int NumBoxesX = 5, NumBoxesY = 5, NumBoxesZ = 5;
+        private Vector3 _startPosition = new Vector3(0, 2, 0);
+
+        public UrdfDemoSimulation()
+        {
+            CollisionConfiguration = new DefaultCollisionConfiguration();
+            Dispatcher = new CollisionDispatcher(CollisionConfiguration);
+            Broadphase = new DbvtBroadphase();
+            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConfiguration);
+
+            CreateGround();
+
+            string[] args = Environment.GetCommandLineArgs();
+            if (args.Length == 1)
+            {
+                LoadUrdf("hinge.urdf");
+            }
+            else
+            {
+                LoadUrdf(args[1]);
+            }
+        }
+
+        public CollisionConfiguration CollisionConfiguration { get; }
+        public CollisionDispatcher Dispatcher { get; }
+        public BroadphaseInterface Broadphase { get; }
+        public DiscreteDynamicsWorld World { get; }
+
+        public void Dispose()
+        {
+            this.StandardCleanup();
+        }
+
+        private void CreateGround()
+        {
+            var groundShape = new BoxShape(50, 1, 50);
+            CollisionObject ground = PhysicsHelper.CreateStaticBody(
+                Matrix.Translation(0, -2, 0), groundShape, World);
+            ground.UserObject = "Ground";
+        }
+
+        private void LoadUrdf(string fileName)
+        {
+            string baseDirectory = "data";
+            string path = Path.Combine(baseDirectory, fileName);
+            UrdfRobot robot = UrdfLoader.FromFile(path);
+            new UrdfToBullet(World).Convert(robot, baseDirectory);
+        }
+
+    }
+}

BulletSharpPInvoke/demos/UrdfDemo/UrdfDemo.csproj

@@ -0,0 +1,82 @@
+<?xml version="1.0" encoding="utf-8"?>
+<Project ToolsVersion="14.0" DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">x86</Platform>
+    <ProjectGuid>{70DB3AEB-5DAC-4B77-9860-6280AF8800B8}</ProjectGuid>
+    <OutputType>WinExe</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>UrdfDemo</RootNamespace>
+    <AssemblyName>UrdfDemo</AssemblyName>
+    <TargetFrameworkVersion>v4.0</TargetFrameworkVersion>
+    <FileAlignment>512</FileAlignment>
+    <AutoGenerateBindingRedirects>true</AutoGenerateBindingRedirects>
+    <TargetFrameworkProfile>Client</TargetFrameworkProfile>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>..\bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <PlatformTarget>AnyCPU</PlatformTarget>
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>..\bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Debug|x86'">
+    <DebugSymbols>true</DebugSymbols>
+    <OutputPath>..\bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <DebugType>full</DebugType>
+    <PlatformTarget>x86</PlatformTarget>
+    <ErrorReport>prompt</ErrorReport>
+    <CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
+  </PropertyGroup>
+  <PropertyGroup Condition="'$(Configuration)|$(Platform)' == 'Release|x86'">
+    <OutputPath>..\bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <Optimize>true</Optimize>
+    <DebugType>pdbonly</DebugType>
+    <PlatformTarget>x86</PlatformTarget>
+    <ErrorReport>prompt</ErrorReport>
+    <CodeAnalysisRuleSet>MinimumRecommendedRules.ruleset</CodeAnalysisRuleSet>
+  </PropertyGroup>
+  <ItemGroup>
+    <Compile Include="UrdfDemo.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\DemoFramework\DemoFramework.csproj">
+      <Project>{7f50b160-6c4d-467a-9aab-10c516326bd1}</Project>
+      <Name>DemoFramework</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <ItemGroup>
+    <Reference Include="BulletSharp">
+      <HintPath>..\..\bin\Release\BulletSharp.dll</HintPath>
+    </Reference>
+    <Reference Include="System" />
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="data\hinge.urdf">
+      <CopyToOutputDirectory>PreserveNewest</CopyToOutputDirectory>
+    </None>
+  </ItemGroup>
+  <Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
+       Other similar extension points exist, see Microsoft.Common.targets.
+  <Target Name="BeforeBuild">
+  </Target>
+  <Target Name="AfterBuild">
+  </Target>
+  -->
+</Project>

BulletSharpPInvoke/demos/UrdfDemo/data/hinge.urdf

@@ -0,0 +1,73 @@
+<?xml version="1.0" ?>
+<robot name="urdf_robot">
+  <link name="baseLink">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.35"/>
+      <mass value="0.0"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.35"/>
+      <geometry>
+        <box size="0.08 0.16 0.7"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.35"/>
+      <geometry>
+        <box size="0.08 0.16 0.7"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="childA">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <geometry>
+        <box size="0.1  0.2 0.72"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <geometry>
+        <box size="0.1 0.2 0.72 "/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_baseLink_childA" type="continuous">
+    <parent link="baseLink"/>
+    <child link="childA"/>
+    <origin xyz="0 0 0"/>
+    <axis xyz="1 0 0"/>
+  </joint>
+
+  <link name="childB">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <geometry>
+        <sphere radius="0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_childA_childB" type="fixed">
+    <parent link="childA"/>
+    <child link="childB"/>
+    <origin xyz="0 0 -0.2"/>
+  </joint>
+</robot>
+