GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/contacts/contact-6d.hxx
Date: 2025-01-16 08:47:40
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 589 ContactModel6DTpl<Scalar>::ContactModel6DTpl(
21 boost::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 589 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 589 times.
✗ Branch 8 not taken.
 589 : Base(state, type, 6, nu), pref_(pref), gains_(gains) {
25 589 id_ = id;
26 589 }
27
28 template <typename Scalar>
29 5 ContactModel6DTpl<Scalar>::ContactModel6DTpl(
30 boost::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 boost::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 boost::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 1192 ContactModel6DTpl<Scalar>::~ContactModel6DTpl() {}
71
72 template <typename Scalar>
73 58207 void ContactModel6DTpl<Scalar>::calc(
74 const boost::shared_ptr<ContactDataAbstract>& data,
75 const Eigen::Ref<const VectorXs>&) {
76 58207 Data* d = static_cast<Data*>(data.get());
77 58207 pinocchio::updateFramePlacement<Scalar>(*state_->get_pinocchio().get(),
78 58207 *d->pinocchio, id_);
79 58207 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
80 58207 id_, pinocchio::LOCAL, d->fJf);
81
1/2
✓ Branch 4 taken 58207 times.
✗ Branch 5 not taken.
 58207 d->a0_local = pinocchio::getFrameAcceleration(*state_->get_pinocchio().get(),
82 58207 *d->pinocchio, id_);
83
84
2/2
✓ Branch 1 taken 58183 times.
✓ Branch 2 taken 24 times.
 58207 if (gains_[0] != 0.) {
85
1/2
✓ Branch 3 taken 58183 times.
✗ Branch 4 not taken.
 58183 d->rMf = pref_.actInv(d->pinocchio->oMf[id_]);
86
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);
87 }
88
2/2
✓ Branch 1 taken 58183 times.
✓ Branch 2 taken 24 times.
 58207 if (gains_[1] != 0.) {
89
1/2
✓ Branch 4 taken 58183 times.
✗ Branch 5 not taken.
 58183 d->v = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
90 58183 *d->pinocchio, id_);
91
1/2
✓ Branch 3 taken 58183 times.
✗ Branch 4 not taken.
 58183 d->a0_local += gains_[1] * d->v;
92 }
93
2/3
✓ Branch 0 taken 30009 times.
✓ Branch 1 taken 28198 times.
✗ Branch 2 not taken.
 58207 switch (type_) {
94 30009 case pinocchio::ReferenceFrame::LOCAL:
95 30009 data->Jc = d->fJf;
96 30009 data->a0 = d->a0_local.toVector();
97 30009 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 58207 }
106
107 template <typename Scalar>
108 5151 void ContactModel6DTpl<Scalar>::calcDiff(
109 const boost::shared_ptr<ContactDataAbstract>& data,
110 const Eigen::Ref<const VectorXs>&) {
111 5151 Data* d = static_cast<Data*>(data.get());
112 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
113 5151 const pinocchio::JointIndex joint =
114 5151 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 5151 pinocchio::getJointAccelerationDerivatives(
120 5151 *state_->get_pinocchio().get(), *d->pinocchio, joint, pinocchio::LOCAL,
121 5151 d->v_partial_dq, d->a_partial_dq, d->a_partial_dv, d->a_partial_da);
122 5151 const std::size_t nv = state_->get_nv();
123
3/6
✓ Branch 2 taken 5151 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5151 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5151 times.
✗ Branch 9 not taken.
 5151 d->da0_local_dx.leftCols(nv).noalias() = d->fXj * d->a_partial_dq;
124
3/6
✓ Branch 2 taken 5151 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5151 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5151 times.
✗ Branch 9 not taken.
 5151 d->da0_local_dx.rightCols(nv).noalias() = d->fXj * d->a_partial_dv;
125
126
1/2
✓ Branch 1 taken 5151 times.
✗ Branch 2 not taken.
 5151 if (gains_[0] != 0.) {
127 5151 pinocchio::Jlog6(d->rMf, d->rMf_Jlog6);
128
4/8
✓ Branch 3 taken 5151 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5151 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5151 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5151 times.
✗ Branch 13 not taken.
 5151 d->da0_local_dx.leftCols(nv).noalias() += gains_[0] * d->rMf_Jlog6 * d->fJf;
129 }
130
1/2
✓ Branch 1 taken 5151 times.
✗ Branch 2 not taken.
 5151 if (gains_[1] != 0.) {
131
4/8
✓ Branch 1 taken 5151 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5151 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5151 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5151 times.
✗ Branch 11 not taken.
 5151 d->da0_local_dx.leftCols(nv).noalias() +=
132 5151 gains_[1] * d->fXj * d->v_partial_dq;
133
3/6
✓ Branch 3 taken 5151 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5151 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 5151 times.
✗ Branch 10 not taken.
 5151 d->da0_local_dx.rightCols(nv).noalias() += gains_[1] * d->fJf;
134 }
135
2/3
✓ Branch 0 taken 2591 times.
✓ Branch 1 taken 2560 times.
✗ Branch 2 not taken.
 5151 switch (type_) {
136 2591 case pinocchio::ReferenceFrame::LOCAL:
137
1/2
✓ Branch 1 taken 2591 times.
✗ Branch 2 not taken.
 2591 d->da0_dx = d->da0_local_dx;
138 2591 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 5151 }
166
167 template <typename Scalar>
168 54956 void ContactModel6DTpl<Scalar>::updateForce(
169 const boost::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 boost::shared_ptr<ContactDataAbstractTpl<Scalar> >
198 63026 ContactModel6DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
199 ✗ return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
200
1/2
✓ Branch 2 taken 63026 times.
✗ Branch 3 not taken.
 63026 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