GCC Code Coverage Report

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