WIP 2014-09-24 tried to add sinusoidal motion to lead agent in flex body carry but does not work. not sure if kinematics are right or if problem is with estimator.