GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/force-set.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 58 58 100.0%
Branches: 46 92 50.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015 CNRS
3 //
4
5 #ifndef __pinocchio_spatial_force_set_hpp__
6 #define __pinocchio_spatial_force_set_hpp__
7
8 #include "pinocchio/spatial/fwd.hpp"
9 #include <Eigen/Geometry>
10
11 namespace pinocchio
12 {
13 template<typename _Scalar, int _Options>
14 class ForceSetTpl
15 {
16 public:
17 typedef _Scalar Scalar;
18 enum
19 {
20 Options = _Options
21 };
22 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
23 typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
24 typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
25 typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
26 typedef SE3Tpl<Scalar, Options> SE3;
27
28 typedef Eigen::Matrix<Scalar, 3, Eigen::Dynamic, Options> Matrix3x;
29 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> Matrix6x;
30
31 public:
32 // Constructors
33 3 ForceSetTpl(const int & ncols)
34 3 : size(ncols)
35
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 , m_f(3, ncols)
36
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 , m_n(3, ncols)
37 {
38
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 m_f.fill(NAN);
39
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 m_n.fill(NAN);
40 3 }
41 9 ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
42 9 : size((int)linear.cols())
43 9 , m_f(linear)
44
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 , m_n(angular)
45 {
46
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
 9 assert(linear.cols() == angular.cols());
47 9 }
48
49 12 Matrix6x matrix() const
50 {
51
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Matrix6x F(6, size);
52
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 F << m_f, m_n;
53 // F.template topRows<3>() = m_f;
54 // F.template bottomRows<3>() = m_n;
55 12 return F;
56 }
57 operator Matrix6x() const
58 {
59 return matrix();
60 }
61
62 // Getters
63 6 const Matrix3x & linear() const
64 {
65 6 return m_f;
66 }
67 6 const Matrix3x & angular() const
68 {
69 6 return m_n;
70 }
71
72 /// af = aXb.act(bf)
73 4 ForceSetTpl se3Action(const SE3 & m) const
74 {
75
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 Matrix3x Rf(m.rotation() * linear());
76
8/16
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 4 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 4 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 4 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 4 times.
✗ Branch 24 not taken.
 8 return ForceSetTpl(Rf, skew(m.translation()) * Rf + m.rotation() * angular());
77 // TODO check if nothing better than explicitely calling skew
78 4 }
79 /// bf = aXb.actInv(af)
80 ForceSetTpl se3ActionInverse(const SE3 & m) const
81 {
82 return ForceSetTpl(
83 m.rotation().transpose() * linear(),
84 m.rotation().transpose() * (angular() - skew(m.translation()) * linear()));
85 // TODO check if nothing better than explicitely calling skew
86 }
87
88 friend std::ostream & operator<<(std::ostream & os, const ForceSetTpl & phi)
89 {
90 os << "F =\n" << phi.linear() << std::endl << "Tau =\n" << phi.angular() << std::endl;
91 return os;
92 }
93
94 /* --- BLOCK ------------------------------------------------------------ */
95 struct Block
96 {
97 ForceSetTpl & ref;
98 int idx, len;
99 6 Block(ForceSetTpl & ref, const int & idx, const int & len)
100 6 : ref(ref)
101 6 , idx(idx)
102 6 , len(len)
103 {
104 6 }
105
106 3 Eigen::Block<ForceSetTpl::Matrix3x> linear()
107 {
108 3 return ref.m_f.block(0, idx, 3, len);
109 }
110 3 Eigen::Block<ForceSetTpl::Matrix3x> angular()
111 {
112 3 return ref.m_n.block(0, idx, 3, len);
113 }
114 3 Eigen::Block<const ForceSetTpl::Matrix3x> linear() const
115 {
116 3 return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0, idx, 3, len);
117 }
118 3 Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
119 {
120 3 return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0, idx, 3, len);
121 }
122
123 1 ForceSetTpl::Matrix6x matrix() const
124 {
125
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ForceSetTpl::Matrix6x res(6, len);
126
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 res << linear(), angular();
127 1 return res;
128 }
129
130 2 Block & operator=(const ForceSetTpl & copy)
131 {
132
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 2 times.
 2 assert(copy.size == len);
133
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 linear() = copy.linear(); // ref.m_f.block(0,idx,3,len) = copy.m_f;
134
1/2
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
 2 angular() = copy.angular(); // ref.m_n.block(0,idx,3,len) = copy.m_n;
135 2 return *this;
136 }
137
138 Block & operator=(const ForceSetTpl::Block & copy)
139 {
140 assert(copy.len == len);
141 linear() =
142 copy.linear(); // ref.m_f.block(0,idx,3,len) = copy.ref.m_f.block(0,copy.idx,3,copy.len);
143 angular() =
144 copy.angular(); // ref.m_n.block(0,idx,3,len) = copy.ref.m_n.block(0,copy.idx,3,copy.len);
145 return *this;
146 }
147
148 template<typename D>
149 1 Block & operator=(const Eigen::MatrixBase<D> & m)
150 {
151 eigen_assert(D::RowsAtCompileTime == 6);
152
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(m.cols() == len);
153
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 linear() = m.template topRows<3>();
154
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 angular() = m.template bottomRows<3>();
155 1 return *this;
156 }
157
158 /// af = aXb.act(bf)
159 2 ForceSetTpl se3Action(const SE3 & m) const
160 {
161 // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
162 // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
163
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
 2 Matrix3x Rf((m.rotation() * linear()));
164
9/18
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
 4 return ForceSetTpl(Rf, skew(m.translation()) * Rf + m.rotation() * angular());
165 // TODO check if nothing better than explicitely calling skew
166 2 }
167 /// bf = aXb.actInv(af)
168 ForceSetTpl se3ActionInverse(const SE3 & m) const
169 {
170 // const Eigen::Block<const Matrix3x> linear = ref.linear().block(0,idx,3,len);
171 // const Eigen::Block<const Matrix3x> angular = ref.angular().block(0,idx,3,len);
172 return ForceSetTpl(
173 m.rotation().transpose() * linear(),
174 m.rotation().transpose() * (angular() - skew(m.translation()) * linear()));
175 // TODO check if nothing better than explicitely calling skew
176 }
177 };
178
179 6 Block block(const int & idx, const int & len)
180 {
181 6 return Block(*this, idx, len);
182 }
183
184 /* CRBA joint operators
185 * - ForceSet::Block = ForceSet
186 * - ForceSet operator* (Inertia Y,Constraint S)
187 * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
188 * - SE3::act(ForceSet::Block)
189 */
190
191 public: //
192 private:
193 int size;
194 Matrix3x m_f, m_n;
195 };
196
197 typedef ForceSetTpl<context::Scalar, context::Options> ForceSet;
198
199 template<>
200 struct SE3GroupAction<ForceSet::Block>
201 {
202 typedef ForceSet ReturnType;
203 };
204
205 } // namespace pinocchio
206
207 #endif // ifndef __pinocchio_spatial_force_set_hpp__
208