Migrate all usages of RigidBodyKinematics.py to a rigidBodyKinematics.hpp wrapped

**Describe your use case**
Migrate all usages of RigidBodyKinematics.py to a rigidBodyKinematics.hpp wrapped

**Describe alternatives solutions you've considered**
A clear and concise description of any alternative solutions or features you've considered.

**Additional context**
Add any other context or screenshots about the feature request here.
