rpy_utils.h

Go to the documentation of this file.

/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
#include <mc_rtc/pragma.h>
 
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace mc_rbdyn
{
 
inline Eigen::Matrix3d rpyToMat(const double & r, const double & p, const double & y)
{
  MC_RTC_diagnostic_push
  MC_RTC_diagnostic_ignored(GCC, "-Wconversion")
  return sva::RotX(r) * sva::RotY(p) * sva::RotZ(y);
  MC_RTC_diagnostic_pop
}
 
inline Eigen::Matrix3d rpyToMat(const Eigen::Vector3d & rpy)
{
  return rpyToMat(rpy(0), rpy(1), rpy(2));
}
 
inline sva::PTransformd rpyToPT(const Eigen::Vector3d & rpy)
{
  return sva::PTransformd(rpyToMat(rpy(0), rpy(1), rpy(2)));
}
 
inline sva::PTransformd rpyToPT(const double & r, const double & p, const double & y)
{
  return sva::PTransformd(rpyToMat(r, p, y));
}
 
inline Eigen::Vector3d rpyFromMat(const Eigen::Matrix3d & E)
{
  double roll = atan2(E(1, 2), E(2, 2));
  double pitch = -asin(E(0, 2));
  double yaw = atan2(E(0, 1), E(0, 0));
  return Eigen::Vector3d(roll, pitch, yaw);
}
 
inline Eigen::Vector3d rpyFromQuat(const Eigen::Quaterniond & quat)
{
  return rpyFromMat(quat.toRotationMatrix());
}
 
template<typename T>
Eigen::Vector3d rpyFromRotation(const T & value)
{
  if constexpr(std::is_same_v<T, Eigen::Quaterniond>) { return rpyFromQuat(value); }
  else
  {
    return rpyFromMat(value);
  }
}
 
} // namespace mc_rbdyn