GCC Code Coverage Report

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