GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-3d.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 117 143 81.8%
Branches: 192 419 45.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 namespace crocoddyl {
11
12 template <typename Scalar>
13 1182 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
14 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
15 const Vector3s& xref, const pinocchio::ReferenceFrame type,
16 const std::size_t nu, const Vector2s& gains)
17
3/6
✓ Branch 2 taken 1182 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1182 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1182 times.
✗ Branch 10 not taken.
 1182 : Base(state, type, 3, nu), xref_(xref), gains_(gains) {
18 1182 id_ = id;
19 1182 }
20
21 template <typename Scalar>
22 5 ContactModel3DTpl<Scalar>::ContactModel3DTpl(
23 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
24 const Vector3s& xref, const pinocchio::ReferenceFrame type,
25 const Vector2s& gains)
26
3/6
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5 times.
✗ Branch 10 not taken.
 5 : Base(state, type, 3), xref_(xref), gains_(gains) {
27 5 id_ = id;
28 5 }
29
30 #pragma GCC diagnostic push // TODO: Remove once the deprecated FrameXX has
31 // been removed in a future release
32 #pragma GCC diagnostic ignored "-Wdeprecated-declarations"
33
34 template <typename Scalar>
35 ✗ ContactModel3DTpl<Scalar>::ContactModel3DTpl(
36 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
37 const Vector3s& xref, const std::size_t nu, const Vector2s& gains)
38 : Base(state, pinocchio::ReferenceFrame::LOCAL, 3, nu),
39 ✗ xref_(xref),
40 ✗ gains_(gains) {
41 ✗ id_ = id;
42 ✗ std::cerr << "Deprecated: Use constructor that passes the type of contact, "
43 "this assumes is pinocchio::LOCAL."
44 ✗ << std::endl;
45 }
46
47 template <typename Scalar>
48 ✗ ContactModel3DTpl<Scalar>::ContactModel3DTpl(
49 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
50 const Vector3s& xref, const Vector2s& gains)
51 : Base(state, pinocchio::ReferenceFrame::LOCAL, 3),
52 ✗ xref_(xref),
53 ✗ gains_(gains) {
54 ✗ id_ = id;
55 ✗ std::cerr << "Deprecated: Use constructor that passes the type of contact, "
56 "this assumes is pinocchio::LOCAL."
57 ✗ << std::endl;
58 }
59
60 #pragma GCC diagnostic pop
61
62 template <typename Scalar>
63 80635 void ContactModel3DTpl<Scalar>::calc(
64 const std::shared_ptr<ContactDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&) {
66 80635 Data* d = static_cast<Data*>(data.get());
67
2/4
✓ Branch 2 taken 80635 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 80635 times.
✗ Branch 7 not taken.
 80635 pinocchio::updateFramePlacement(*state_->get_pinocchio().get(), *d->pinocchio,
68 id_);
69
2/4
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
 80635 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
70 80635 id_, pinocchio::LOCAL, d->fJf);
71
3/6
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 80635 times.
✗ Branch 9 not taken.
 80635 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
72 80635 *d->pinocchio, id_);
73
1/2
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
 80635 d->a0_local =
74 pinocchio::getFrameClassicalAcceleration(
75
1/2
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
 80635 *state_->get_pinocchio().get(), *d->pinocchio, id_, pinocchio::LOCAL)
76
2/4
✓ Branch 2 taken 80635 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
 161270 .linear();
77
78
2/4
✓ Branch 2 taken 80635 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80635 times.
✗ Branch 6 not taken.
 80635 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
