I'm currently doing this tutorial which teaches how to write a controller.

But how do I create a multiple joint controller like the default controller of pr2(rarmcontroller).

My aim is to create a controller that can make pr2 rotate its wrist using force control and yet possess some stiffness in the rest of the arm. If there is anyone who can give advice on a creating a multiple joint controller or implementing force control on rarmcontroller, I'll be very thankful.