GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/force-set.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 53 0.0%
Branches: 0 92 0.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 ✗ ForceSetTpl(const int & ncols)
34 ✗ : size(ncols)
35 ✗ , m_f(3, ncols)
36 ✗ , m_n(3, ncols)
37 {
38 ✗ m_f.fill(NAN);
39 ✗ m_n.fill(NAN);
40 }
41 ✗ ForceSetTpl(const Matrix3x & linear, const Matrix3x & angular)
42 ✗ : size((int)linear.cols())
43 ✗ , m_f(linear)
44 ✗ , m_n(angular)
45 {
46 ✗ assert(linear.cols() == angular.cols());
47 }
48
49 ✗ Matrix6x matrix() const
50 {
51 ✗ Matrix6x F(6, size);
52 ✗ F << m_f, m_n;
53 // F.template topRows<3>() = m_f;
54 // F.template bottomRows<3>() = m_n;
55 ✗ return F;
56 }
57 operator Matrix6x() const
58 {
59 return matrix();
60 }
61
62 // Getters
63 ✗ const Matrix3x & linear() const
64 {
65 ✗ return m_f;
66 }
67 ✗ const Matrix3x & angular() const
68 {
69 ✗ return m_n;
70 }
71
72 /// af = aXb.act(bf)
73 ✗ ForceSetTpl se3Action(const SE3 & m) const
74 {
75 ✗ Matrix3x Rf(m.rotation() * linear());
76 ✗ return ForceSetTpl(Rf, skew(m.translation()) * Rf + m.rotation() * angular());
77 // TODO check if nothing better than explicitely calling skew
78 }
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 ✗ Block(ForceSetTpl & ref, const int & idx, const int & len)
100 ✗ : ref(ref)
101 ✗ , idx(idx)
102 ✗ , len(len)
103 {
104 }
105
106 ✗ Eigen::Block<ForceSetTpl::Matrix3x> linear()
107 {
108 ✗ return ref.m_f.block(0, idx, 3, len);
109 }
110 ✗ Eigen::Block<ForceSetTpl::Matrix3x> angular()
111 {
112 ✗ return ref.m_n.block(0, idx, 3, len);
113 }
114 ✗ Eigen::Block<const ForceSetTpl::Matrix3x> linear() const
115 {
116 ✗ return ((const ForceSetTpl::Matrix3x &)(ref.m_f)).block(0, idx, 3, len);
117 }
118 ✗ Eigen::Block<const ForceSetTpl::Matrix3x> angular() const
119 {
120 ✗ return ((const ForceSetTpl::Matrix3x &)(ref.m_n)).block(0, idx, 3, len);
121 }
122
123 ✗ ForceSetTpl::Matrix6x matrix() const
124 {
125 ✗ ForceSetTpl::Matrix6x res(6, len);
126 ✗ res << linear(), angular();
127 ✗ return res;
128 }
129
130 ✗ Block & operator=(const ForceSetTpl & copy)
131 {
132 ✗ assert(copy.size == len);
133 ✗ linear() = copy.linear(); // ref.m_f.block(0,idx,3,len) = copy.m_f;
134 ✗ angular() = copy.angular(); // ref.m_n.block(0,idx,3,len) = copy.m_n;
135 ✗ 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 ✗ Block & operator=(const Eigen::MatrixBase<D> & m)
150 {
151 eigen_assert(D::RowsAtCompileTime == 6);
152 ✗ assert(m.cols() == len);
153 ✗ linear() = m.template topRows<3>();
154 ✗ angular() = m.template bottomRows<3>();
155 ✗ return *this;
156 }
157
158 /// af = aXb.act(bf)
159 ✗ 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 ✗ Matrix3x Rf((m.rotation() * linear()));
164 ✗ return ForceSetTpl(Rf, skew(m.translation()) * Rf + m.rotation() * angular());
165 // TODO check if nothing better than explicitely calling skew
166 }
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 ✗ Block block(const int & idx, const int & len)
180 {
181 ✗ 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