fwd kin full implementation
Description
Filled in full implementation of forward kinematics, ported from mainboard firmware. This is in preparation for dynamics calculations that will be used to estimate patient effort.
What to focus on
- put content here.
Reviewers
Resources
Issues
What issues does this fix?
- Fixes #issue-number
Edited by Chris Woodall