79
3/4
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 80571 times.
✓ Branch 4 taken 64 times.
 80635 if (gains_[0] != 0.) {
80
3/6
✓ Branch 2 taken 80571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 80571 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 80571 times.
✗ Branch 9 not taken.
 80571 d->dp = d->pinocchio->oMf[id_].translation() - xref_;
81
4/8
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80571 times.
✗ Branch 11 not taken.
 80571 d->dp_local.noalias() = oRf.transpose() * d->dp;
82
3/6
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
 80571 d->a0_local += gains_[0] * d->dp_local;
83 }
84
3/4
✓ Branch 1 taken 80635 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 80571 times.
✓ Branch 4 taken 64 times.
 80635 if (gains_[1] != 0.) {
85
4/8
✓ Branch 1 taken 80571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 80571 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 80571 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 80571 times.
✗ Branch 11 not taken.
 80571 d->a0_local += gains_[1] * d->v.linear();
86 }
87
2/3
✓ Branch 0 taken 46129 times.
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
 80635 switch (type_) {
88 46129 case pinocchio::ReferenceFrame::LOCAL:
89
2/4
✓ Branch 1 taken 46129 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 46129 times.
✗ Branch 5 not taken.
 46129 d->Jc = d->fJf.template topRows<3>();
90
1/2
✓ Branch 1 taken 46129 times.
✗ Branch 2 not taken.
 46129 d->a0 = d->a0_local;
91 46129 break;
92 34506 case pinocchio::ReferenceFrame::WORLD:
93 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
94
4/8
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34506 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 34506 times.
✗ Branch 11 not taken.
 34506 d->Jc.noalias() = oRf * d->fJf.template topRows<3>();
95
3/6
✓ Branch 1 taken 34506 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34506 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 34506 times.
✗ Branch 8 not taken.
 34506 d->a0.noalias() = oRf * d->a0_local;
96 34506 break;
97 }
98 80635 }
99
100 template <typename Scalar>
101 12665 void ContactModel3DTpl<Scalar>::calcDiff(
102 const std::shared_ptr<ContactDataAbstract>& data,
103 const Eigen::Ref<const VectorXs>&) {
104 12665 Data* d = static_cast<Data*>(data.get());
105 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
106 12665 const pinocchio::JointIndex joint =
107
1/2
✓ Branch 2 taken 12665 times.
✗ Branch 3 not taken.
 12665 state_->get_pinocchio()->frames[d->frame].parentJoint;
108 #else
109 const pinocchio::JointIndex joint =
110 state_->get_pinocchio()->frames[d->frame].parent;
111 #endif
112
1/2
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
 12665 pinocchio::getJointAccelerationDerivatives(
113
1/2
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
 12665 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
114 12665 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
115
1/2
✓ Branch 2 taken 12665 times.
✗ Branch 3 not taken.
 12665 const std::size_t nv = state_->get_nv();
116
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 pinocchio::skew(d->v.linear(), d->vv_skew);
117
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 pinocchio::skew(d->v.angular(), d->vw_skew);
118
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjdv_dq.noalias() = d->fXj * d->v_partial_dq;
119
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjda_dq.noalias() = d->fXj * d->a_partial_dq;
120
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->fXjda_dv.noalias() = d->fXj * d->a_partial_dv;
121
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv) = d->fXjda_dq.template topRows<3>();
122
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
123
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vw_skew * d->fXjdv_dq.template topRows<3>();
124
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() -=
125
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vv_skew * d->fXjdv_dq.template bottomRows<3>();
126
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv) = d->fXjda_dv.template topRows<3>();
127
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() +=
128
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vw_skew * d->fJf.template topRows<3>();
129
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() -=
130
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
 12665 d->vv_skew * d->fJf.template bottomRows<3>();
131
2/4
✓ Branch 2 taken 12665 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12665 times.
✗ Branch 6 not taken.
 12665 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
