Line |
Branch |
Exec |
Source |
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 void |
25 |
|
716 |
run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv) |
26 |
|
|
{ |
27 |
|
716 |
M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv); |
28 |
|
716 |
Dinv_.setIdentity(); |
29 |
1/2
✓ Branch 2 taken 358 times.
✗ Branch 3 not taken.
|
716 |
StYS.llt().solveInPlace(Dinv_); |
30 |
|
|
} |
31 |
|
|
}; |
32 |
|
|
|
33 |
|
|
template<typename Scalar> |
34 |
|
|
struct PerformStYSInversion<Scalar, false> |
35 |
|
|
{ |
36 |
|
|
template<typename M1, typename M2> |
37 |
|
|
static EIGEN_STRONG_INLINE void |
38 |
|
|
run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv) |
39 |
|
|
{ |
40 |
|
|
M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv); |
41 |
|
|
inverse(StYS, Dinv_); |
42 |
|
|
} |
43 |
|
|
}; |
44 |
|
|
} // namespace internal |
45 |
|
|
|
46 |
|
|
/// |
47 |
|
|
/// \brief Linear affine transformation of the configuration vector. |
48 |
|
|
/// Valide for most common joints which are evolving on a vector space. |
49 |
|
|
/// |
50 |
|
|
struct LinearAffineTransform |
51 |
|
|
{ |
52 |
|
|
template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut> |
53 |
|
✗ |
static void run( |
54 |
|
|
const Eigen::MatrixBase<ConfigVectorIn> & q, |
55 |
|
|
const Scalar & scaling, |
56 |
|
|
const Scalar & offset, |
57 |
|
|
const Eigen::MatrixBase<ConfigVectorOut> & dest) |
58 |
|
|
{ |
59 |
|
✗ |
assert(q.size() == dest.size()); |
60 |
|
✗ |
PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, dest).noalias() = |
61 |
|
|
scaling * q + ConfigVectorOut::Constant(dest.size(), offset); |
62 |
|
|
} |
63 |
|
|
}; |
64 |
|
|
|
65 |
|
|
/// |
66 |
|
|
/// \brief Assign the correct configuration vector space affine transformation according to the |
67 |
|
|
/// joint type. |
68 |
|
|
/// |
69 |
|
|
template<typename Joint> |
70 |
|
|
struct ConfigVectorAffineTransform |
71 |
|
|
{ |
72 |
|
|
typedef LinearAffineTransform Type; |
73 |
|
|
}; |
74 |
|
|
|
75 |
|
|
} // namespace pinocchio |
76 |
|
|
|
77 |
|
|
#endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__ |
78 |
|
|
|