Joint.h

Go to the documentation of this file.

/*
 * Copyright 2012-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */
 
#pragma once
 
// includes
// std
#include <cmath>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
 
// SpaceVecAlg
#include <SpaceVecAlg/SpaceVecAlg>
 
namespace rbd
{
 
template<typename T>
Eigen::Matrix3<T> QuatToE(const std::vector<T> & q);
 
class Joint
{
public:
  enum Type
  {
    Rev, 
    Prism, 
    Spherical, 
    Planar, 
    Cylindrical, 
    Free, 
    Fixed 
  };
 
  enum OldType
  {
    RevX, 
    RevY, 
    RevZ, 
    PrismX, 
    PrismY, 
    PrismZ 
  };
 
public:
  Joint();
 
  Joint(OldType type, bool forward, std::string name);
 
  Joint(Type type, const Eigen::Vector3d & axis, bool forward, std::string name);
 
  Joint(Type type, bool forward, std::string name);
 
  Joint(std::string name);
 
  void makeMimic(const std::string & name, double multiplier, double offset);
 
  Type type() const
  {
    return type_;
  }
 
  double direction() const
  {
    return dir_;
  }
 
  bool forward() const
  {
    return dir_ == 1.;
  }
 
  void forward(bool forward)
  {
    if(forward != this->forward()) S_ *= -1;
    dir_ = forward ? 1. : -1;
  }
 
  int params() const
  {
    return params_;
  }
 
  int dof() const
  {
    return dof_;
  }
 
  const std::string & name() const
  {
    return name_;
  }
 
  bool isMimic() const
  {
    return isMimic_;
  }
 
  const std::string & mimicName() const
  {
    return mimicName_;
  }
 
  double mimicMultiplier() const
  {
    return mimicMultiplier_;
  }
 
  double mimicOffset() const
  {
    return mimicOffset_;
  }
 
  const Eigen::Matrix<double, 6, Eigen::Dynamic> & motionSubspace() const
  {
    return S_;
  }
 
  template<typename T>
  sva::PTransform<T> pose(const std::vector<T> & q) const;
 
  sva::MotionVecd motion(const std::vector<double> & alpha) const;
 
  sva::MotionVecd tanAccel(const std::vector<double> & alphaD) const;
 
  std::vector<double> zeroParam() const;
 
  std::vector<double> zeroDof() const;
 
  sva::PTransformd sPose(const std::vector<double> & q) const;
 
  sva::MotionVecd sMotion(const std::vector<double> & alpha) const;
 
  sva::MotionVecd sTanAccel(const std::vector<double> & alphaD) const;
 
  bool operator==(const Joint & b) const
  {
    return name_ == b.name_;
  }
 
  bool operator!=(const Joint & b) const
  {
    return name_ != b.name_;
  }
 
public:
  static std::vector<double> ZeroParam(Type type);
 
  static std::vector<double> ZeroDof(Type type);
 
private:
  void constructJoint(Type t, const Eigen::Vector3d & a);
 
private:
  Type type_;
  Eigen::Matrix<double, 6, Eigen::Dynamic> S_;
  double dir_; 
 
  int params_;
  int dof_;
 
  std::string name_;
 
