| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2018-2020 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_regressor_hxx__ |
||
6 |
#define __pinocchio_algorithm_regressor_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/algorithm/check.hpp" |
||
9 |
#include "pinocchio/algorithm/kinematics.hpp" |
||
10 |
#include "pinocchio/spatial/skew.hpp" |
||
11 |
#include "pinocchio/spatial/symmetric3.hpp" |
||
12 |
|||
13 |
namespace pinocchio |
||
14 |
{ |
||
15 |
|||
16 |
namespace internal |
||
17 |
{ |
||
18 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
||
19 |
573 |
void computeJointKinematicRegressorGeneric(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
20 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
21 |
const JointIndex joint_id, |
||
22 |
const ReferenceFrame rf, |
||
23 |
const SE3Tpl<Scalar,Options> & global_frame_placement, |
||
24 |
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) |
||
25 |
{ |
||
26 |
✓✗✓✗ |
573 |
assert(model.check(data) && "data is not consistent with model."); |
27 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
573 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.rows(), 6); |
28 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
573 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(kinematic_regressor.cols(), 6*(model.njoints-1)); |
29 |
|||
30 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
31 |
typedef typename Data::SE3 SE3; |
||
32 |
|||
33 |
573 |
Matrix6xReturnType & kinematic_regressor_ = kinematic_regressor.const_cast_derived(); |
|
34 |
✓✗ | 573 |
kinematic_regressor_.setZero(); |
35 |
|||
36 |
573 |
const SE3Tpl<Scalar,Options> & oMi = global_frame_placement; |
|
37 |
✓✗ | 573 |
SE3 oMp; // placement of the frame following the jointPlacement transform |
38 |
✓✗ | 573 |
SE3 iMp; // relative placement between the joint frame and the jointPlacement |
39 |
✓✓ | 3519 |
for(JointIndex i = joint_id; i > 0; i = model.parents[i]) |
40 |
{ |
||
41 |
2946 |
const JointIndex parent_id = model.parents[i]; |
|
42 |
✓✗ | 2946 |
oMp = data.oMi[parent_id] * model.jointPlacements[i]; |
43 |
✓✓✓✗ |
2946 |
switch(rf) |
44 |
{ |
||
45 |
982 |
case LOCAL: |
|
46 |
✓✗ | 982 |
iMp = oMi.actInv(oMp); |
47 |
✓✗✓✗ ✓✗ |
982 |
kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = iMp.toActionMatrix(); // TODO: we can avoid a copy |
48 |
982 |
break; |
|
49 |
982 |
case LOCAL_WORLD_ALIGNED: |
|
50 |
✓✗✓✗ ✓✗ |
982 |
iMp.rotation() = oMp.rotation(); |
51 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
982 |
iMp.translation() = oMp.translation() - oMi.translation(); |
52 |
✓✗✓✗ ✓✗ |
982 |
kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = iMp.toActionMatrix(); // TODO: we can avoid a copy |
53 |
982 |
break; |
|
54 |
982 |
case WORLD: |
|
55 |
✓✗✓✗ ✓✗ |
982 |
kinematic_regressor_.template middleCols<6>((Eigen::DenseIndex)(6*(i-1))) = oMp.toActionMatrix(); // TODO: we can avoid a copy |
56 |
982 |
break; |
|
57 |
} |
||
58 |
} |
||
59 |
573 |
} |
|
60 |
} |
||
61 |
|||
62 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
||
63 |
162 |
void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
64 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
65 |
const JointIndex joint_id, |
||
66 |
const ReferenceFrame rf, |
||
67 |
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) |
||
68 |
{ |
||
69 |
✓✗✗✓ ✗✗ |
162 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints); |
70 |
162 |
internal::computeJointKinematicRegressorGeneric(model,data,joint_id,rf,data.oMi[joint_id], |
|
71 |
kinematic_regressor.const_cast_derived()); |
||
72 |
162 |
} |
|
73 |
|||
74 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
||
75 |
246 |
void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
76 |
const DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
77 |
const JointIndex joint_id, |
||
78 |
const ReferenceFrame rf, |
||
79 |
const SE3Tpl<Scalar,Options> & placement, |
||
80 |
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) |
||
81 |
{ |
||
82 |
✓✗✗✓ ✗✗ |
246 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_id > 0 && (Eigen::DenseIndex)joint_id < model.njoints); |
83 |
|||
84 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
85 |
typedef typename Data::SE3 SE3; |
||
86 |
|||
87 |
✓✗ | 246 |
const SE3 global_placement = data.oMi[joint_id] * placement; |
88 |
|||
89 |
✓✗ | 246 |
internal::computeJointKinematicRegressorGeneric(model,data,joint_id,rf,global_placement, |
90 |
kinematic_regressor.const_cast_derived()); |
||
91 |
246 |
} |
|
92 |
|||
93 |
|||
94 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xReturnType> |
||
95 |
165 |
void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
96 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
97 |
const FrameIndex frame_id, |
||
98 |
const ReferenceFrame rf, |
||
99 |
const Eigen::MatrixBase<Matrix6xReturnType> & kinematic_regressor) |
||
100 |
{ |
||
101 |
✓✗✗✓ ✗✗ |
165 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id > 0 && (Eigen::DenseIndex)frame_id < model.nframes); |
102 |
|||
103 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
104 |
typedef typename Model::Frame Frame; |
||
105 |
|||
106 |
165 |
const Frame & frame = model.frames[frame_id]; |
|
107 |
165 |
data.oMf[frame_id] = data.oMi[frame.parent] * frame.placement; |
|
108 |
|||
109 |
165 |
internal::computeJointKinematicRegressorGeneric(model,data,frame.parent,rf,data.oMf[frame_id], |
|
110 |
kinematic_regressor.const_cast_derived()); |
||
111 |
165 |
} |
|
112 |
|||
113 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType> |
||
114 |
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix3x & |
||
115 |
1 |
computeStaticRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
116 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
117 |
const Eigen::MatrixBase<ConfigVectorType> & q) |
||
118 |
{ |
||
119 |
✓✗✓✗ |
1 |
assert(model.check(data) && "data is not consistent with model."); |
120 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
121 |
|||
122 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
123 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
124 |
typedef typename Model::JointIndex JointIndex; |
||
125 |
typedef typename Data::SE3 SE3; |
||
126 |
|||
127 |
typedef typename Data::Matrix3x Matrix3x; |
||
128 |
typedef typename SizeDepType<4>::ColsReturn<Matrix3x>::Type ColsBlock; |
||
129 |
|||
130 |
✓✗ | 1 |
forwardKinematics(model,data,q.derived()); |
131 |
|||
132 |
// Computes the total mass of the system |
||
133 |
1 |
Scalar mass = Scalar(0); |
|
134 |
✓✓ | 28 |
for(JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
135 |
27 |
mass += model.inertias[(JointIndex)i].mass(); |
|
136 |
|||
137 |
1 |
const Scalar mass_inv = Scalar(1)/mass; |
|
138 |
✓✓ | 28 |
for(JointIndex i = 1; i < (JointIndex)model.njoints; ++i) |
139 |
{ |
||
140 |
27 |
const SE3 & oMi = data.oMi[i]; |
|
141 |
✓✗ | 27 |
ColsBlock sr_cols = data.staticRegressor.template middleCols<4>((Eigen::DenseIndex)(i-1)*4); |
142 |
✓✗✓✗ ✓✗ |
27 |
sr_cols.col(0) = oMi.translation(); |
143 |
✓✗✓✗ ✓✗ |
27 |
sr_cols.template rightCols<3>() = oMi.rotation(); |
144 |
✓✗ | 27 |
sr_cols *= mass_inv; |
145 |
} |
||
146 |
|||
147 |
1 |
return data.staticRegressor; |
|
148 |
} |
||
149 |
|||
150 |
namespace details { |
||
151 |
// auxiliary function for bodyRegressor: bigL(omega)*I.toDynamicParameters().tail<6>() = I.inertia() * omega |
||
152 |
/* |
||
153 |
template<typename Vector3Like> |
||
154 |
inline Eigen::Matrix<typename Vector3Like::Scalar,3,6,PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options> |
||
155 |
bigL(const Eigen::MatrixBase<Vector3Like> & omega) |
||
156 |
{ |
||
157 |
typedef typename Vector3Like::Scalar Scalar; |
||
158 |
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options }; |
||
159 |
typedef Eigen::Matrix<Scalar,3,6,Options> ReturnType; |
||
160 |
|||
161 |
ReturnType L; |
||
162 |
L << omega[0], omega[1], Scalar(0), omega[2], Scalar(0), Scalar(0), |
||
163 |
Scalar(0), omega[0], omega[1], Scalar(0), omega[2], Scalar(0), |
||
164 |
Scalar(0), Scalar(0), Scalar(0), omega[0], omega[1], omega[2]; |
||
165 |
return L; |
||
166 |
} |
||
167 |
*/ |
||
168 |
|||
169 |
// auxiliary function for bodyRegressor: res += bigL(omega) |
||
170 |
template<typename Vector3Like, typename OutputType> |
||
171 |
inline void |
||
172 |
30 |
addBigL(const Eigen::MatrixBase<Vector3Like> & omega, const Eigen::MatrixBase<OutputType> & out) |
|
173 |
{ |
||
174 |
30 |
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out); |
|
175 |
30 |
res(0,0)+=omega[0]; res(0,1)+=omega[1]; res(0,3)+=omega[2]; |
|
176 |
30 |
res(1,1)+=omega[0]; res(1,2)+=omega[1]; res(1,4)+=omega[2]; |
|
177 |
30 |
res(2,3)+=omega[0]; res(2,4)+=omega[1]; res(2,5)+=omega[2]; |
|
178 |
30 |
} |
|
179 |
|||
180 |
// auxiliary function for bodyRegressor: res = cross(omega,bigL(omega)) |
||
181 |
template<typename Vector3Like, typename OutputType> |
||
182 |
inline void |
||
183 |
30 |
crossBigL(const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<OutputType> & out) |
|
184 |
{ |
||
185 |
typedef typename Vector3Like::Scalar Scalar; |
||
186 |
30 |
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, out); |
|
187 |
|||
188 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
30 |
res << Scalar(0), -v[2]*v[0], -v[2]*v[1], v[1]*v[0], v[1]*v[1]-v[2]*v[2], v[2]*v[1], |
189 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
30 |
v[2]*v[0], v[2]*v[1], Scalar(0), v[2]*v[2]-v[0]*v[0], -v[1]*v[0], -v[2]*v[0], |
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
30 |
-v[1]*v[0], v[0]*v[0]-v[1]*v[1], v[1]*v[0], -v[2]*v[1], v[2]*v[0], Scalar(0); |
191 |
30 |
} |
|
192 |
} |
||
193 |
|||
194 |
template<typename MotionVelocity, typename MotionAcceleration, typename OutputType> |
||
195 |
inline void |
||
196 |
30 |
bodyRegressor(const MotionDense<MotionVelocity> & v, |
|
197 |
const MotionDense<MotionAcceleration> & a, |
||
198 |
const Eigen::MatrixBase<OutputType> & regressor) |
||
199 |
{ |
||
200 |
✓✗✓✗ ✓✗✓✗ |
30 |
PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, regressor, 6, 10); |
201 |
|||
202 |
typedef typename MotionVelocity::Scalar Scalar; |
||
203 |
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options }; |
||
204 |
|||
205 |
typedef Symmetric3Tpl<Scalar,Options> Symmetric3; |
||
206 |
typedef typename Symmetric3::SkewSquare SkewSquare; |
||
207 |
using ::pinocchio::details::crossBigL; |
||
208 |
using ::pinocchio::details::addBigL; |
||
209 |
|||
210 |
30 |
OutputType & res = PINOCCHIO_EIGEN_CONST_CAST(OutputType, regressor); |
|
211 |
|||
212 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
30 |
res.template block<3,1>(MotionVelocity::LINEAR,0) = a.linear() + v.angular().cross(v.linear()); |
213 |
✓✗ | 30 |
const Eigen::Block<OutputType,3,1> & acc = res.template block<3,1>(MotionVelocity::LINEAR,0); |
214 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
30 |
res.template block<3,3>(MotionVelocity::LINEAR,1) = Symmetric3(SkewSquare(v.angular())).matrix(); |
215 |
✓✗✓✗ ✓✗ |
30 |
addSkew(a.angular(), res.template block<3,3>(MotionVelocity::LINEAR,1)); |
216 |
|||
217 |
✓✗✓✗ |
30 |
res.template block<3,6>(MotionVelocity::LINEAR,4).setZero(); |
218 |
|||
219 |
✓✗✓✗ |
30 |
res.template block<3,1>(MotionVelocity::ANGULAR,0).setZero(); |
220 |
✓✗✓✗ ✓✗ |
30 |
skew(-acc, res.template block<3,3>(MotionVelocity::ANGULAR,1)); |
221 |
// res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(), bigL(v.angular())); |
||
222 |
✓✗✓✗ ✓✗ |
30 |
crossBigL(v.angular(), res.template block<3,6>(MotionVelocity::ANGULAR,4)); |
223 |
✓✗✓✗ ✓✗ |
30 |
addBigL(a.angular(), res.template block<3,6>(MotionVelocity::ANGULAR,4)); |
224 |
30 |
} |
|
225 |
|||
226 |
template<typename MotionVelocity, typename MotionAcceleration> |
||
227 |
inline Eigen::Matrix<typename MotionVelocity::Scalar,6,10,PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options> |
||
228 |
1 |
bodyRegressor(const MotionDense<MotionVelocity> & v, |
|
229 |
const MotionDense<MotionAcceleration> & a) |
||
230 |
{ |
||
231 |
typedef typename MotionVelocity::Scalar Scalar; |
||
232 |
enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options }; |
||
233 |
typedef Eigen::Matrix<Scalar,6,10,Options> ReturnType; |
||
234 |
|||
235 |
1 |
ReturnType res; |
|
236 |
1 |
bodyRegressor(v,a,res); |
|
237 |
1 |
return res; |
|
238 |
} |
||
239 |
|||
240 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
241 |
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType & |
||
242 |
28 |
jointBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
243 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
244 |
JointIndex jointId) |
||
245 |
{ |
||
246 |
✓✗ | 28 |
assert(model.check(data) && "data is not consistent with model."); |
247 |
|||
248 |
PINOCCHIO_UNUSED_VARIABLE(model); |
||
249 |
|||
250 |
28 |
bodyRegressor(data.v[jointId], data.a_gf[jointId], data.bodyRegressor); |
|
251 |
28 |
return data.bodyRegressor; |
|
252 |
} |
||
253 |
|||
254 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
255 |
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType & |
||
256 |
1 |
frameBodyRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
257 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
258 |
FrameIndex frameId) |
||
259 |
{ |
||
260 |
✓✗ | 1 |
assert(model.check(data) && "data is not consistent with model."); |
261 |
|||
262 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
263 |
typedef typename Model::Frame Frame; |
||
264 |
typedef typename Model::JointIndex JointIndex; |
||
265 |
typedef typename Model::SE3 SE3; |
||
266 |
|||
267 |
1 |
const Frame & frame = model.frames[frameId]; |
|
268 |
1 |
const JointIndex & parent = frame.parent; |
|
269 |
1 |
const SE3 & placement = frame.placement; |
|
270 |
|||
271 |
✓✗✓✗ |
1 |
bodyRegressor(placement.actInv(data.v[parent]), placement.actInv(data.a_gf[parent]), data.bodyRegressor); |
272 |
1 |
return data.bodyRegressor; |
|
273 |
} |
||
274 |
|||
275 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
276 |
struct JointTorqueRegressorForwardStep |
||
277 |
: public fusion::JointUnaryVisitorBase< JointTorqueRegressorForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> > |
||
278 |
{ |
||
279 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
280 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
281 |
|||
282 |
typedef boost::fusion::vector<const Model &, |
||
283 |
Data &, |
||
284 |
const ConfigVectorType &, |
||
285 |
const TangentVectorType1 &, |
||
286 |
const TangentVectorType2 & |
||
287 |
> ArgsType; |
||
288 |
|||
289 |
template<typename JointModel> |
||
290 |
54 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
291 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
292 |
const Model & model, |
||
293 |
Data & data, |
||
294 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
295 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
296 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
297 |
{ |
||
298 |
typedef typename Model::JointIndex JointIndex; |
||
299 |
|||
300 |
54 |
const JointIndex i = jmodel.id(); |
|
301 |
54 |
const JointIndex parent = model.parents[i]; |
|
302 |
|||
303 |
54 |
jmodel.calc(jdata.derived(),q.derived(),v.derived()); |
|
304 |
|||
305 |
✓✗ | 54 |
data.liMi[i] = model.jointPlacements[i]*jdata.M(); |
306 |
|||
307 |
54 |
data.v[i] = jdata.v(); |
|
308 |
✓✓ | 54 |
if(parent>0) |
309 |
✓✗ | 52 |
data.v[i] += data.liMi[i].actInv(data.v[parent]); |
310 |
|||
311 |
✓✗✗✗ ✓✗ |
54 |
data.a_gf[i] = jdata.c() + (data.v[i] ^ jdata.v()); |
312 |
✓✗✓✗ ✓✗ |
54 |
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a); |
313 |
✓✗ | 54 |
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]); |
314 |
} |
||
315 |
|||
316 |
}; |
||
317 |
|||
318 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
319 |
struct JointTorqueRegressorBackwardStep |
||
320 |
: public fusion::JointUnaryVisitorBase< JointTorqueRegressorBackwardStep<Scalar,Options,JointCollectionTpl> > |
||
321 |
{ |
||
322 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
323 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
324 |
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::BodyRegressorType BodyRegressorType; |
||
325 |
|||
326 |
typedef boost::fusion::vector<const Model &, |
||
327 |
Data &, |
||
328 |
const JointIndex & |
||
329 |
> ArgsType; |
||
330 |
|||
331 |
template<typename JointModel> |
||
332 |
276 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
333 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
334 |
const Model & model, |
||
335 |
Data & data, |
||
336 |
const JointIndex & col_idx) |
||
337 |
{ |
||
338 |
typedef typename Model::JointIndex JointIndex; |
||
339 |
|||
340 |
276 |
const JointIndex i = jmodel.id(); |
|
341 |
276 |
const JointIndex parent = model.parents[i]; |
|
342 |
|||
343 |
552 |
data.jointTorqueRegressor.block(jmodel.idx_v(),10*(Eigen::DenseIndex(col_idx)-1), |
|
344 |
✓✓✗✓ ✓✗✓✓ ✗✓✓✗ ✓✗ |
276 |
jmodel.nv(),10) = jdata.S().transpose()*data.bodyRegressor; |
345 |
|||
346 |
✓✓ | 276 |
if(parent>0) |
347 |
222 |
forceSet::se3Action(data.liMi[i],data.bodyRegressor,data.bodyRegressor); |
|
348 |
} |
||
349 |
}; |
||
350 |
|||
351 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> |
||
352 |
inline typename DataTpl<Scalar,Options,JointCollectionTpl>::MatrixXs & |
||
353 |
1 |
computeJointTorqueRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
354 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
355 |
const Eigen::MatrixBase<ConfigVectorType> & q, |
||
356 |
const Eigen::MatrixBase<TangentVectorType1> & v, |
||
357 |
const Eigen::MatrixBase<TangentVectorType2> & a) |
||
358 |
{ |
||
359 |
✓✗✓✗ |
1 |
assert(model.check(data) && "data is not consistent with model."); |
360 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(q.size(), model.nq); |
361 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), model.nv); |
362 |
✓✗✗✓ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
1 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(a.size(), model.nv); |
363 |
|||
364 |
✓✗ | 1 |
data.v[0].setZero(); |
365 |
✓✗ | 1 |
data.a_gf[0] = -model.gravity; |
366 |
✓✗ | 1 |
data.jointTorqueRegressor.setZero(); |
367 |
|||
368 |
typedef JointTorqueRegressorForwardStep<Scalar,Options,JointCollectionTpl,ConfigVectorType,TangentVectorType1,TangentVectorType2> Pass1; |
||
369 |
✓✗ | 1 |
typename Pass1::ArgsType arg1(model,data,q.derived(),v.derived(),a.derived()); |
370 |
✓✓ | 28 |
for(JointIndex i=1; i<(JointIndex)model.njoints; ++i) |
371 |
{ |
||
372 |
✓✗✓✗ |
27 |
Pass1::run(model.joints[i],data.joints[i], |
373 |
arg1); |
||
374 |
} |
||
375 |
|||
376 |
typedef JointTorqueRegressorBackwardStep<Scalar,Options,JointCollectionTpl> Pass2; |
||
377 |
✓✓ | 28 |
for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) |
378 |
{ |
||
379 |
✓✗ | 27 |
jointBodyRegressor(model,data,i); |
380 |
|||
381 |
✓✗ | 27 |
typename Pass2::ArgsType arg2(model,data,i); |
382 |
✓✓ | 165 |
for(JointIndex j=i; j>0; j = model.parents[j]) |
383 |
{ |
||
384 |
✓✗✓✗ |
138 |
Pass2::run(model.joints[j],data.joints[j], |
385 |
arg2); |
||
386 |
} |
||
387 |
} |
||
388 |
|||
389 |
1 |
return data.jointTorqueRegressor; |
|
390 |
} |
||
391 |
|||
392 |
} // namespace pinocchio |
||
393 |
|||
394 |
#endif // ifndef __pinocchio_algorithm_regressor_hxx__ |
| Generated by: GCOVR (Version 4.2) |