- [ ] analytical formulas - [x] calculate screw invariants from pose trajectories - casadi opti implementation - [ ] generate new pose trajectory from screw invariants - casadi opti implementation