132
133
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12665 times.
✗ Branch 4 not taken.
 12665 if (gains_[0] != 0.) {
134
1/2
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
 12665 pinocchio::skew(d->dp_local, d->dp_skew);
135
4/8
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12665 times.
✗ Branch 11 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
136
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 gains_[0] * d->dp_skew * d->fJf.template bottomRows<3>();
137
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
138
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 25330 gains_[0] * d->fJf.template topRows<3>();
139 }
140
2/4
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 12665 times.
✗ Branch 4 not taken.
 12665 if (gains_[1] != 0.) {
141
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.leftCols(nv).noalias() +=
142
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 gains_[1] * d->fXjdv_dq.template topRows<3>();
143
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 12665 d->da0_local_dx.rightCols(nv).noalias() +=
144
3/6
✓ Branch 1 taken 12665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12665 times.
✗ Branch 8 not taken.
 25330 gains_[1] * d->fJf.template topRows<3>();
145 }
146
2/3
✓ Branch 0 taken 7591 times.
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
 12665 switch (type_) {
147 7591 case pinocchio::ReferenceFrame::LOCAL:
148
1/2
✓ Branch 1 taken 7591 times.
✗ Branch 2 not taken.
 7591 d->da0_dx = d->da0_local_dx;
149 7591 break;
150 5074 case pinocchio::ReferenceFrame::WORLD:
151 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
152 // Recalculate the constrained accelerations after imposing contact
153 // constraints. This is necessary for the forward-dynamics case.
154
1/2
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
 5074 d->a0_local = pinocchio::getFrameClassicalAcceleration(
155
1/2
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
 5074 *state_->get_pinocchio().get(), *d->pinocchio, id_,
156 pinocchio::LOCAL)
157
2/4
✓ Branch 2 taken 5074 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5074 times.
✗ Branch 6 not taken.
 10148 .linear();
158
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5074 times.
✗ Branch 4 not taken.
 5074 if (gains_[0] != 0.) {
159
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0_local += gains_[0] * d->dp_local;
160 }
161
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 5074 times.
✗ Branch 4 not taken.
 5074 if (gains_[1] != 0.) {
162
4/8
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5074 times.
✗ Branch 11 not taken.
 5074 d->a0_local += gains_[1] * d->v.linear();
163 }
164
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0.noalias() = oRf * d->a0_local;
165
166
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
 5074 pinocchio::skew(d->a0.template head<3>(), d->a0_skew);
167
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->a0_world_skew.noalias() = d->a0_skew * oRf;
168
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->da0_dx.noalias() = oRf * d->da0_local_dx;
169
3/6
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5074 times.
✗ Branch 8 not taken.
 5074 d->da0_dx.leftCols(nv).noalias() -=
170
2/4
✓ Branch 1 taken 5074 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5074 times.
✗ Branch 5 not taken.
 5074 d->a0_world_skew * d->fJf.template bottomRows<3>();
171 5074 break;
172 }
173 12665 }
174
175 template <typename Scalar>
176 73537 void ContactModel3DTpl<Scalar>::updateForce(
177 const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
178
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 73537 times.
 73537 if (force.size() != 3) {
179 ✗ throw_pretty(
180 "Invalid argument: " << "lambda has wrong dimension (it should be 3)");
181 }
182 73537 Data* d = static_cast<Data*>(data.get());
183
1/2
✓ Branch 3 taken 73537 times.
✗ Branch 4 not taken.
 73537 data->f.linear() = force;
184
1/2
✓ Branch 3 taken 73537 times.
✗ Branch 4 not taken.
 73537 data->f.angular().setZero();
185
2/3
✓ Branch 0 taken 42045 times.
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
 73537 switch (type_) {
186 42045 case pinocchio::ReferenceFrame::LOCAL:
187
2/4
✓ Branch 2 taken 42045 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 42045 times.
✗ Branch 7 not taken.
 42045 data->fext = d->jMf.act(data->f);
188
1/2
✓ Branch 2 taken 42045 times.
✗ Branch 3 not taken.
 42045 data->dtau_dq.setZero();
189 42045 break;
190 31492 case pinocchio::ReferenceFrame::WORLD:
191 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
192
2/4
✓ Branch 2 taken 31492 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31492 times.
✗ Branch 6 not taken.
 31492 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
193
5/10
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31492 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 31492 times.
✗ Branch 14 not taken.
 31492 d->f_local.linear().noalias() = oRf.transpose() * force;
194
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 d->f_local.angular().setZero();
195
2/4
✓ Branch 2 taken 31492 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31492 times.
✗ Branch 7 not taken.
 31492 data->fext = data->jMf.act(d->f_local);
196
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 pinocchio::skew(d->f_local.linear(), d->f_skew);
197
4/8
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31492 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 31492 times.
✗ Branch 11 not taken.
 31492 d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
198
4/8
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31492 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31492 times.
✗ Branch 12 not taken.
 31492 data->dtau_dq.noalias() =
199
2/4
✓ Branch 1 taken 31492 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31492 times.
✗ Branch 5 not taken.
 31492 -d->fJf.template topRows<3>().transpose() * d->fJf_df;
200 31492 break;
201 }
202 73537 }
203
204 template <typename Scalar>
205 std::shared_ptr<ContactDataAbstractTpl<Scalar> >
206 92586 ContactModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
207 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
208
1/2
✓ Branch 2 taken 92586 times.
✗ Branch 3 not taken.
 92586 data);
209 }
210
211 template <typename Scalar>
212 template <typename NewScalar>
213 ✗ ContactModel3DTpl<NewScalar> ContactModel3DTpl<Scalar>::cast() const {
214 typedef ContactModel3DTpl<NewScalar> ReturnType;
215 typedef StateMultibodyTpl<NewScalar> StateType;
216 ✗ ReturnType ret(
217 ✗ std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
218 ✗ xref_.template cast<NewScalar>(), type_, nu_,
219 ✗ gains_.template cast<NewScalar>());
220 ✗ return ret;
221 }
222
223 template <typename Scalar>
224 15 void ContactModel3DTpl<Scalar>::print(std::ostream& os) const {
225 15 os << "ContactModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
226 30 << ", type=" << type_ << "}";
227 15 }
228
229 template <typename Scalar>
230 const typename MathBaseTpl<Scalar>::Vector3s&
231 ✗ ContactModel3DTpl<Scalar>::get_reference() const {
232 ✗ return xref_;
233 }
234
235 template <typename Scalar>
236 const typename MathBaseTpl<Scalar>::Vector2s&
237 ✗ ContactModel3DTpl<Scalar>::get_gains() const {
238 ✗ return gains_;
239 }
240
241 template <typename Scalar>
242 ✗ void ContactModel3DTpl<Scalar>::set_reference(const Vector3s& reference) {
243 ✗ xref_ = reference;
244 }
245
246 } // namespace crocoddyl
247