PointOnBoxSurfaceHelper Failure Reproduction

box_small.urdf

<?xml version="1.0"?>

<robot name="box_scene">
  <material name="Blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="Red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="Gray">
    <color rgba="0.2 0.2 0.2 1.0"/>
  </material>

  <material name="li">
    <color rgba="0.5 0.5 0.5 1.0"/>
  </material>

  <link name="base"/>

  <link name="box1">
    <collision>
       <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 2.1"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 2.1"/>
      </geometry>
      <material name="Gray"/>
    </visual>
  </link>

  <link name="box2">
    <collision>
       <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 1.5"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 1.5"/>
      </geometry>
      <material name="Gray"/>
    </visual>
  </link>

  <link name="box3">
    <collision>
       <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 2.1"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 2.1"/>
      </geometry>
      <material name="Gray"/>
    </visual>
  </link>

  <link name="box4">
    <collision>
       <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 1.5"/>
      </geometry>
    </collision>
    <visual>
      <origin xyz="0 0 .0" rpy="0 0 0"/>
      <geometry>
        <box size="0.4 0.1 1.5"/>
      </geometry>
      <material name="Gray"/>
    </visual>
  </link>

  <joint name="jbox1" type="fixed">
    <origin rpy="1.5707 0 0" xyz="0 0 -0.05"/>
    <parent link="base"/>
    <child link="box1"/>
  </joint>

  <joint name="jbox2" type="fixed">
    <origin rpy="0 0 0" xyz="0 1.0 .75"/>
    <parent link="base"/>
    <child link="box2"/>
  </joint>

  <joint name="jbox3" type="fixed">
    <origin rpy="1.5707 0 0" xyz="0 0 1.55"/>
    <parent link="base"/>
    <child link="box3"/>
  </joint>

  <joint name="jbox4" type="fixed">
    <origin rpy="0 0 0" xyz="0 -1 .75"/>
    <parent link="base"/>
    <child link="box4"/>
  </joint>
  
</robot>

oneDOF_iiwa7_with_box_collision.sdf

