Added post processing functions. Renamed SBPLKinematicModel to RobotModel