GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-common-operations.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 22 24 91.7%
Branches: 13 33 39.4%
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 #include "pinocchio/math/fwd.hpp"
11
12 #include <boost/type_traits.hpp>
13 #include <boost/variant.hpp>
14
15 namespace pinocchio
16 {
17 namespace internal
18 {
19 ///
20 /// \brief Operation called in JointModelBase<JointModel>::calc_aba
21 ///
22 template<typename Scalar, bool is_floating_point = pinocchio::is_floating_point<Scalar>::value>
23 struct PerformStYSInversion
24 {
25 template<typename M1, typename M2>
26 static EIGEN_STRONG_INLINE void
27 5396 run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv)
28 {
29 5396 M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv);
30 5396 Dinv_.setIdentity();
31
1/2
✓ Branch 2 taken 2698 times.
✗ Branch 3 not taken.
 5396 StYS.llt().solveInPlace(Dinv_);
32 5396 }
33 };
34
35 template<typename Scalar>
36 struct PerformStYSInversion<Scalar, false>
37 {
38 template<typename M1, typename M2>
39 static EIGEN_STRONG_INLINE void
40 220 run(const Eigen::MatrixBase<M1> & StYS, const Eigen::MatrixBase<M2> & Dinv)
41 {
42 220 M2 & Dinv_ = PINOCCHIO_EIGEN_CONST_CAST(M2, Dinv);
43 220 inverse(StYS, Dinv_);
44 }
45 };
46 } // namespace internal
47
48 ///
49 /// \brief Linear affine transformation of the configuration vector.
50 /// Valide for most common joints which are evolving on a vector space.
51 ///
52 struct LinearAffineTransform
53 {
54 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
55 1855 static void run(
56 const Eigen::MatrixBase<ConfigVectorIn> & qIn,
57 const Scalar & scaling,
58 const Scalar & offset,
59 const Eigen::MatrixBase<ConfigVectorOut> & qOut)
60 {
61
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1820 times.
 1855 assert(qIn.size() == qOut.size());
62
4/8
✓ Branch 3 taken 1820 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1820 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1820 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1820 times.
✗ Branch 14 not taken.
 1855 PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut).noalias() =
63 scaling * qIn + ConfigVectorOut::Constant(qOut.size(), offset);
64 1855 }
65 };
66
67 struct UnboundedRevoluteAffineTransform
68 {
69 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
70 28 static void run(
71 const Eigen::MatrixBase<ConfigVectorIn> & qIn,
72 const Scalar & scaling,
73 const Scalar & offset,
74 const Eigen::MatrixBase<ConfigVectorOut> & qOut)
75 {
76
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
 28 assert(qIn.size() == 2);
77
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 14 times.
 28 assert(qOut.size() == 2);
78
79
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 const typename ConfigVectorIn::Scalar & ca = qIn(0);
80
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 const typename ConfigVectorIn::Scalar & sa = qIn(1);
81
82
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 const typename ConfigVectorIn::Scalar & theta = math::atan2(sa, ca);
83
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 28 const typename ConfigVectorIn::Scalar & theta_transform = scaling * theta + offset;
84
85 28 ConfigVectorOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorOut, qOut);
86
2/7
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 28 SINCOS(theta_transform, &dest_.coeffRef(1), &dest_.coeffRef(0));
87 28 }
88 };
89
90 struct NoAffineTransform
91 {
92 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
93 ✗ static void run(
94 const Eigen::MatrixBase<ConfigVectorIn> &,
95 const Scalar &,
96 const Scalar &,
97 const Eigen::MatrixBase<ConfigVectorOut> &)
98 {
99 ✗ assert(false && "Joint cannot be used with JointMimic.");
100 }
101 };
102
103 ///
104 /// \brief Assign the correct configuration vector space affine transformation according to the
105 /// joint type. Must be specialized for every joint type.
106 ///
107 template<typename Joint>
108 struct ConfigVectorAffineTransform
109 {
110 typedef NoAffineTransform Type;
111 };
112
113 } // namespace pinocchio
114
115 #endif // ifndef __pinocchio_multibody_joint_joint_common_operations_hpp__
116