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 = boost::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_);
      }
    };
  }
  
  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;
  };
  
}

#endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__