Added macro reader for twist.
Added macro reader for displacement.
Fix bug.

* Add corresponding test.
Fix bug.

* Try optional. Not working ?
Start elbow manipulator example.

* See murray ly sastry p. 90.