GCC Code Coverage Report

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