Search code examples
roboticsdrake

Pick and place simulation using the Robotiq 2F-140 manipulator in drake

I would like to use drake to simulate pick and place with different grasp poses on meshes generated from unknown objects, because of its superb hydroelastic contact and friction modelling. I have to specifically use the Robotiq 2F-140 for this which I why I want to integrate it into drake.

I see multiple ways of doing it. My overall question would be what is the easiest?

In my GitHub repository, I have gathered some information about this. First, I successfully converted the official Robotiq 2F-140 .xacro file to a .urdf file in an Ubuntu 16.04 ROS Kinetic docker image. Then I successfully replaced the Schunk WSG manipulator urdf file in the Pick and Place Jupyter Notebook of Prof. Russ Tedrake (which almost perfectly sets up the pick and place scene the way I need it) with it as you can see in the following picture.

Pick and Place ipynb with robotiq

One problem hindering me to grasp with this is the 15th cell of that linked Notebook.

from manipulation.station import LoadScenario

X_O = {
    "initial": RigidTransform(
        RotationMatrix.MakeZRotation(np.pi / 2.0), [0, -0.6, 0.0]
    ),
    "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi), [0.5, 0, 0.0]),
}

meshcat.Delete()
builder = DiagramBuilder()

scenario_data = """
directives:
- add_directives:
    file: package://manipulation/clutter.dmd.yaml
- add_model:
    name: foam_brick
    file: package://manipulation/hydro/061_foam_brick.sdf
model_drivers:
    iiwa: !IiwaDriver
      hand_model_name: wsg
    wsg: !SchunkWsgDriver {}
"""

scenario = LoadScenario(data=scenario_data)

I have not written a custom controller for the Robotiq 140. I would attempt to write one if it is needed, but I suspect there might be an easier way here. I think that maybe there is already enough information in the following that I have observed about the drake repository:

  1. In Issue#19842 "Minimal reproduction of very unstable contact forces for robotiq gripper simulation" Prof. Tedrake and user XuChenHan-TRI made it possible to grasp with the similar Robotiq 2F-85 gripper it seems, but the resulting code and models are not merged into the master and seem unavailable. I tried loading the model here and it couldn't find it anymore:
error: URI 'package://movo_description/meshes/manipulation/robotiq/visual/kinova_robotiq_coupler.obj' refers to unknown package 'movo_description'

  1. I have observed that in the repository models there are several .urdf files named robotiq. I tried loading robotiq.urdf and robotiq_simple.urdf in this noteboook, but robotiq_simple.urdf does not appear at all (which you already mentioned in Issue#19992) and the robotiq.urdf returns the following mesh in Meshcat: enter image description here

  2. Additionally, I want to mention Matt Corsaro's Robotiq 3F implementation in drake here for potential reference.

Solution

  • Thanks for your thorough investigation. I believe the robotiq models that you seek can be found here.

    The urdf in that repository uses mimic joints, which are fully supported in Drake for simulation when using the SAP solver for MultibodyPlant. Unfortunately, they are not supported (yet) in all parts of Drake. We are tracking that in this issue. For the grasp planning, you might choose to use the welded version of the gripper -- just like the notebook you found of mine used the welded WSG. You'll see that I have a WELDED option in the robotiq xacro.

    For completeness, this is the notes I left for myself on how to build the two versions of the movo description, including the welded fingers and the unwelded fingers.

    • Install ROS1 (I have noetic), apt install ros-noetic-xacro
    • Make a movo_description_ws with a src directory, and symlink only the movo_common/movo_description subdirectory into it.
    • Run cmake_init_workspace in the src directory
    • Run cmake_make in the root workspace directory
    • source devel/setup.sh
    • source movo_common/movo_config/movo_config.bash
    • xacro src/movo_description/urdf/movo.urdf.xacro > movo.urdf
    • WELD_FINGERS=true xacro src/movo_description/urdf/movo.urdf.xacro > movo_welded_fingers.urdf

    You can change parameters using movo_common/movo_config/movo_config.bash. This recipe can be readily adapted to build only the two robotiq models (welded and unwelded).