Making a robot draw on an arbitrary smooth surface

For my final project for 6.4212, Robotic Manipulation, I was in charge of implementing an operation space controller for the Kuka IIWA, a 7 DOF robot arm, which included a null space controller. The project was done in Drake.

The full final project writeup can be read here, where details of our operation space control derivation as well as our depth camera based perception system come together to help us orient the end effector and apply the correct joint torques to both track the reference trajectory while keeping constant force normal to the surface. I implemented the controller while my two group mates implemented the perception system and project infrastructure like making the pen leave a trace on surfaces.

A high level system diagram is as follows:

At a high level, the system works by placing a reference trajectory beneath the surface in which it should be drawn.

Depending on whether or not we believe the end effector is within a certain distance threshold to the surface (which we can determine based on our rough point cloud mapping) we decide how to command the operation space impedance controller by changing the pose setpoint as well as rotating the impedance parameters to be stiff along the surface and not stiff in direction of the local surface normal, allowing us to inject a constant force in that direction.

More details about this method in the writeup.