Add cartesian force control

[eeberhard]

To enable the direct command of some end-effector wrench force and torque, a simple cartesian force "controller" is added. It converts a subscribed EE wrench (in base reference frame!) into joint torques, which the joint_controller then applies in the torque impedence robot control mode.

• Create new derived Base_controller class Cartesian_force
• Subscribe to topic "command_wrench" of type geometry_msg/Wrench to update property F_ee_des (desired end-effector wrench force)
• On update(&torque, Jacobian), calculate joint torque with Jacobian^T * F_ee_des
• Add new control mode CART_FORCE which calls the cartesian force controller and switches to TORQUE_IMP robot mode.

Tested in simulation with: roslaunch lwr_simple_example sim.launch rostopic pub -1 /lwr/joint_controllers/command_wrench geometry_msgs/Wrench '{force: {x: 0, y: 0, z: 0}, torque: {x: 0, y: 0, z: 0}}' (modify to see the effect).

What's the point of all this? I think it allows the controller for the specific application to be split from this lwr repo itself. Specifically, I want to experiment with different passive DS controllers to couple position and orientation. Since passive_ds_controller is already defined in another repo, I can make a standalone controller node to use / configure / extend it, and then just send the generated force / torque to the robot in this manner. I leave the task of converting to joint space to the lwr repo, so the external control node can be robot agnostic.

[eeberhard]

I think this was a decent idea, but it was never really used!