GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/contact-jacobian.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 74 82 90.2%
Branches: 93 273 34.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2021 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_contact_jacobian_hxx__
6 #define __pinocchio_algorithm_contact_jacobian_hxx__
7
8 #include "pinocchio/multibody/model.hpp"
9 #include "pinocchio/multibody/data.hpp"
10
11 namespace pinocchio
12 {
13
14 template<
15 typename Scalar,
16 int Options,
17 template<typename, int> class JointCollectionTpl,
18 typename Matrix6or3Like>
19 31 void getConstraintJacobian(
20 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
21 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
22 const RigidConstraintModelTpl<Scalar, Options> & constraint_model,
23 RigidConstraintDataTpl<Scalar, Options> & constraint_data,
24 const Eigen::MatrixBase<Matrix6or3Like> & J_)
25 {
26
1/2
✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
 31 assert(model.check(data) && "data is not consistent with model.");
27
1/24
✗ Branch 2 not taken.
✓ Branch 3 taken 31 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
 31 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_model.size());
28
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 31 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 31 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv);
29
30 typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
31 typedef typename Model::Motion Motion;
32 typedef typename Model::SE3 SE3;
33 typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
34
35 31 Matrix6or3Like & J = J_.const_cast_derived();
36
37 typedef RigidConstraintModelTpl<Scalar, Options> ConstraintModel;
38 31 const typename ConstraintModel::BooleanVector & colwise_joint1_sparsity =
39 constraint_model.colwise_joint1_sparsity;
40 31 const typename ConstraintModel::BooleanVector & colwise_joint2_sparsity =
41 constraint_model.colwise_joint2_sparsity;
42 31 const typename ConstraintModel::IndexVector & colwise_span_indexes =
43 constraint_model.colwise_span_indexes;
44
45 31 SE3 & oMc1 = constraint_data.oMc1;
46
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 oMc1 = data.oMi[constraint_model.joint1_id] * constraint_model.joint1_placement;
47 31 SE3 & oMc2 = constraint_data.oMc2;
48
1/2
✓ Branch 3 taken 31 times.
✗ Branch 4 not taken.
 31 oMc2 = data.oMi[constraint_model.joint2_id] * constraint_model.joint2_placement;
49 31 SE3 & c1Mc2 = constraint_data.c1Mc2;
50
1/2
✓ Branch 2 taken 31 times.
✗ Branch 3 not taken.
 31 c1Mc2 = oMc1.actInv(oMc2);
51
52
2/2
✓ Branch 1 taken 390 times.
✓ Branch 2 taken 31 times.
 421 for (size_t k = 0; k < colwise_span_indexes.size(); ++k)
