Implement derived trajpts

Not all methods are fully implemented, but it should be a start. Should also mostly address issue #4.
This whole section will be replaced by a descartes `RobotModel` type (see PR #11).  However, this know tells me how use robot state to get what I want.