GCC Code Coverage Report

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