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 |
|
|
{ |
17 |
|
|
/// |
18 |
|
|
/// \brief Operation called in JointModelBase<JointModel>::calc_aba |
19 |
|
|
/// |
20 |
|
|
template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value> |
21 |
|
|
struct PerformStYSInversion |
22 |
|
|
{ |
23 |
|
|
template<typename M1, typename M2> |
24 |
|
|
static EIGEN_STRONG_INLINE |
25 |
|
670 |
void run(const Eigen::MatrixBase<M1> & StYS, |
26 |
|
|
const Eigen::MatrixBase<M2> & Dinv) |
27 |
|
|
{ |
28 |
|
670 |
M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2,Dinv); |
29 |
|
670 |
Dinv_.setIdentity(); |
30 |
✓✗ |
670 |
StYS.llt().solveInPlace(Dinv_); |
31 |
|
670 |
} |
32 |
|
|
}; |
33 |
|
|
|
34 |
|
|
template<typename Scalar> |
35 |
|
|
struct PerformStYSInversion<Scalar, false> |
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 |
|
|
|
48 |
|
|
/// |
49 |
|
|
/// \brief Linear affine transformation of the configuration vector. |
50 |
|
|
/// Valide for most common joints which are evolving on a vectorial space. |
51 |
|
|
/// |
52 |
|
|
struct LinearAffineTransform |
53 |
|
|
{ |
54 |
|
|
template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut> |
55 |
|
84 |
static void run(const Eigen::MatrixBase<ConfigVectorIn> & q, |
56 |
|
|
const Scalar & scaling, |
57 |
|
|
const Scalar & offset, |
58 |
|
|
const Eigen::MatrixBase<ConfigVectorOut> & dest) |
59 |
|
|
{ |
60 |
✗✓ |
84 |
assert(q.size() == dest.size()); |
61 |
✓✗✓✗
✓✗✓✗
|
84 |
PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut,dest).noalias() = scaling * q + ConfigVectorOut::Constant(dest.size(),offset); |
62 |
|
84 |
} |
63 |
|
|
}; |
64 |
|
|
|
65 |
|
|
/// |
66 |
|
|
/// \brief Assign the correct configuration vector space affine transformation according to the joint type. |
67 |
|
|
/// |
68 |
|
|
template<typename Joint> |
69 |
|
|
struct ConfigVectorAffineTransform |
70 |
|
|
{ |
71 |
|
|
typedef LinearAffineTransform Type; |
72 |
|
|
}; |
73 |
|
|
|
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
#endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__ |