| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2019-2021 CNRS INRIA |
||
3 |
// |
||
4 |
|||
5 |
#ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__ |
||
6 |
#define __pinocchio_algorithm_center_of_mass_derivatives_hxx__ |
||
7 |
|||
8 |
#include "pinocchio/multibody/visitor.hpp" |
||
9 |
#include "pinocchio/algorithm/check.hpp" |
||
10 |
|||
11 |
namespace pinocchio |
||
12 |
{ |
||
13 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, |
||
14 |
typename Matrix3xOut> |
||
15 |
struct CoMVelocityDerivativesForwardStep |
||
16 |
: public fusion::JointUnaryVisitorBase< CoMVelocityDerivativesForwardStep<Scalar,Options,JointCollectionTpl,Matrix3xOut> > |
||
17 |
{ |
||
18 |
/* Assumes that both computeForwardKinematicsDerivatives and centerOfMass (or |
||
19 |
* equivalent methods like computeAllTerms) have been computed beforehand. */ |
||
20 |
|||
21 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
22 |
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data; |
||
23 |
|||
24 |
typedef boost::fusion::vector<const Model &, |
||
25 |
Data &, |
||
26 |
Matrix3xOut & |
||
27 |
> ArgsType; |
||
28 |
|||
29 |
template<typename JointModel> |
||
30 |
116 |
static void algo(const JointModelBase<JointModel> & jmodel, |
|
31 |
JointDataBase<typename JointModel::JointDataDerived> & jdata, |
||
32 |
const Model & model, |
||
33 |
Data & data, |
||
34 |
const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq) |
||
35 |
{ |
||
36 |
typedef typename Model::JointIndex JointIndex; |
||
37 |
typedef typename Data::Motion Motion; |
||
38 |
|||
39 |
✓✗ | 116 |
const JointIndex i = jmodel.id(); |
40 |
116 |
const JointIndex parent = model.parents[i]; |
|
41 |
|||
42 |
116 |
Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,vcom_partial_dq); |
|
43 |
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix3x>::Type |
||
44 |
✓✗ | 116 |
dvcom_dqi = jmodel.jointCols(dvcom_dq); |
45 |
|||
46 |
✓✓✓✗ ✓✗✓✗ ✓✗ |
116 |
Motion vpc = (parent>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero(); |
47 |
✓✗✓✗ |
116 |
vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ] |
48 |
|||
49 |
116 |
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6>::Type vxS = |
|
50 |
✓✗✓✗ |
116 |
SizeDepType<JointModel::NV>::middleCols(data.M6tmp,0,jmodel.nv()); |
51 |
✓✗✓✗ ✓✗ |
116 |
vxS = vpc.cross(jdata.S()); |
52 |
|||
53 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
232 |
dvcom_dqi.noalias() = (data.mass[i]/data.mass[0])*data.oMi[i].rotation() |
54 |
116 |
*( vxS.template middleRows<3>(Motion::LINEAR) - cross( data.com[i], vxS.template middleRows<3>(Motion::ANGULAR)) ); |
|
55 |
} |
||
56 |
}; |
||
57 |
|||
58 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, |
||
59 |
typename Matrix3xOut> |
||
60 |
2 |
inline void getCenterOfMassVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, |
|
61 |
DataTpl<Scalar,Options,JointCollectionTpl> & data, |
||
62 |
const Eigen::MatrixBase<Matrix3xOut> & vcom_partial_dq) |
||
63 |
{ |
||
64 |
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix3xOut,Data::Matrix3x); |
||
65 |
|||
66 |
✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗ |
2 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(vcom_partial_dq.cols(), model.nv); |
67 |
✓✗ | 2 |
assert(model.check(data) && "data is not consistent with model."); |
68 |
|||
69 |
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model; |
||
70 |
typedef typename Model::JointIndex JointIndex; |
||
71 |
|||
72 |
2 |
Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,vcom_partial_dq); |
|
73 |
|||
74 |
typedef CoMVelocityDerivativesForwardStep<Scalar,Options,JointCollectionTpl,Matrix3xOut> Pass1; |
||
75 |
✓✓ | 60 |
for(JointIndex i = 1; i < (JointIndex)model.njoints; i ++) |
76 |
{ |
||
77 |
✓✗ | 58 |
Pass1::run(model.joints[i],data.joints[i], |
78 |
typename Pass1::ArgsType(model,data,dvcom_dq)); |
||
79 |
} |
||
80 |
2 |
} |
|
81 |
|||
82 |
} // namespace pinocchio |
||
83 |
|||
84 |
#endif // ifndef __pinocchio_algorithm_center_of_mass_derivatives_hxx__ |
| Generated by: GCOVR (Version 4.2) |