Extend RobotModel

[rhaschke]

• 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