Update forward kinematics for continuous joints

[bpapaspyros]

The current issue with continuous joints is that pinocchio seems to be storing 2 parameters for these joints: sin(theta) and cos(theta). That, however, breaks forward kinematics unless we properly normalize these values. The following snippet works in our unit tests but should now be adapted to be part of the current forward_kinematics function.

  auto& pmodel = this->robot_->get_pinocchio_model();
  auto& data = this->robot_->get_pinocchio_data();

  Eigen::VectorXd q = pinocchio::neutral(pmodel);
  const std::size_t idx_q = pmodel.joints[1].idx_q();
  const double angle = M_PI / 3.0;
  q[idx_q + 0] = std::cos(angle);
  q[idx_q + 1] = std::sin(angle);

  pinocchio::forwardKinematics(pmodel, data, q);

  const auto& M = data.oMi[1];

  Eigen::Matrix3d R_exp = Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ()).toRotationMatrix();

  EXPECT_TRUE(M.rotation().isApprox(R_exp, 1e-9)) << "Rotation should be a pure Rz(pi/3).";
  EXPECT_TRUE(M.translation().isZero(1e-9)) << "Translation should remain zero.";