[C++][unittest] Fix computations in jointcomposite::calc() + reworked unittest (comparison of R3xSO3 (either two joints with no inertia between them or Jointcomposite))