Easy Inverse Kinematics with Drake

If you've ever needed to figure out what joint angles will move your robot's arm to a specific position, you've dealt with inverse kinematics (IK). It's a common problem in robotics, but not as easily accessible as one would like it to be. It is especially frustrating when you use the simple gradient descent version, start getting errors, and realize it is slow. For that, there is an alternative.

I put together a simple library that shows how to use Drake for IK. This approach works for any robot, but I will demonstrate it with the Sawyer.



What This Library Does

This library takes a target position and orientation in 3D space and calculates the joint angles needed to move the robot's end effector to that spot. Under the hood, it uses Drake's built-in IK solver, which handles all the complex math for you.

You can find the code here: https://github.com/nataliya-dev/sawyer_ik



Getting Started

The library uses uv for Python environment management, which keeps dependencies clean and simple. Here's how to set it up:



uv venv venv-sawyer --python 3.10
source venv-sawyer/bin/activate
uv pip install -r requirements.txt


Note: There are two requirements files (requirements_ubuntu20.txt and requirements_ubuntu22.txt) depending on your Ubuntu version. Drake has specific builds for different systems, so make sure to use the right one.



Once installed, running the IK solver is just:

python3 run_ik_test.py


This will calculate the joint positions and generate an HTML file you can open in your browser to see the robot in the solved configuration.



The Drake-Specific Piece: URDF Transmissions

Here's the key thing that makes this work with Drake: your URDF file needs transmission elements for each actuated joint. This is Drake-specific and tells Drake about the actuator properties.



Here's what a transmission block looks like:

<transmission name="sawyer_tran1">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="right_j1">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
  </joint>
  <actuator name="sawyer_motor1">
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <mechanicalReduction>1</mechanicalReduction>
    <drake:gear_ratio value="100.0" />
    <drake:rotor_inertia value="0.0000605721456" />
  </actuator>
</transmission>


Without these transmission blocks, Drake won't recognize the actuators. You need one for each joint you want to control. The drake:gear_ratio and drake:rotor_inertia tags are Drake-specific extensions that help with dynamics calculations.



How the Code Works

The core is pretty simple. The SawyerKinematicsSolver class has a solve_ik method that:



  1. Sets up the IK problem with your target position and orientation
  2. Adds constraints to keep solutions within joint limits
  3. Uses Drake's solver to find joint angles
  4. Returns whether it succeeded and what the solution is


success, q = ik_solver.solve_ik(
    plant,
    pos_W=position,
    q_W=orientation,
    ee_link_name="right_hand"
)


That's it. Drake handles all the optimization and constraint satisfaction behind the scenes.



Why This Matters

The point of this library isn't just to solve IK for Sawyer but it is to show how little code you need to add IK to any robot when you use Drake. The same pattern works for other robots: load your URDF, make sure it has the transmission blocks, and call the solver.

No need to implement Jacobian calculations, deal with singularities, or write your own optimization loop. Drake gives you a robust, tested IK solver that just works.



Try It Yourself

Clone the repo, run the example, and see the robot move to the target pose. Then try swapping in your own robot's URDF and target positions. As long as your URDF has the Drake transmission elements, the same code should work.

The visualization feature is nice too, you get an HTML file you can open in any browser to see if your solution looks right. No special viewers needed.

Let me know if this works for you!