GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-force.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 60 88 68.2%
Branches: 29 128 22.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2023, 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 #include "crocoddyl/multibody/residuals/contact-force.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 1674 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
16 boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
17 const Force& fref, const std::size_t nc, const std::size_t nu,
18 const bool fwddyn)
19 : Base(state, nc, nu, fwddyn ? true : false, fwddyn ? true : false, true),
20 1674 fwddyn_(fwddyn),
21 1674 update_jacobians_(true),
22 1674 id_(id),
23
6/8
✓ Branch 0 taken 624 times.
✓ Branch 1 taken 1050 times.
✓ Branch 2 taken 624 times.
✓ Branch 3 taken 1050 times.
✓ Branch 6 taken 1674 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 1674 times.
✗ Branch 11 not taken.
 1674 fref_(fref) {
24
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1674 times.
 1674 if (nc > 6) {
25 ✗ throw_pretty(
26 "Invalid argument in ResidualModelContactForce: nc should be less than "
27 "6");
28 }
29
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1674 times.
 1674 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
30 id) {
31 ✗ throw_pretty(
32 "Invalid argument: "
33 << "the frame index is wrong (it does not exist in the robot)");
34 }
35 1674 }
36
37 template <typename Scalar>
38 1 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
39 boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
40 const Force& fref, const std::size_t nc)
41 : Base(state, nc),
42 1 fwddyn_(true),
43 1 update_jacobians_(true),
44 1 id_(id),
45
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 fref_(fref) {
46
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (nc > 6) {
47 ✗ throw_pretty(
48 "Invalid argument in ResidualModelContactForce: nc should be less than "
49 "6");
50 }
51
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
52 id) {
53 ✗ throw_pretty(
54 "Invalid argument: "
55 << "the frame index is wrong (it does not exist in the robot)");
56 }
57 1 }
58
59 template <typename Scalar>
60 3354 ResidualModelContactForceTpl<Scalar>::~ResidualModelContactForceTpl() {}
61
62 template <typename Scalar>
63 71770 void ResidualModelContactForceTpl<Scalar>::calc(
64 const boost::shared_ptr<ResidualDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
66 71770 Data* d = static_cast<Data*>(data.get());
67
68 // We transform the force to the contact frame
69
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 40014 times.
✓ Branch 2 taken 31756 times.
✗ Branch 3 not taken.
 71770 switch (d->contact_type) {
70 ✗ case Contact1D:
71 ✗ data->r = ((d->contact->f - fref_).linear()).row(2);
72 ✗ break;
73 40014 case Contact3D:
74
2/4
✓ Branch 3 taken 40014 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 40014 times.
✗ Branch 8 not taken.
 40014 data->r = (d->contact->f - fref_).linear();
75 40014 break;
76 31756 case Contact6D:
77
2/4
✓ Branch 3 taken 31756 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 31756 times.
✗ Branch 8 not taken.
 31756 data->r = (d->contact->f - fref_).toVector();
78 31756 break;
79 ✗ default:
80 ✗ break;
81 }
82 71770 }
83
84 template <typename Scalar>
85 15197 void ResidualModelContactForceTpl<Scalar>::calc(
86 const boost::shared_ptr<ResidualDataAbstract>& data,
87 const Eigen::Ref<const VectorXs>&) {
88 15197 data->r.setZero();
89 15197 }
90
91 template <typename Scalar>
92 9412 void ResidualModelContactForceTpl<Scalar>::calcDiff(
93 const boost::shared_ptr<ResidualDataAbstract>& data,
94 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
95
3/4
✓ Branch 0 taken 4389 times.
✓ Branch 1 taken 5023 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4389 times.
 9412 if (fwddyn_ || update_jacobians_) {
96 5023 updateJacobians(data);
97 }
98 9412 }
99
100 template <typename Scalar>
101 384 void ResidualModelContactForceTpl<Scalar>::calcDiff(
102 const boost::shared_ptr<ResidualDataAbstract>& data,
103 const Eigen::Ref<const VectorXs>&) {
104 384 data->Rx.setZero();
105 384 }
106
107 template <typename Scalar>
108 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
109 161402 ResidualModelContactForceTpl<Scalar>::createData(
110 DataCollectorAbstract* const data) {
111
1/2
✓ Branch 1 taken 161402 times.
✗ Branch 2 not taken.
 322804 boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
112 161402 Eigen::aligned_allocator<Data>(), this, data);
113
2/2
✓ Branch 0 taken 114294 times.
✓ Branch 1 taken 47108 times.
 161402 if (!fwddyn_) {
114
1/2
✓ Branch 1 taken 114294 times.
✗ Branch 2 not taken.
 114294 updateJacobians(d);
115 }
116 161402 return d;
117 }
118
119 template <typename Scalar>
120 119317 void ResidualModelContactForceTpl<Scalar>::updateJacobians(
121 const boost::shared_ptr<ResidualDataAbstract>& data) {
122 119317 Data* d = static_cast<Data*>(data.get());
123
124 119317 const MatrixXs& df_dx = d->contact->df_dx;
125 119317 const MatrixXs& df_du = d->contact->df_du;
126
2/4
✗ Branch 0 not taken.
✓ Branch 1 taken 74887 times.
✓ Branch 2 taken 44430 times.
✗ Branch 3 not taken.
 119317 switch (d->contact_type) {
127 ✗ case Contact1D:
128 ✗ data->Rx = df_dx.template topRows<1>();
129 ✗ data->Ru = df_du.template topRows<1>();
130 ✗ break;
131 74887 case Contact3D:
132
1/2
✓ Branch 3 taken 74887 times.
✗ Branch 4 not taken.
 74887 data->Rx = df_dx.template topRows<3>();
133
1/2
✓ Branch 3 taken 74887 times.
✗ Branch 4 not taken.
 74887 data->Ru = df_du.template topRows<3>();
134 74887 break;
135 44430 case Contact6D:
136 44430 data->Rx = df_dx;
137 44430 data->Ru = df_du;
138 44430 break;
139 ✗ default:
140 ✗ break;
141 }
142 119317 update_jacobians_ = false;
143 119317 }
144
145 template <typename Scalar>
146 ✗ void ResidualModelContactForceTpl<Scalar>::print(std::ostream& os) const {
147 ✗ boost::shared_ptr<StateMultibody> s =
148 ✗ boost::static_pointer_cast<StateMultibody>(state_);
149 ✗ const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
150 "]");
151 os << "ResidualModelContactForce {frame="
152 ✗ << s->get_pinocchio()->frames[id_].name
153 ✗ << ", fref=" << fref_.toVector().head(nr_).transpose().format(fmt) << "}";
154 }
155
156 template <typename Scalar>
157 bool ResidualModelContactForceTpl<Scalar>::is_fwddyn() const {
158 return fwddyn_;
159 }
160
161 template <typename Scalar>
162 161402 pinocchio::FrameIndex ResidualModelContactForceTpl<Scalar>::get_id() const {
163 161402 return id_;
164 }
165
166 template <typename Scalar>
167 const pinocchio::ForceTpl<Scalar>&
168 ✗ ResidualModelContactForceTpl<Scalar>::get_reference() const {
169 ✗ return fref_;
170 }
171
172 template <typename Scalar>
173 ✗ void ResidualModelContactForceTpl<Scalar>::set_id(
174 const pinocchio::FrameIndex id) {
175 ✗ id_ = id;
176 }
177
178 template <typename Scalar>
179 ✗ void ResidualModelContactForceTpl<Scalar>::set_reference(
180 const Force& reference) {
181 ✗ fref_ = reference;
182 ✗ update_jacobians_ = true;
183 }
184
185 } // namespace crocoddyl
186