<?xml version="1.0"?>
<sdf version="1.7">
  <model name="iiwa7_oneDOF">
    <link name="iiwa_oneDOF_link_0">
      <inertial>
        <pose>-0.013 0 0.07 0 0 0</pose>
        <mass>3.863</mass>
        <inertia>
          <ixx>0.0141</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0171</iyy>
          <iyz>0</iyz>
          <izz>0.0178</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_0_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_0.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_0_collision">
        <pose>-0.004563 0 0.07875 0 0 0</pose>
        <geometry>
          <box>
            <size>0.216874 0.207874 0.1575</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <link name="iiwa_oneDOF_link_1">
      <pose>0 0 0.1575 0 0 0</pose>
      <inertial>
        <pose>0 -0.0347 0.113 0 0 0</pose>
        <mass>2.7025</mass>
        <inertia>
          <ixx>0.0171</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0163</iyy>
          <iyz>0</iyz>
          <izz>0.006</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_1_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_1.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_1_collision">
        <pose>0 -0.023301 0.127997 0 0 0</pose>
        <geometry>
          <box>
            <size>0.13596 0.182584 0.260995</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_1" type="fixed">
      <child>iiwa_oneDOF_link_1</child>
      <parent>iiwa_oneDOF_link_0</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_2">
      <pose>0 0 0.3405 1.5708 0 -3.14159</pose>
      <inertial>
        <pose>0.000 0.0668 0.0344 0 0 0</pose>
        <mass>2.7258</mass>
        <inertia>
          <ixx>0.0170</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0162</iyy>
          <iyz>0</iyz>
          <izz>0.0061</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_2_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_2.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_2_collision">
        <pose>0 0.0580045 0.0173035 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135988 0.251991 0.182605</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_2" type="revolute">
      <child>iiwa_oneDOF_link_2</child>
      <parent>iiwa_oneDOF_link_1</parent>
      <axis>
        <xyz expressed_in="__model__">-0 1 0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_3">
      <pose>0 0 0.5245 0 0 0</pose>
      <inertial>
        <pose>0 0.0296 0.1265 0 0 0</pose>
        <mass>3.175</mass>
        <inertia>
          <ixx>0.025</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0238</iyy>
          <iyz>0</iyz>
          <izz>0.0076</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_3_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_3.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_3_collision">
        <pose>0 0.0182965 0.11073 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135987 0.182593 0.29346</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_3" type="fixed">
      <child>iiwa_oneDOF_link_3</child>
      <parent>iiwa_oneDOF_link_2</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_4">
      <pose>0 0 0.74 1.5708 0 0</pose>
      <inertial>
        <pose>0 0.067 0.034 0 0 0</pose>
        <mass>2.73</mass>
        <inertia>
          <ixx>0.017</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0162</iyy>
          <iyz>0</iyz>
          <izz>0.0061</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_4_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_4.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_4_collision">
        <pose>0 0.0580045 0.0233035 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135988 0.251991 0.182605</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_4" type="fixed">
      <child>iiwa_oneDOF_link_4</child>
      <parent>iiwa_oneDOF_link_3</parent>
      <axis>
        <xyz expressed_in="__model__">0 -1 0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_5">
      <pose>0 0 0.924 0 0 -3.14159</pose>
      <inertial>
        <pose>0.0001 0.021 0.076 0 0 0</pose>
        <mass>1.69</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0087</iyy>
          <iyz>0</iyz>
          <izz>0.00449</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_5_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_5.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_5_collision">
        <pose>0 0.015546 0.102458 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135999 0.167092 0.276916</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_5" type="fixed">
      <child>iiwa_oneDOF_link_5</child>
      <parent>iiwa_oneDOF_link_4</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_6">
      <pose>0 0 1.1395 1.5708 0 -3.14159</pose>
      <inertial>
        <pose>0 0.0026 0.0005 0 0 0</pose>
        <mass>1.8</mass>
        <inertia>
          <ixx>0.0051</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0049</iyy>
          <iyz>0</iyz>
          <izz>0.0035</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_6_visual">
        <pose>0 0 -0.060700 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_6.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_6_collision">
        <geometry>
          <box>
            <size>0.132334 0.177387 0.137409</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_6" type="fixed">
      <child>iiwa_oneDOF_link_6</child>
      <parent>iiwa_oneDOF_link_5</parent>
      <axis>
        <xyz expressed_in="__model__">-0 1 -0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_oneDOF_link_7">
      <pose>0 0 1.22 0 0 0</pose>
      <inertial>
        <pose>0 0 0.0294 0 0 0</pose>
        <mass>.4</mass>
        <inertia>
          <ixx>0.0004</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0004</iyy>
          <iyz>0</iyz>
          <izz>0.0005</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_oneDOF_link_7_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_7.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_oneDOF_link_7_collision">
        <pose>0 0 0.021997 0 0 0</pose>
        <geometry>
          <box>
            <size>0.10385 0.103885 0.045</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_7" type="fixed">
      <child>iiwa_oneDOF_link_7</child>
      <parent>iiwa_oneDOF_link_6</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-3.05433</lower>
          <upper>3.05433</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <static>0</static>
    <!-- <plugin name="gazebo_ros_controller" filename="libgazebo_ros_control.so">
      <robotNamespace>/iiwa</robotNamespace>
    </plugin> -->
  </model>
</sdf>

tmp.py

from functools import partial
from pydrake.all import (StartMeshcat,
                         RobotDiagramBuilder,
                         MeshcatVisualizer,
                         LoadModelDirectives,
                         ProcessModelDirectives,
                         RigidTransform,
                         RotationMatrix,
                         MeshcatVisualizerParams,
                         Role,
                         RollPitchYaw,
                         Meldis,
                         AddDefaultVisualization,
                         Box,
                         GeometrySet
                         )
import numpy as np
import os

