joint-common-operations.hpp

//
// Copyright (c) 2019 INRIA
//
 
#ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
#define __pinocchio_multibody_joint_joint_common_operations_hpp__
 
#include "pinocchio/macros.hpp"
#include "pinocchio/math/matrix.hpp"
 
#include <boost/type_traits.hpp>
 
namespace pinocchio
{
  namespace internal
  {
    template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value>
    struct PerformStYSInversion
    {
      template<typename M1, typename M2>
      static EIGEN_STRONG_INLINE void
      run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv)
      {
        M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv);
        Dinv_.setIdentity();
        StYS.llt().solveInPlace(Dinv_);
      }
    };
 
    template<typename Scalar>
    struct PerformStYSInversion<Scalar, false>
    {
      template<typename M1, typename M2>
      static EIGEN_STRONG_INLINE void
      run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv)
      {
        M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv);
        inverse(StYS, Dinv_);
      }
    };
  } // namespace internal
 
  struct LinearAffineTransform
  {
    template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
    static void run(
      const Eigen::MatrixBase<ConfigVectorIn> & q,
      const Scalar & scaling,
      const Scalar & offset,
      const Eigen::MatrixBase<ConfigVectorOut> & dest)
    {
      assert(q.size() == dest.size());
      PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() =
        scaling * q + ConfigVectorOut::Constant(dest.size(), offset);
    }
  };
 
  template<typename Joint>
  struct ConfigVectorAffineTransform
  {
    typedef LinearAffineTransform Type;
  };
 
} // namespace pinocchio
 
#endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__