Add "Cartesian Impediance" when in joint impedance mode

[bsaund]

Our robot arm planners (almost?) exclusively operate in joint space, but having the effect of "Cartesian Impedance" (e.g. stiff in Z but soft in x, y) can still be useful.

We can implement this in the java code by changing joint stiffnesses based on current position and desired cartesian stiffness.

[dmcconachie]

Right now the control mode setting is running at 1 Hz on Victor, we'll want to see what sort of rate we can actually do this switching at. We may want to add another LCM channel that is just changes to joint impedance mode parameters rather than mucking with the the SetControlMode service, which is designed to be a blocking call in the LCM bridge so as to ensure that the message is received (which PubSub messages are not, at least I think service calls are guaranteed not to be dropped).

Have we tested to see if there are any side effects from Kuka's internal joint controller that come from switching impedance parameters at high speed?

[bsaund]

We don't even need LCM to set these Impedance parameters. We can bake this into the control mode. It can update as fast as the Java code runs.

Switching at slow speeds (0.5 Hz) is all I've tested. The only danger I've seen: If you are in a soft impedance mode, push the robot far from the setpoint, then switch to stiff impedance the robot "jumps" back to the setpoint, violating the speed limit and triggering a stop.