Refactor joint constraints; add shape matching #15

Closed acgaudette closed 6 years ago