joint-common-operations.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019 INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
6 #define __pinocchio_multibody_joint_joint_common_operations_hpp__
7 
8 #include "pinocchio/macros.hpp"
9 #include "pinocchio/math/matrix.hpp"
10 
11 #include <boost/type_traits.hpp>
12 
13 namespace pinocchio
14 {
15  namespace internal
16  {
20  template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value>
22  {
23  template<typename M1, typename M2>
24  static EIGEN_STRONG_INLINE
25  void run(const Eigen::MatrixBase<M1> & StYS,
26  const Eigen::MatrixBase<M2> & Dinv)
27  {
28  M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2,Dinv);
29  Dinv_.setIdentity();
30  StYS.llt().solveInPlace(Dinv_);
31  }
32  };
33 
34  template<typename Scalar>
36  {
37  template<typename M1, typename M2>
38  static EIGEN_STRONG_INLINE
39  void run(const Eigen::MatrixBase<M1> & StYS,
40  const Eigen::MatrixBase<M2> & Dinv)
41  {
42  M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2,Dinv);
43  inverse(StYS,Dinv_);
44  }
45  };
46  }
47 
53  {
54  template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
55  static void run(const Eigen::MatrixBase<ConfigVectorIn> & q,
56  const Scalar & scaling,
57  const Scalar & offset,
58  const Eigen::MatrixBase<ConfigVectorOut> & dest)
59  {
60  assert(q.size() == dest.size());
61  PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest).noalias() = scaling * q + ConfigVectorOut::Constant(dest.size(),offset);
62  }
63  };
64 
68  template<typename Joint>
70  {
72  };
73 
74 }
75 
76 #endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
Linear affine transformation of the configuration vector. Valide for most common joints which are evo...
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
Assign the correct configuration vector space affine transformation according to the joint type...
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
SE3::Scalar Scalar
Definition: conversions.cpp:13
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
Operation called in JointModelBase<JointModel>::calc_aba.
static void run(const Eigen::MatrixBase< ConfigVectorIn > &q, const Scalar &scaling, const Scalar &offset, const Eigen::MatrixBase< ConfigVectorOut > &dest)
Main pinocchio namespace.
Definition: timings.cpp:28


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30