  bool isMimic_ = false;
  std::string mimicName_ = "";
  double mimicMultiplier_ = 1.0;
  double mimicOffset_ = 0.0;
};
 
inline std::ostream & operator<<(std::ostream & out, const Joint & b)
{
  out << "Joint: " << b.name();
  return out;
}
 
inline Joint::Joint() : dir_(0.0), params_(0), dof_(0), name_("") {}
 
inline Joint::Joint(OldType type, bool forward, std::string name)
: dir_(forward ? 1. : -1), params_(0), dof_(0), name_(name)
{
  switch(type)
  {
    case RevX:
      constructJoint(Rev, Eigen::Vector3d::UnitX());
      break;
    case RevY:
      constructJoint(Rev, Eigen::Vector3d::UnitY());
      break;
    case RevZ:
      constructJoint(Rev, Eigen::Vector3d::UnitZ());
      break;
    case PrismX:
      constructJoint(Prism, Eigen::Vector3d::UnitX());
      break;
    case PrismY:
      constructJoint(Prism, Eigen::Vector3d::UnitY());
      break;
    case PrismZ:
      constructJoint(Prism, Eigen::Vector3d::UnitZ());
      break;
    default:
      constructJoint(Fixed, Eigen::Vector3d::Zero());
      break;
  }
}
 
inline Joint::Joint(Type type, const Eigen::Vector3d & axis, bool forward, std::string name)
: dir_(forward ? 1. : -1), params_(0), dof_(0), name_(name)
{
  constructJoint(type, axis);
}
 
inline Joint::Joint(Type type, bool forward, std::string name)
: dir_(forward ? 1. : -1), params_(0), dof_(0), name_(name)
{
  constructJoint(type, Eigen::Vector3d::UnitZ());
}
 
inline Joint::Joint(std::string name) : dir_(1), params_(0), dof_(0), name_(name)
{
  constructJoint(Joint::Type::Fixed, Eigen::Vector3d::UnitZ());
}
 
inline void Joint::makeMimic(const std::string & name, double multiplier, double offset)
{
  isMimic_ = true;
  mimicName_ = name;
  mimicMultiplier_ = multiplier;
  mimicOffset_ = offset;
}
 
template<typename T>
inline sva::PTransform<T> Joint::pose(const std::vector<T> & q) const
{
  Eigen::Matrix3<T> rot;
  switch(type_)
  {
    case Rev:
      // minus S because rotation is anti trigonometric
      return sva::PTransform<T>(Eigen::AngleAxis<T>(-q[0], S_.block<3, 1>(0, 0).cast<T>()).matrix());
    case Prism:
      return sva::PTransform<T>(Eigen::Vector3<T>(S_.block<3, 1>(3, 0).cast<T>() * q[0]));
    case Spherical:
      return sva::PTransform<T>(Eigen::Quaternion<T>(q[0], dir_ * q[1], dir_ * q[2], dir_ * q[3]).inverse());
    case Planar:
      rot = sva::RotZ(q[0]);
      if(dir_ == 1.)
      {
        return sva::PTransform<T>(rot, rot.transpose() * Eigen::Vector3<T>(q[1], q[2], 0.));
      }
      else
      {
        return sva::PTransform<T>(rot, rot.transpose() * Eigen::Vector3<T>(q[1], q[2], 0.)).inv();
      }
    case Cylindrical:
      return sva::PTransform<T>(Eigen::AngleAxis<T>(-q[0], S_.col(0).head<3>().cast<T>()).matrix(),
                                S_.col(1).tail<3>().cast<T>() * q[1]);
    case Free:
      rot = QuatToE(q);
      if(dir_ == 1.)
      {
        return sva::PTransform<T>(rot, Eigen::Vector3<T>(q[4], q[5], q[6]));
      }
      else
      {
        return sva::PTransform<T>(rot, Eigen::Vector3<T>(q[4], q[5], q[6])).inv();
      }
    case Fixed:
    default:
      return sva::PTransform<T>::Identity();
  }
}
 
inline sva::MotionVecd Joint::motion(const std::vector<double> & alpha) const
{
  switch(type_)
  {
    case Rev:
      return sva::MotionVecd(
          (Eigen::Vector6d() << S_.block<3, 1>(0, 0) * alpha[0], Eigen::Vector3d::Zero()).finished());
    case Prism:
      return sva::MotionVecd(
          (Eigen::Vector6d() << Eigen::Vector3d::Zero(), S_.block<3, 1>(3, 0) * alpha[0]).finished());
    case Spherical:
      return sva::MotionVecd(S_ * Eigen::Vector3d(alpha[0], alpha[1], alpha[2]));
    case Planar:
      return sva::MotionVecd(S_ * Eigen::Vector3d(alpha[0], alpha[1], alpha[2]));
    case Cylindrical:
      return sva::MotionVecd(S_ * Eigen::Vector2d(alpha[0], alpha[1]));
    case Free:
      return sva::MotionVecd(
          S_ * (Eigen::Vector6d() << alpha[0], alpha[1], alpha[2], alpha[3], alpha[4], alpha[5]).finished());
    case Fixed:
    default:
      return sva::MotionVecd(Eigen::Vector6d::Zero());
  }
}
 
inline sva::MotionVecd Joint::tanAccel(const std::vector<double> & alphaD) const
{
  switch(type_)
  {
    case Rev:
      return sva::MotionVecd(
          (Eigen::Vector6d() << S_.block<3, 1>(0, 0) * alphaD[0], Eigen::Vector3d::Zero()).finished());
    case Prism:
      return sva::MotionVecd(
          (Eigen::Vector6d() << Eigen::Vector3d::Zero(), S_.block<3, 1>(3, 0) * alphaD[0]).finished());
    case Spherical:
      return sva::MotionVecd(S_ * Eigen::Vector3d(alphaD[0], alphaD[1], alphaD[2]));
    case Planar:
      return sva::MotionVecd(S_ * Eigen::Vector3d(alphaD[0], alphaD[1], alphaD[2]));
    case Cylindrical:
      return sva::MotionVecd(S_ * Eigen::Vector2d(alphaD[0], alphaD[1]));
    case Free:
      return sva::MotionVecd(
          S_ * (Eigen::Vector6d() << alphaD[0], alphaD[1], alphaD[2], alphaD[3], alphaD[4], alphaD[5]).finished());
    case Fixed:
    default:
      return sva::MotionVecd(Eigen::Vector6d::Zero());
  }
}
 
inline std::vector<double> Joint::zeroParam() const
{
  auto q = ZeroParam(type_);
  if(isMimic_)
  {
    for(auto & qi : q)
    {
      qi += mimicOffset_;
    }
  }
  return q;
}
 
inline std::vector<double> Joint::zeroDof() const
{
  return ZeroDof(type_);
}
 
inline sva::PTransformd Joint::sPose(const std::vector<double> & q) const
{
  if(q.size() != static_cast<unsigned int>(params_))
  {
    std::ostringstream str;
    str << "Wrong number of generalized position variable: expected " << params_ << " gived " << q.size();
    throw std::domain_error(str.str());
  }
  return pose(q);
}
 
inline sva::MotionVecd Joint::sMotion(const std::vector<double> & alpha) const
{
  if(alpha.size() != static_cast<unsigned int>(dof_))
  {
    std::ostringstream str;
    str << "Wrong number of generalized speed variable: expected " << params_ << " gived " << alpha.size();
    throw std::domain_error(str.str());
  }
  return motion(alpha);
}
 
inline sva::MotionVecd Joint::sTanAccel(const std::vector<double> & alphaD) const
{
  if(alphaD.size() != static_cast<unsigned int>(dof_))
  {
    std::ostringstream str;
    str << "Wrong number of generalized acceleration variable: expected " << params_ << " gived " << alphaD.size();
    throw std::domain_error(str.str());
  }
  return tanAccel(alphaD);
}
 
inline std::vector<double> Joint::ZeroParam(Type type)
{
  switch(type)
  {
    case Rev:
    case Prism:
      return {0.};
    case Spherical:
      return {1., 0., 0., 0.};
    case Planar:
      return {0., 0., 0.};
    case Cylindrical:
      return {0., 0.};
    case Free:
      return {1., 0., 0., 0., 0., 0., 0.};
    case Fixed:
    default:
      return {};
  }
}
 
inline std::vector<double> Joint::ZeroDof(Type type)
{
  switch(type)
  {
    case Rev:
    case Prism:
      return {0.};
    case Spherical:
      return {0., 0., 0.};
    case Planar:
      return {0., 0., 0.};
    case Cylindrical:
      return {0., 0.};
    case Free:
      return {0., 0., 0., 0., 0., 0.};
    case Fixed:
    default:
      return {};
  }
}
 
inline void Joint::constructJoint(Type t, const Eigen::Vector3d & a)
{
  type_ = t;
 
  switch(t)
  {
    case Rev:
      S_ = dir_ * (Eigen::Vector6d() << a, Eigen::Vector3d::Zero()).finished();
      params_ = 1;
      dof_ = 1;
      break;
    case Prism:
      S_ = dir_ * (Eigen::Vector6d() << Eigen::Vector3d::Zero(), a).finished();
      params_ = 1;
      dof_ = 1;
      break;
    case Spherical:
      S_ = Eigen::Matrix<double, 6, 3>::Zero();
      S_.block<3, 3>(0, 0).setIdentity();
      S_ *= dir_;
      params_ = 4;
      dof_ = 3;
      break;
    case Planar:
      S_ = Eigen::Matrix<double, 6, 3>::Zero();
      S_.block<3, 3>(2, 0).setIdentity();
      S_ *= dir_;
      params_ = 3;
      dof_ = 3;
      break;
    case Cylindrical:
      S_ = Eigen::Matrix<double, 6, 2>::Zero();
      S_.col(0).head<3>() = a;
      S_.col(1).tail<3>() = a;
      S_ *= dir_;
      params_ = 2;
      dof_ = 2;
      break;
    case Free:
      S_ = dir_ * Eigen::Matrix6d::Identity();
      params_ = 7;
      dof_ = 6;
      break;
    case Fixed:
    default:
      S_ = Eigen::Matrix<double, 6, 0>::Zero();
      params_ = 0;
      dof_ = 0;
      break;
  }
}
 
template<typename T>
inline Eigen::Matrix3<T> QuatToE(const std::vector<T> & q)
{
  T p0 = q[0];
  T p1 = q[1];
  T p2 = q[2];
  T p3 = q[3];
 
  T p0p1 = p0 * p1;
  T p0p2 = p0 * p2;
  T p0p3 = p0 * p3;
 
  T p1p2 = p1 * p2;
  T p1p3 = p1 * p3;
 
  T p2p3 = p2 * p3;
 
  T p0s = std::pow(p0, 2);
  T p1s = std::pow(p1, 2);
  T p2s = std::pow(p2, 2);
  T p3s = std::pow(p3, 2);
 
  return 2.
         * (Eigen::Matrix3<T>() << p0s + p1s - 0.5, p1p2 + p0p3, p1p3 - p0p2, p1p2 - p0p3, p0s + p2s - 0.5, p2p3 + p0p1,
            p1p3 + p0p2, p2p3 - p0p1, p0s + p3s - 0.5)
               .finished();
}
 
} // namespace rbd