restructure SimpleDynamicsFullKinematicsPlanner, set access = protected for some methods