Search code examples
pythondrake

Drake: Add weld joint to world in SDF instead of after parsing (pydrake)


| drake 1.19.0 | python 3.7 | OS: ubuntu 20.04 | pycharm |

I have a Kuka arm fixed to a representative table via a simple SDF and I'd like to weld the table to world with a given offset within the SDF rather than after parsing the model. (I've tried to leverage the drake:parent tag to make the table a child of the world frame, but this changes nothing.)

Does this functionality exist?
Current SDF:

<?xml version="1.0" ?>
<sdf version="1.7">
  <model name="iiwa_and_table">
    <include>
      <uri>package://drake/manipulation/models/iiwa_description/urdf/iiwa14_spheres_collision.urdf</uri>
      <name>robot</name>
      <pose>0 0 0 0 0 0</pose>
    </include>
    <include>
      <uri>src/mast/descriptions/sdf/table.sdf</uri>
      <name>table</name>
      <pose>0 0 -0.5 0 0 0</pose>
      <!-- <drake:parent>world</drake:parent>-->
    </include>
    <joint name="static_joint" type="fixed">
      <parent>table::table</parent>
      <child>robot::base</child>
      <pose>0 0 0 0 0 0</pose>
    </joint>
  </model>
</sdf>

My current method for welding the table in a particular position is as follows:

builder = DiagramBuilder()
self.plant, self.scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
resource = FindResourceOrThrow(<path to my SDF>)
Parser(self.plant, self.scene_graph).AddModels(resource)

self.model = get_model_by_name(self.plant, name="robot")
self.weld_model = get_model_by_name(self.plant, name="table")
self.plant.WeldFrames(self.plant.world_frame(), self.plant.get_body(
                self.plant.GetBodyIndices(self.weld_model)[0]).body_frame(), RigidTransform(RotationMatrix.Identity(), [0, 0, -0.5]))

def get_model_by_name(plant: MultibodyPlant, name: str) -> ModelInstanceIndex:
    for idx in range(plant.num_model_instances()):
        model = ModelInstanceIndex(idx)
        if name in plant.GetModelInstanceName(model):
            return model
    raise Exception (print('Model name not found'))

Solution

  • Yes, this is supported. You just need to add the fixed joint in the SDF and reference the world body and sdf body... as in this example. I believe this adaptation should work:

    <?xml version="1.0"?>
    <sdf version="1.8">
      <world name="SimWorld">
        <include>
          <uri>model://simple_robot1/</uri>
        </include>
        <include>
          <uri>model://simple_robot2/</uri>
        </include>
        <model name="weld_models">
          <include>
            <uri>model://simple_robot1/</uri>
          </include>
          <include>
            <uri>model://simple_robot2/</uri>
          </include>
          <joint name="weld_robots" type="revolute">
            <parent>world</parent>
            <child>robot2::base_link</child>
            <axis>
              <xyz>0 0 1</xyz>
            </axis>
          </joint>
        </model>
      </world>
    </sdf>