GCC Code Coverage Report

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