user gradients for force elements with tests

The commit also includes an example of trajectory optimization using a RigidBodyManipulator.

<strong>NOTE</strong>: manipulatorDynamics.m on line 5 sets use_mex to false. This is because HandCmex.cpp returns empty matrices for the gradients. It might be a memory management (pointer) issue. The matlab code that does the same thing directly inside manipulatorDynamics.m however works well.