Implement derived trajpts

Not all methods are fully implemented, but it should be a start. Should also mostly address issue #4.
Good, that was the purpose of filling this in at all, as there is not enough information to actually do what we want with the RobotState as is. For one, we need to know what group we are working with, either by a string name of JointModelGroup