GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/numdiff/contact.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 33 42 78.6%
Branches: 33 86 38.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/multibody/numdiff/contact.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 100 ContactModelNumDiffTpl<Scalar>::ContactModelNumDiffTpl(
16 const std::shared_ptr<Base>& model)
17 : Base(model->get_state(), model->get_type(), model->get_nc(),
18 model->get_nu()),
19 100 model_(model),
20
1/2
✓ Branch 10 taken 50 times.
✗ Branch 11 not taken.
 100 e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
21
22 template <typename Scalar>
23 100 void ContactModelNumDiffTpl<Scalar>::calc(
24 const std::shared_ptr<ContactDataAbstract>& data,
25 const Eigen::Ref<const VectorXs>& x) {
26 100 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
27
1/2
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
 100 model_->calc(d->data_0, x);
28
1/2
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
 100 d->a0 = d->data_0->a0;
29 }
30
31 template <typename Scalar>
32 100 void ContactModelNumDiffTpl<Scalar>::calcDiff(
33 const std::shared_ptr<ContactDataAbstract>& data,
34 const Eigen::Ref<const VectorXs>& x) {
35 100 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
36
37 100 const VectorXs& a0 = d->a0;
38
39 100 assertStableStateFD(x);
40
41 // Computing the d contact(x,u) / dx
42
6/12
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 50 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 50 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 50 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 50 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 50 times.
✗ Branch 22 not taken.
 100 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
43
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 d->x_norm = d->dx.norm();
44
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 d->dx.setZero();
45 100 d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
46
3/4
✓ Branch 2 taken 2070 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2020 times.
✓ Branch 5 taken 50 times.
 4140 for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
47
1/2
✓ Branch 3 taken 2020 times.
✗ Branch 4 not taken.
 4040 d->dx(ix) = d->xh_jac;
48
4/8
✓ Branch 2 taken 2020 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 2020 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 2020 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2020 times.
✗ Branch 15 not taken.
 4040 model_->get_state()->integrate(x, d->dx, d->xp);
49 // call the update function on the pinocchio data
50
2/2
✓ Branch 1 taken 2020 times.
✓ Branch 2 taken 2020 times.
 8080 for (size_t i = 0; i < reevals_.size(); ++i) {
51
4/8
✓ Branch 3 taken 2020 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2020 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2020 times.
✗ Branch 10 not taken.
✓ Branch 13 taken 2020 times.
✗ Branch 14 not taken.
 4040 reevals_[i](d->xp, VectorXs::Zero(model_->get_nu()));
52 }
53
2/4
✓ Branch 3 taken 2020 times.
✗ Branch 4 not taken.
✓ Branch 8 taken 2020 times.
✗ Branch 9 not taken.
 4040 model_->calc(d->data_x[ix], d->xp);
54
4/8
✓ Branch 5 taken 2020 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2020 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 2020 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2020 times.
✗ Branch 16 not taken.
 4040 d->da0_dx.col(ix) = (d->data_x[ix]->a0 - a0) / d->xh_jac;
55
1/2
✓ Branch 2 taken 2020 times.
✗ Branch 3 not taken.
 4040 d->dx(ix) = Scalar(0.);
56 }
57 }
58
59 template <typename Scalar>
60 ✗ void ContactModelNumDiffTpl<Scalar>::updateForce(
61 const std::shared_ptr<ContactDataAbstract>& data, const VectorXs& force) {
62 ✗ if (static_cast<std::size_t>(force.size()) != model_->get_nc()) {
63 ✗ throw_pretty("Invalid argument: "
64 << "lambda has wrong dimension (it should be "
65 << model_->get_nc() << ")");
66 }
67
68 ✗ std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
69
70 ✗ model_->updateForce(d->data_0, force);
71 }
72
73 template <typename Scalar>
74 std::shared_ptr<ContactDataAbstractTpl<Scalar> >
75 100 ContactModelNumDiffTpl<Scalar>::createData(
76 pinocchio::DataTpl<Scalar>* const data) {
77 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
78
1/2
✓ Branch 2 taken 50 times.
✗ Branch 3 not taken.
 100 data);
79 }
80
81 template <typename Scalar>
82 template <typename NewScalar>
83 ✗ ContactModelNumDiffTpl<NewScalar> ContactModelNumDiffTpl<Scalar>::cast() const {
84 typedef ContactModelNumDiffTpl<NewScalar> ReturnType;
85 ✗ ReturnType res(model_->template cast<NewScalar>());
86 ✗ return res;
87 }
88
89 template <typename Scalar>
90 const std::shared_ptr<ContactModelAbstractTpl<Scalar> >&
91 4240 ContactModelNumDiffTpl<Scalar>::get_model() const {
92 4240 return model_;
93 }
94
95 template <typename Scalar>
96 50 const Scalar ContactModelNumDiffTpl<Scalar>::get_disturbance() const {
97 50 return e_jac_;
98 }
99
100 template <typename Scalar>
101 void ContactModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
102 if (disturbance < Scalar(0.)) {
103 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
104 }
105 e_jac_ = disturbance;
106 }
107
108 template <typename Scalar>
109 50 void ContactModelNumDiffTpl<Scalar>::set_reevals(
110 const std::vector<ReevaluationFunction>& reevals) {
111 50 reevals_ = reevals;
112 50 }
113
114 template <typename Scalar>
115 100 void ContactModelNumDiffTpl<Scalar>::assertStableStateFD(
116 const Eigen::Ref<const VectorXs>& /*x*/) {
117 // do nothing in the general case
118 }
119
120 } // namespace crocoddyl
121