Extend RobotModel #7
Description
- Allow extending the RobotModel with fixed frames from static TF, i.e.
- subscribe to /tf_static
- add fixed joints to RobotModel
- have a method add_link(t: TransformStamped)
- Introduce shortcuts for a chain of fixed transforms, i.e.
- pre-multiply Joint.T of all fixed joints and the parent active joint, stored e.g. in shortcut_T
- safe the direct active parent joint in shortcut_parent
- This needs to be updated if any fixed TF changes!
- Implement caching of FK results
- avoid re-computing the same FK from A to B during a single (across multiple?) control cycles