def plant_builder_3dof_flipper(usemeshcat = False):
    meshcat = StartMeshcat()
    builder = RobotDiagramBuilder()
    plant = builder.plant()
    scene_graph = builder.scene_graph()
    parser = builder.parser()
    parser.SetAutoRenaming(True)
    path_repo = os.path.dirname(os.path.abspath(__file__)) 
    # plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
    # parser = Parser(plant)
    rel_path = path_repo
    oneDOF_iiwa_asset = rel_path + "/oneDOF_iiwa7_with_box_collision.sdf"#FindResourceOrThrow("drake/C_Iris_Examples/assets/oneDOF_iiwa7_with_box_collision.sdf")
    twoDOF_iiwa_asset = rel_path + "/twoDOF_iiwa7_with_box_collision.sdf"#FindResourceOrThrow("drake/C_Iris_Examples/assets/twoDOF_iiwa7_with_box_collision.sdf")

    box_asset = rel_path + "/box_small.urdf" #FindResourceOrThrow("drake/C_Iris_Examples/assets/box_small.urdf")
    print(box_asset)
    models = []
    models.append(parser.AddModels(box_asset)[0])
    models.append(parser.AddModels(twoDOF_iiwa_asset)[0])
    models.append(parser.AddModels(oneDOF_iiwa_asset)[0])

    locs = [[0.,0.,0.],
            [0.,.55,0.],
            [0.,-.55,0.]]
    plant.WeldFrames(plant.world_frame(), 
        plant.GetFrameByName("base", models[0]),
        RigidTransform(locs[0]))
    plant.WeldFrames(plant.world_frame(), 
                    plant.GetFrameByName("iiwa_twoDOF_link_0", models[1]), 
                    RigidTransform(RollPitchYaw([0,0, -np.pi/2]).ToRotationMatrix(), locs[1]))
    plant.WeldFrames(plant.world_frame(), 
                    plant.GetFrameByName("iiwa_oneDOF_link_0", models[2]), 
                    RigidTransform(RollPitchYaw([0,0, -np.pi/2]).ToRotationMatrix(), locs[2]))

    plant.Finalize()
    inspector = scene_graph.model_inspector()

    meshcat_params = MeshcatVisualizerParams()
    meshcat_params.role = Role.kIllustration
    visualizer = MeshcatVisualizer.AddToBuilder(
            builder.builder(), scene_graph, meshcat, meshcat_params)
    # X_WC = RigidTransform(RollPitchYaw(0,0,0),np.array([5, 4, 2]) ) # some drake.RigidTransform()
    # meshcat.SetTransform("/Cameras/default", X_WC) 
    # meshcat.SetProperty("/Background", "top_color", [0.8, 0.8, 0.6])
    # meshcat.SetProperty("/Background", "bottom_color",
    #                                 [0.9, 0.9, 0.9])
    diagram = builder.Build()
    diagram_context = diagram.CreateDefaultContext()
    plant_context = plant.GetMyMutableContextFromRoot(diagram_context)
    diagram.ForcedPublish(diagram_context)
    print(meshcat.web_url())
    return plant, scene_graph, diagram, diagram_context, plant_context, meshcat if usemeshcat else None

plant, scene_graph, diagram, diagram_context, plant_context, meshcat = plant_builder_3dof_flipper(usemeshcat = True)

pos = np.array([-0.438428242186379, -2.0944, -1.35135973487098])
plant.SetPositions(plant_context, pos)

query_object = plant.get_geometry_query_input_port().Eval(plant_context);
inspector = query_object.inspector()
all_geom_ids = inspector.GetGeometryIds(GeometrySet(inspector.GetAllGeometryIds()), Role.kProximity)

geomAName = "iiwa7_twoDOF::iiwa_twoDOF_link_5"
geomBName = "iiwa7_oneDOF::iiwa_oneDOF_link_6"

for geom_id in all_geom_ids:
    if inspector.GetName(inspector.GetFrameId(geom_id)) == geomAName:
        geomA = geom_id
    if inspector.GetName(inspector.GetFrameId(geom_id)) == geomBName:
        geomB = geom_id

query_object.ComputeSignedDistancePairClosestPoints(geomA, geomB)

twoDOF_iiwa7_with_box_collision.sdf