53 {
54 390 const Eigen::DenseIndex col_id = colwise_span_indexes[k];
55
56
2/4
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 390 times.
✗ Branch 5 not taken.
 390 const int sign = colwise_joint1_sparsity[col_id] != colwise_joint2_sparsity[col_id]
57
5/6
✓ Branch 0 taken 372 times.
✓ Branch 1 taken 18 times.
✓ Branch 3 taken 372 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 342 times.
✓ Branch 6 taken 30 times.
 390 ? colwise_joint1_sparsity[col_id] ? +1 : -1
58 : 0; // specific case for CONTACT_3D
59
60 typedef typename Data::Matrix6x::ConstColXpr ColXprIn;
61
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 const ColXprIn Jcol_in = data.J.col(col_id);
62
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 const MotionRef<const ColXprIn> Jcol_motion_in(Jcol_in);
63
64 typedef typename Matrix6or3Like::ColXpr ColXprOut;
65
1/2
✓ Branch 1 taken 390 times.
✗ Branch 2 not taken.
 390 ColXprOut Jcol_out = J.col(col_id);
66
67
2/3
✓ Branch 0 taken 318 times.
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 390 switch (constraint_model.type)
68 {
69 318 case CONTACT_3D: {
70
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 276 times.
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 318 switch (constraint_model.reference_frame)
71 {
72 ✗ case WORLD: {
73 ✗ Jcol_out.noalias() = Jcol_motion_in.linear() * Scalar(sign);
74 ✗ break;
75 }
76 276 case LOCAL: {
77
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 264 times.
 276 if (sign == 0)
78 {
79
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local1(oMc1.actInv(Jcol_motion_in)); // TODO: simplify computations
80
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local2(oMc2.actInv(Jcol_motion_in)); // TODO: simplify computations
81
7/14
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 12 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 12 times.
✗ Branch 20 not taken.
 12 Jcol_out.noalias() = Jcol_local1.linear() - c1Mc2.rotation() * Jcol_local2.linear();
82 }
83
2/2
✓ Branch 0 taken 252 times.
✓ Branch 1 taken 12 times.
 264 else if (sign == 1)
84 {
85
1/2
✓ Branch 1 taken 252 times.
✗ Branch 2 not taken.
 252 const Motion Jcol_local(oMc1.actInv(Jcol_motion_in));
86
3/6
✓ Branch 1 taken 252 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 252 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 252 times.
✗ Branch 8 not taken.
 252 Jcol_out.noalias() = Jcol_local.linear();
87 }
88 else // sign == -1
89 {
90
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Motion Jcol_local(oMc2.actInv(Jcol_motion_in)); // TODO: simplify computations
91
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 Jcol_out.noalias() =
92
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 24 -c1Mc2.rotation() * Jcol_local.linear(); // TODO: simplify computations
93 }
94 276 break;
95 }
96 42 case LOCAL_WORLD_ALIGNED: {
97
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 36 times.
 42 if (sign == 0)
98 {
99
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 Jcol_out.noalias() =
100
4/8
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
 12 (oMc2.translation() - oMc1.translation()).cross(Jcol_motion_in.angular());
101 }
102 else
103 {
104
2/2
✓ Branch 0 taken 30 times.
✓ Branch 1 taken 6 times.
 36 if (sign == 1)
105
4/8
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 30 times.
✗ Branch 11 not taken.
 60 Jcol_out.noalias() =
106
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 90 Jcol_motion_in.linear() - oMc1.translation().cross(Jcol_motion_in.angular());
107 else
108
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
 12 Jcol_out.noalias() =
109
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 18 -Jcol_motion_in.linear() + oMc2.translation().cross(Jcol_motion_in.angular());
110 }
111 42 break;
112 }
113 }
114 318 break;
115 }
116 72 case CONTACT_6D: {
117
1/2
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
 72 MotionRef<ColXprOut> Jcol_motion_out(Jcol_out);
118
2/4
✓ Branch 1 taken 72 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 72 times.
✗ Branch 4 not taken.
 72 assert(check_expression_if_real<Scalar>(sign != 0) && "sign should be equal to +1 or -1.");
119
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 36 times.
✓ Branch 2 taken 36 times.
✗ Branch 3 not taken.
 72 switch (constraint_model.reference_frame)
120 {
121 ✗ case WORLD: {
122 ✗ Jcol_motion_out = Jcol_motion_in;
123 ✗ break;
124 }
125 36 case LOCAL: {
126
3/8
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 36 Jcol_motion_out = Scalar(sign) * oMc1.actInv(Jcol_motion_in);
127 36 break;
128 }
129 36 case LOCAL_WORLD_ALIGNED: {
130
1/2
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
 36 Motion Jcol_local_world_aligned(Jcol_motion_in);
131
3/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 36 times.
✗ Branch 8 not taken.
 36 Jcol_local_world_aligned.linear() -=
132
2/4
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 oMc1.translation().cross(Jcol_local_world_aligned.angular());
133
2/6
✓ Branch 1 taken 36 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 36 Jcol_motion_out = Scalar(sign) * Jcol_local_world_aligned;
134 36 break;
135 }
136 }
137 72 break;
138 }
139
140 ✗ default:
141 ✗ break;
142 }
143 }
144 31 }
145
146 template<
147 typename Scalar,
148 int Options,
149 template<typename, int> class JointCollectionTpl,
150 typename DynamicMatrixLike,
151 class ConstraintModelAllocator,
152 class ConstraintDataAllocator>
153 8 void getConstraintsJacobian(
154 const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
155 const DataTpl<Scalar, Options, JointCollectionTpl> & data,
156 const std::vector<RigidConstraintModelTpl<Scalar, Options>, ConstraintModelAllocator> &
157 constraint_models,
158 std::vector<RigidConstraintDataTpl<Scalar, Options>, ConstraintDataAllocator> &
159 constraint_datas,
160 const Eigen::MatrixBase<DynamicMatrixLike> & J_)
161 {
162 typedef RigidConstraintModelTpl<Scalar, Options> ContraintModel;
163 typedef RigidConstraintDataTpl<Scalar, Options> ContraintData;
164
165 8 const Eigen::DenseIndex constraint_size = getTotalConstraintSize(constraint_models);
166
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.rows(), constraint_size);
167
1/24
✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
 8 PINOCCHIO_CHECK_ARGUMENT_SIZE(J_.cols(), model.nv);
168
169 8 DynamicMatrixLike & J = J_.const_cast_derived();
170 8 Eigen::DenseIndex row_id = 0;
171
2/2
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 8 times.
 24 for (size_t k = 0; k < constraint_models.size(); ++k)
172 {
173 16 const ContraintModel & cmodel = constraint_models[k];
174 16 ContraintData & cdata = constraint_datas[k];
175
176
1/2
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
 16 getConstraintJacobian(model, data, cmodel, cdata, J.middleRows(row_id, cmodel.size()));
177
178 16 row_id += cmodel.size();
179 }
180 8 }
181
182 } // namespace pinocchio
183
184 #endif // ifndef __pinocchio_algorithm_contact_jacobian_hxx__
185