title: Strange inconsistency when using ComputeSignedDistancePairClosestPoints in pydrake
body: <p>I'm using drake to generate a walking gait for a legged robot. To do this, I need a way to determine when it's appropriate to apply contact forces. For this I'm using ComputeSignedDistancePairClosestPoints between the claws (feet) of the robot and a box which I'm using as the floor (world body):</p>
  <pre><code># Relevant geometry ids
  
  Worldbody_id = inspector.GetAllGeometryIds()[body_list.index('WorldBody')]
  FL_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_FL_3')]
  FR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_FR_3')]
  ML_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_ML_3')]
  MR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_MR_3')]
  BL_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_BL_3')]
  BR_claw_id = inspector.GetAllGeometryIds()[body_list.index('Link_BR_3')]
  
  Claw_id = [FL_claw_id, FR_claw_id, ML_claw_id, MR_claw_id, BL_claw_id, BR_claw_id]
  # Claw_id[0] = FL_claw_id
  # Claw_id[1] = FR_claw_id
  # Claw_id[2] = ML_claw_id
  # Claw_id[3] = MR_claw_id
  # Claw_id[4] = BL_claw_id
  # Claw_id[5] = BR_claw_id
  print(Claw_id)
  </code></pre>
  <p>As a test I'm doing the following:</p>
  <pre><code>Distances = np.zeros(5)
  for i in range(6):
      print(Distances)
      Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i]).distance
  </code></pre>
  <p>Unfortunately this produces the following error:</p>
  <pre><code>[0. 0. 0. 0. 0.]
  [-0.04070674  0.          0.          0.          0.        ]
  ---------------------------------------------------------------------------
  RuntimeError                              Traceback (most recent call last)
  &lt;ipython-input-11-077874ed3da0&gt; in &lt;module&gt;
        2 for i in range(6):
        3     print(Distances)
  ----&gt; 4     Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i]).distance
        5 
        6 
  
  RuntimeError: The geometry given by id 63 does not reference a geometry that can be used in a signed distance query
  </code></pre>
  <p>This implies that the geometry type used by the geometry corresponding to the FR_Claw_id differs from the one corresponding to FL_Claw_id. This is despite the fact that they're both meshes (obj files).</p>
  <p>Does anybody know what I'm not seeing?</p>
  <hr />
  <p>Using a urdf with much simpler geometry (only boxes), this problem persists (this urdf was made by hand, so the intertial data is probably wrong).</p>
  <p>Here's the code used:</p>
  <pre><code>    # Imports required for this programme
  import numpy as np
  import os
  
  from pydrake.common import FindResourceOrThrow, temp_directory
  from pydrake.geometry import (
      MeshcatVisualizerCpp,
      MeshcatVisualizerParams,
      Role,
      StartMeshcat,
  )
  from pydrake.solvers.mathematicalprogram import MathematicalProgram
  from pydrake.math import RigidTransform, RollPitchYaw
  from pydrake.multibody.meshcat import JointSliders
  from pydrake.multibody.parsing import Parser
  from pydrake.multibody.plant import AddMultibodyPlantSceneGraph
  from pydrake.multibody.plant import MultibodyPlant
  from pydrake.multibody.optimization import StaticEquilibriumProblem
  from pydrake.multibody.tree import Joint
  from pydrake.systems.analysis import Simulator
  from pydrake.systems.framework import DiagramBuilder
  from pydrake.all import *
  
  hexapod_file = &quot;/home/remi/catkin_ws/src/hexapod/SCAWeR.urdf&quot;
  builder = DiagramBuilder()
  
  # Note: the time_step here is chosen arbitrarily.
  plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
  
  # Adding a ground plane
  normal_W = [0, 0, 1]
  point_W = [0, 0, 10]
  
  surface_friction = CoulombFriction(
    0.8 # static friction
   ,0.3)# dynamic friction
  
  # A box for the ground geometry.
  plant.RegisterCollisionGeometry(
      plant.world_body(), HalfSpace.MakePose(normal_W, point_W),
      Box(100, 100, 1), &quot;collision&quot;, surface_friction)
  
  # Load the file into the plant/scene_graph.
  parser = Parser(plant)
  parser.package_map().PopulateFromFolder(&quot;/home/remi/catkin_ws/src/hexapod&quot;)
  parser.AddModelFromFile(hexapod_file)
  plant.Finalize()
  
  # create context for plant
  plant_ad = plant.ToAutoDiffXd()
  context_ad = plant_ad.CreateDefaultContext()
  
  # creating an inspector
  diagram = builder.Build()
  diagram_context = diagram.CreateDefaultContext()
  scene_graph_context = scene_graph.GetMyContextFromRoot(diagram_context) 
  query_object = scene_graph.get_query_output_port().Eval(scene_graph_context)
  inspector = query_object.inspector()
  
  # Compiling a list of the names of the bodies in the world (same order as geometry id list)
  body_list = []
  for geometry_id in inspector.GetAllGeometryIds():
      body = plant.GetBodyFromFrameId(inspector.GetFrameId(geometry_id))
      body_list.append(body.name())
      
  # Relevant geometry ids
  Worldbody_id = inspector.GetAllGeometryIds()[body_list.index('WorldBody')]
  FL_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l1_3')]
  FR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r1_3')]
  ML_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l2_3')]
  MR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r2_3')]
  BL_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_l3_3')]
  BR_claw_id = inspector.GetAllGeometryIds()[body_list.index('leg_r3_3')]
  
  # Assigning the claw_ids to a list for ease of use
  Claw_id = [FL_claw_id, FR_claw_id, ML_claw_id, MR_claw_id, BL_claw_id, BR_claw_id]
  # Claw_id[0] = FL_claw_id
  # Claw_id[1] = FR_claw_id
  # Claw_id[2] = ML_claw_id
  # Claw_id[3] = MR_claw_id
  # Claw_id[4] = BL_claw_id
  # Claw_id[5] = BR_claw_id
  
  # Assigning the relevant geometries for ease of use
  Claw_name = ['leg_l1_3', 'leg_r1_3', 'leg_l2_3', 'leg_r2_3', 'leg_l3_3', 'leg_r3_3']
  
  def GetNameFromGeometryID(ID):
      return plant.GetBodyFromFrameId(inspector.GetFrameId(ID)).name()
  
  def SignedDistance(name):
      dList = query_object.ComputeSignedDistancePairwiseClosestPoints()
      idW = Worldbody_id
      for i in range(len(dList)):
          idB = dList[i].id_B
          idA = dList[i].id_A
          if (idW == dList[i].id_A) and (name == GetNameFromGeometryID(idB)):
              distance = dList[i]
              return distance
          elif (idW == dList[i].id_B) and (name == GetNameFromGeometryID(idA)):
              distance = dList[i]
              return distance
  </code></pre>
  <p>Thhe custom function defined here, SignedDistance, uses ComputeSignedDistancePairwiseClosestPoints as a proxy to find the distances I need:</p>
  <pre><code>Distances = np.zeros(6)
  for i in range(len(Claw_name)):
      Distances[i] = SignedDistance(Claw_name[i]).distance
      
          
  print(Distances)
  </code></pre>
  <p>Curiously this works as opposed to using ComputeSignedDistancePairClosestPoints:</p>
  <pre><code>Distances = np.zeros(6)
  for i in range(len(Claw_id)):
      Distances[i] = query_object.ComputeSignedDistancePairClosestPoints(Worldbody_id, Claw_id[i])
      
  print(Distances)
  </code></pre>
  <p>Here's the urdf I used:</p>
  <pre><code>&lt;?xml version=&quot;1.0&quot; encoding=&quot;utf-8&quot;?&gt;
  &lt;robot name=&quot;scawer&quot;&gt;
      &lt;!-- Defining materials --&gt;
      &lt;material name=&quot;mat_body&quot;&gt;
          &lt;color rgba=&quot;0.1 0.1 0.4 1.0&quot;/&gt;
      &lt;/material&gt;
  
      &lt;material name=&quot;mat_link_0&quot;&gt;
          &lt;color rgba=&quot;0.35 0.4 0.35 1.0&quot;/&gt;
      &lt;/material&gt;
  
      &lt;material name=&quot;mat_link_1&quot;&gt;
          &lt;color rgba=&quot;0.5 0.35 0.35 1.0&quot;/&gt;
      &lt;/material&gt;
  
      &lt;material name=&quot;mat_link_2&quot;&gt;
          &lt;color rgba=&quot;0.35 0.35 0.5 1.0&quot;/&gt;
      &lt;/material&gt;
  
      &lt;material name=&quot;mat_link_3&quot;&gt;
          &lt;color rgba=&quot;0.25 0.25 0.25 1.0&quot;/&gt;
      &lt;/material&gt;
      &lt;!-- End Defining materials --&gt;
  
      &lt;link name=&quot;body&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.2 0.1 0.05&quot;/&gt;
              &lt;/geometry&gt;
              &lt;material name=&quot;mat_body&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.2 0.1 0.05&quot;/&gt;
              &lt;/geometry&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.8&quot;/&gt;
              &lt;inertia ixx=&quot;8.33e-4&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;2.83e-3&quot; iyz=&quot;0.0&quot; izz=&quot;3.33e-3&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;!-- Leg R1 Begin --&gt;
      &lt;link name=&quot;leg_r1_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r1_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r1_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r1_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;r1_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0.1 -0.05 0&quot;/&gt; &lt;!--rpy=&quot;0.0 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_r1_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 1.0&quot;/&gt;
          &lt;limit lower=&quot;-1.0471975512&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r1_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r1_0&quot;/&gt;
          &lt;child link=&quot;leg_r1_1&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r1_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r1_1&quot;/&gt;
          &lt;child link=&quot;leg_r1_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;r1_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r1_2&quot;/&gt;
          &lt;child link=&quot;leg_r1_3&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg R1 End --&gt;
  
      &lt;!-- Leg L1 Begin --&gt;
      &lt;link name=&quot;leg_l1_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l1_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l1_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l1_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;l1_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0.1 0.05 0&quot;/&gt; &lt;!-- rpy=&quot;3.1416 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_l1_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 -1.0&quot;/&gt;
          &lt;limit lower=&quot;-1.0471975512&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l1_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l1_0&quot;/&gt;
          &lt;child link=&quot;leg_l1_1&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l1_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l1_1&quot;/&gt;
          &lt;child link=&quot;leg_l1_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;l1_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l1_2&quot;/&gt;
          &lt;child link=&quot;leg_l1_3&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg L1 End --&gt;
  
      &lt;!-- Leg R2 Begin --&gt;
      &lt;link name=&quot;leg_r2_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r2_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r2_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r2_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;r2_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.05 0&quot;/&gt; &lt;!--rpy=&quot;0.0 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_r2_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 1.0&quot;/&gt;
          &lt;limit lower=&quot;-1.0471975512&quot; upper=&quot;1.047197551&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r2_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r2_0&quot;/&gt;
          &lt;child link=&quot;leg_r2_1&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r2_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r2_1&quot;/&gt;
          &lt;child link=&quot;leg_r2_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;r2_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r2_2&quot;/&gt;
          &lt;child link=&quot;leg_r2_3&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg R2 End --&gt;
  
      &lt;!-- Leg L2 Begin --&gt;
      &lt;link name=&quot;leg_l2_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l2_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l2_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l2_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;l2_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.05 0&quot;/&gt; &lt;!-- rpy=&quot;3.1416 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_l2_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 -1.0&quot;/&gt;
          &lt;limit lower=&quot;-1.0471975512&quot; upper=&quot;1.047197551&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l2_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l2_0&quot;/&gt;
          &lt;child link=&quot;leg_l2_1&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l2_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l2_1&quot;/&gt;
          &lt;child link=&quot;leg_l2_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;l2_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l2_2&quot;/&gt;
          &lt;child link=&quot;leg_l2_3&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg L2 End --&gt;
  
      &lt;!-- Leg R3 Begin --&gt;
      &lt;link name=&quot;leg_r3_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r3_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r3_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_r3_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 -0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;r3_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;-0.1 -0.05 0&quot;/&gt; &lt;!--rpy=&quot;0.0 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_r3_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 1.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;1.0471975512&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r3_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r3_0&quot;/&gt;
          &lt;child link=&quot;leg_r3_1&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;r3_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r3_1&quot;/&gt;
          &lt;child link=&quot;leg_r3_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;r3_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 -0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_r3_2&quot;/&gt;
          &lt;child link=&quot;leg_r3_3&quot;/&gt;
          &lt;axis xyz=&quot;1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg R3 End --&gt;
  
      &lt;!-- Leg L3 Begin --&gt;
      &lt;link name=&quot;leg_l3_0&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_0&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.02 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.01 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;2.604e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;2.604e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l3_1&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_1&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l3_2&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_2&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.045 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.0225 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;9.375e-6&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;9.375e-6&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;link name=&quot;leg_l3_3&quot;&gt;
          &lt;visual&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
              &lt;material name=&quot;mat_link_3&quot;/&gt;
          &lt;/visual&gt;
  
          &lt;collision&gt;
              &lt;geometry&gt;
                  &lt;box size=&quot;0.015 0.09 0.015&quot;/&gt;
              &lt;/geometry&gt;
              &lt;origin xyz=&quot;0.0 0.045 0.0&quot;/&gt;
          &lt;/collision&gt;
  
          &lt;inertial&gt;
              &lt;mass value=&quot;0.05&quot;/&gt;
              &lt;inertia ixx=&quot;3.46875e-5&quot; ixy=&quot;0.0&quot; ixz=&quot;0.0&quot; iyy=&quot;1.875e-6&quot; iyz=&quot;0.0&quot; izz=&quot;3.46875e-5&quot;/&gt;
          &lt;/inertial&gt;
      &lt;/link&gt;
  
      &lt;joint name=&quot;l3_j0&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;-0.1 0.05 0&quot;/&gt; &lt;!-- rpy=&quot;3.1416 0.0 0.0&quot;/&gt;--&gt;
          &lt;parent link=&quot;body&quot;/&gt;
          &lt;child link=&quot;leg_l3_0&quot;/&gt;
          &lt;axis xyz=&quot;0.0 0.0 -1.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;1.0471975512&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l3_j1&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.02 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l3_0&quot;/&gt;
          &lt;child link=&quot;leg_l3_1&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.09439510239&quot; upper=&quot;2.09439510239&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      
      &lt;joint name=&quot;l3_j2&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l3_1&quot;/&gt;
          &lt;child link=&quot;leg_l3_2&quot;/&gt;
          &lt;axis xyz=&quot;0.0 -1.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-1.57&quot; upper=&quot;1.57&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
  
      &lt;joint name=&quot;l3_j3&quot; type=&quot;revolute&quot;&gt;
          &lt;origin xyz=&quot;0 0.045 0&quot;/&gt; &lt;!-- rpy=&quot;0.0 0.0 0.0&quot;/&gt; --&gt;
          &lt;parent link=&quot;leg_l3_2&quot;/&gt;
          &lt;child link=&quot;leg_l3_3&quot;/&gt;
          &lt;axis xyz=&quot;-1.0 0.0 0.0&quot;/&gt;
          &lt;limit lower=&quot;-2.356&quot; upper=&quot;2.356&quot; effort=&quot;0.0&quot; velocity=&quot;0.0&quot;/&gt;
      &lt;/joint&gt;
      &lt;!-- Leg L3 1End --&gt;
  &lt;/robot&gt;
  </code></pre>