<?xml version="1.0"?>
<sdf version="1.7">
  <model name="iiwa7_twoDOF">
    <link name="iiwa_twoDOF_link_0">
      <inertial>
        <pose>-0.013 0 0.07 0 0 0</pose>
        <mass>3.863</mass>
        <inertia>
          <ixx>0.0141</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0171</iyy>
          <iyz>0</iyz>
          <izz>0.0178</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_0_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_0.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_0_collision">
        <pose>-0.004563 0 0.07875 0 0 0</pose>
        <geometry>
          <box>
            <size>0.216874 0.207874 0.1575</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <link name="iiwa_twoDOF_link_1">
      <pose>0 0 0.1575 0 0 0</pose>
      <inertial>
        <pose>0 -0.0347 0.113 0 0 0</pose>
        <mass>2.7025</mass>
        <inertia>
          <ixx>0.0171</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0163</iyy>
          <iyz>0</iyz>
          <izz>0.006</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_1_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_1.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_1_collision">
        <pose>0 -0.023301 0.127997 0 0 0</pose>
        <geometry>
          <box>
            <size>0.13596 0.182584 0.260995</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_1" type="fixed">
      <child>iiwa_twoDOF_link_1</child>
      <parent>iiwa_twoDOF_link_0</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_2">
      <pose>0 0 0.3405 1.5708 0 -3.14159</pose>
      <inertial>
        <pose>0.000 0.0668 0.0344 0 0 0</pose>
        <mass>2.7258</mass>
        <inertia>
          <ixx>0.0170</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0162</iyy>
          <iyz>0</iyz>
          <izz>0.0061</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_2_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_2.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_2_collision">
        <pose>0 0.0580045 0.0173035 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135988 0.251991 0.182605</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_2" type="revolute">
      <child>iiwa_twoDOF_link_2</child>
      <parent>iiwa_twoDOF_link_1</parent>
      <axis>
        <xyz expressed_in="__model__">-0 1 0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_3">
      <pose>0 0 0.5245 0 0 0</pose>
      <inertial>
        <pose>0 0.0296 0.1265 0 0 0</pose>
        <mass>3.175</mass>
        <inertia>
          <ixx>0.025</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0238</iyy>
          <iyz>0</iyz>
          <izz>0.0076</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_3_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_3.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_3_collision">
        <pose>0 0.0182965 0.11073 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135987 0.182593 0.29346</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_3" type="fixed">
      <child>iiwa_twoDOF_link_3</child>
      <parent>iiwa_twoDOF_link_2</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_4">
      <pose>0 0 0.74 1.5708 0 0</pose>
      <inertial>
        <pose>0 0.067 0.034 0 0 0</pose>
        <mass>2.73</mass>
        <inertia>
          <ixx>0.017</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0162</iyy>
          <iyz>0</iyz>
          <izz>0.0061</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_4_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_4.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_4_collision">
        <pose>0 0.0580045 0.0233035 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135988 0.251991 0.182605</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_4" type="revolute">
      <child>iiwa_twoDOF_link_4</child>
      <parent>iiwa_twoDOF_link_3</parent>
      <axis>
        <xyz expressed_in="__model__">0 -1 0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_5">
      <pose>0 0 0.924 0 0 -3.14159</pose>
      <inertial>
        <pose>0.0001 0.021 0.076 0 0 0</pose>
        <mass>1.69</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0087</iyy>
          <iyz>0</iyz>
          <izz>0.00449</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_5_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_5.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_5_collision">
        <pose>0 0.015546 0.102458 0 0 0</pose>
        <geometry>
          <box>
            <size>0.135999 0.167092 0.276916</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_5" type="fixed">
      <child>iiwa_twoDOF_link_5</child>
      <parent>iiwa_twoDOF_link_4</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_6">
      <pose>0 0 1.1395 1.5708 0 -3.14159</pose>
      <inertial>
        <pose>0 0.0026 0.0005 0 0 0</pose>
        <mass>1.8</mass>
        <inertia>
          <ixx>0.0051</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0049</iyy>
          <iyz>0</iyz>
          <izz>0.0035</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_6_visual">
        <pose>0 0 -0.060700 0 0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_6.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_6_collision">
        <geometry>
          <box>
            <size>0.132334 0.177387 0.137409</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_6" type="fixed">
      <child>iiwa_twoDOF_link_6</child>
      <parent>iiwa_twoDOF_link_5</parent>
      <axis>
        <xyz expressed_in="__model__">-0 1 -0</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_twoDOF_link_7">
      <pose>0 0 1.22 0 0 0</pose>
      <inertial>
        <pose>0 0 0.0294 0 0 0</pose>
        <mass>.4</mass>
        <inertia>
          <ixx>0.0004</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0004</iyy>
          <iyz>0</iyz>
          <izz>0.0005</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_twoDOF_link_7_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>package://drake_models/iiwa_description/meshes/iiwa7/visual/link_7.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <collision name="iiwa_twoDOF_link_7_collision">
        <pose>0 0 0.021997 0 0 0</pose>
        <geometry>
          <box>
            <size>0.10385 0.103885 0.045</size>
          </box>
        </geometry>
      </collision>
      <gravity>1</gravity>
      <!-- <velocity_decay/> -->
      <!-- <self_collide>0</self_collide> -->
    </link>
    <joint name="iiwa_joint_7" type="fixed">
      <child>iiwa_twoDOF_link_7</child>
      <parent>iiwa_twoDOF_link_6</parent>
      <axis>
        <xyz expressed_in="__model__">0 0 1</xyz>
        <limit>
          <lower>-3.05433</lower>
          <upper>3.05433</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <static>0</static>
    <!-- <plugin name="gazebo_ros_controller" filename="libgazebo_ros_control.so">
      <robotNamespace>/iiwa</robotNamespace>
    </plugin> -->
  </model>
</sdf>