Add size() method.
Refactor initJointTrajectory()

- Rename from init().
- Move to a separate header, combine with code in controller.
- Complete wrapping joint support, unit test it.
- The unit test of initJointTrajectory() is commented-out. Needs to be updated.