Robot 2: Jacobian Inverse Dynamics
At the end of last time, we built a simple low-cost robot simulation in Drake, and are ready to move on to the pick-and-place piece of the manipulation course. As usual, the accompanying notebook is here, but building this yourself will be a much more interesting learning experience. Reading the lecture notes will also provide interesting context.
We’ll start with some practical considerations on how to implement inverse kinematics with our setup, and do some code-cleanup to make things easy to test on the real robot at the end. Then I want to build intuition for Jacobian inverse kinematics with a two-link arm, and seeing how different parameters here affect the trajectory of the arm.
Jacobians in Drake
We start off by defining a robot the same way we did last time. The Jacobian controller is defined as a LeafSystem in Drake and we can reuse the same class defined in the manipulation course by just updating the joint lengths and the part names.
After that, we connect up our PseudoInverseController to the rest of the robot, and things just.. work! The robot smoothly moves over to [0.1, 0.1, 0].
The above sentence is wrong, the robot doesn’t move to position
[0.1, 0.1, 0]but rather is being commanded to have a constant gripper velocity of[0.1, 0.1, 0]. I didn’t realize this error until the next part.
The setup right now is quite verbose, and isn’t easy to swap out for the real robot when we need to. I’d ideally like the sim and real robot to be swappable with a single line, and that means they have to offer the same interface. The way to do that in Drake is by defining a Diagram. Our Diagram will have 5 inputs and 5 outputs to match the real robot (excluding the gripper motor). We also create some utility LeafSystems that make it easy to truncate, extend and duplicate vectors through our system in order to match the simulation robot size. At the end of all this, our setup for the Jacobian controller looks really nice:
The Two-Link Arm
While the full arm is a nice test bed, I find it hard to build intuition in three dimensions. A two arm link setup will let us gain some intuition for how the Jacobian works and try out different motion planning strategies.
Setting up the URDF for the two link arm was fairly straightforward, but to get it to work with Drake I had to manually figure out how to add a mass and moments of intertia. The code to get it to work with the Pseudoinverse controller was fairly straightforward (and notice how similar it looks to the previous example!):
It was interesting to explore the singularities of this arm, and really understand what the implications of a singularity are. Even though there is no velocity command you can give the robot in a particular direction, you can still “jiggle” the robot around and allow it to have a non-zero acceleration in that direction (which allows it to escape the singularity).
There are some issues with the current arm when you run it - namely the collision boundaries don’t work well and the arm collides with itself. I’m also not sure if the controller works correctly since it just spins a rapid loop when I’d expect it to move towards a particular direction. We’re going to try and model a better 2D arm in the next part when we learn some CAD.