control laws rigid body kinematics passive non-linear systems