GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-force.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 87 0.0%
Branches: 0 138 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, 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 ✗ ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
16 std::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 ✗ fwddyn_(fwddyn),
21 ✗ update_jacobians_(true),
22 ✗ id_(id),
23 ✗ fref_(fref) {
24 ✗ if (nc > 6) {
25 ✗ throw_pretty(
26 "Invalid argument in ResidualModelContactForce: nc should be less than "
27 "6");
28 }
29 ✗ 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 }
36
37 template <typename Scalar>
38 ✗ ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
39 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
40 const Force& fref, const std::size_t nc)
41 : Base(state, nc),
42 ✗ fwddyn_(true),
43 ✗ update_jacobians_(true),
44 ✗ id_(id),
45 ✗ fref_(fref) {
46 ✗ if (nc > 6) {
47 ✗ throw_pretty(
48 "Invalid argument in ResidualModelContactForce: nc should be less than "
49 "6");
50 }
51 ✗ 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 }
58
59 template <typename Scalar>
60 ✗ void ResidualModelContactForceTpl<Scalar>::calc(
61 const std::shared_ptr<ResidualDataAbstract>& data,
62 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
63 ✗ Data* d = static_cast<Data*>(data.get());
64
65 // We transform the force to the contact frame
66 ✗ switch (d->contact_type) {
67 ✗ case Contact1D:
68 ✗ data->r = ((d->contact->f - fref_).linear()).row(2);
69 ✗ break;
70 ✗ case Contact3D:
71 ✗ data->r = (d->contact->f - fref_).linear();
72 ✗ break;
73 ✗ case Contact6D:
74 ✗ data->r = (d->contact->f - fref_).toVector();
75 ✗ break;
76 ✗ default:
77 ✗ break;
78 }
79 }
80
81 template <typename Scalar>
82 ✗ void ResidualModelContactForceTpl<Scalar>::calc(
83 const std::shared_ptr<ResidualDataAbstract>& data,
84 const Eigen::Ref<const VectorXs>&) {
85 ✗ data->r.setZero();
86 }
87
88 template <typename Scalar>
89 ✗ void ResidualModelContactForceTpl<Scalar>::calcDiff(
90 const std::shared_ptr<ResidualDataAbstract>& data,
91 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
92 ✗ if (fwddyn_ || update_jacobians_) {
93 ✗ updateJacobians(data);
94 }
95 }
96
97 template <typename Scalar>
98 ✗ void ResidualModelContactForceTpl<Scalar>::calcDiff(
99 const std::shared_ptr<ResidualDataAbstract>& data,
100 const Eigen::Ref<const VectorXs>&) {
101 ✗ data->Rx.setZero();
102 }
103
104 template <typename Scalar>
105 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
106 ✗ ResidualModelContactForceTpl<Scalar>::createData(
107 DataCollectorAbstract* const data) {
108 ✗ std::shared_ptr<ResidualDataAbstract> d =
109 ✗ std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, data);
110 ✗ if (!fwddyn_) {
111 ✗ updateJacobians(d);
112 }
113 ✗ return d;
114 }
115
116 template <typename Scalar>
117 ✗ void ResidualModelContactForceTpl<Scalar>::updateJacobians(
118 const std::shared_ptr<ResidualDataAbstract>& data) {
119 ✗ Data* d = static_cast<Data*>(data.get());
120
121 ✗ const MatrixXs& df_dx = d->contact->df_dx;
122 ✗ const MatrixXs& df_du = d->contact->df_du;
123 ✗ switch (d->contact_type) {
124 ✗ case Contact1D:
125 ✗ data->Rx = df_dx.template topRows<1>();
126 ✗ data->Ru = df_du.template topRows<1>();
127 ✗ break;
128 ✗ case Contact3D:
129 ✗ data->Rx = df_dx.template topRows<3>();
130 ✗ data->Ru = df_du.template topRows<3>();
131 ✗ break;
132 ✗ case Contact6D:
133 ✗ data->Rx = df_dx;
134 ✗ data->Ru = df_du;
135 ✗ break;
136 ✗ default:
137 ✗ break;
138 }
139 ✗ update_jacobians_ = false;
140 }
141
142 template <typename Scalar>
143 template <typename NewScalar>
144 ResidualModelContactForceTpl<NewScalar>
145 ✗ ResidualModelContactForceTpl<Scalar>::cast() const {
146 typedef ResidualModelContactForceTpl<NewScalar> ReturnType;
147 typedef StateMultibodyTpl<NewScalar> StateType;
148 ✗ ReturnType ret(
149 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
150 ✗ id_, fref_.template cast<NewScalar>(), nr_, nu_, fwddyn_);
151 ✗ return ret;
152 }
153
154 template <typename Scalar>
155 ✗ void ResidualModelContactForceTpl<Scalar>::print(std::ostream& os) const {
156 ✗ std::shared_ptr<StateMultibody> s =
157 ✗ std::static_pointer_cast<StateMultibody>(state_);
158 ✗ const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
159 "]");
160 os << "ResidualModelContactForce {frame="
161 ✗ << s->get_pinocchio()->frames[id_].name
162 ✗ << ", fref=" << fref_.toVector().head(nr_).transpose().format(fmt) << "}";
163 }
164
165 template <typename Scalar>
166 ✗ bool ResidualModelContactForceTpl<Scalar>::is_fwddyn() const {
167 ✗ return fwddyn_;
168 }
169
170 template <typename Scalar>
171 ✗ pinocchio::FrameIndex ResidualModelContactForceTpl<Scalar>::get_id() const {
172 ✗ return id_;
173 }
174
175 template <typename Scalar>
176 const pinocchio::ForceTpl<Scalar>&
177 ✗ ResidualModelContactForceTpl<Scalar>::get_reference() const {
178 ✗ return fref_;
179 }
180
181 template <typename Scalar>
182 ✗ void ResidualModelContactForceTpl<Scalar>::set_id(
183 const pinocchio::FrameIndex id) {
184 ✗ id_ = id;
185 }
186
187 template <typename Scalar>
188 ✗ void ResidualModelContactForceTpl<Scalar>::set_reference(
189 const Force& reference) {
190 ✗ fref_ = reference;
191 ✗ update_jacobians_ = true;
192 }
193
194 } // namespace crocoddyl
195