Added a cartesian control test that uses PD control in the configuration space to translate cartesian errors into cartesian accelerations and then into corresponding joint accelerations. Uses a primitive gain scheduling to increase the gains as the robot nears the goal.