| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 |
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 |
100 |
boost::shared_ptr<Data> d = boost::static_pointer_cast<Data>(data); |
|
31 |
✓✗ | 50 |
model_->calc(d->data_0, x); |
32 |
✓✗ | 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 |
100 |
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 |
✓✗✓✗ ✓✗✓✗ |
50 |
model_->get_state()->diff(model_->get_state()->zero(), x, d->dx); |
47 |
✓✗ | 50 |
d->x_norm = d->dx.norm(); |
48 |
✓✗ | 50 |
d->dx.setZero(); |
49 |
50 |
d->xh_jac = e_jac_ * std::max(1., d->x_norm); |
|
50 |
✓✓ | 2070 |
for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) { |
51 |
✓✗ | 2020 |
d->dx(ix) = d->xh_jac; |
52 |
✓✗✓✗ ✓✗ |
2020 |
model_->get_state()->integrate(x, d->dx, d->xp); |
53 |
// call the update function on the pinocchio data |
||
54 |
✓✓ | 4040 |
for (size_t i = 0; i < reevals_.size(); ++i) { |
55 |
✓✗✓✗ ✓✗ |
2020 |
reevals_[i](d->xp, VectorXs::Zero(model_->get_nu())); |
56 |
} |
||
57 |
✓✗✓✗ |
2020 |
model_->calc(d->data_x[ix], d->xp); |
58 |
✓✗✓✗ ✓✗✓✗ |
2020 |
d->da0_dx.col(ix) = (d->data_x[ix]->a0 - a0) / d->xh_jac; |
59 |
✓✗ | 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 |
✓✗ | 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: " |
||
100 |
<< "Disturbance constant is positive"); |
||
101 |
} |
||
102 |
e_jac_ = disturbance; |
||
103 |
} |
||
104 |
|||
105 |
template <typename Scalar> |
||
106 |
50 |
void ContactModelNumDiffTpl<Scalar>::set_reevals( |
|
107 |
const std::vector<ReevaluationFunction>& reevals) { |
||
108 |
50 |
reevals_ = reevals; |
|
109 |
50 |
} |
|
110 |
|||
111 |
template <typename Scalar> |
||
112 |
50 |
void ContactModelNumDiffTpl<Scalar>::assertStableStateFD( |
|
113 |
const Eigen::Ref<const VectorXs>& /*x*/) { |
||
114 |
// do nothing in the general case |
||
115 |
50 |
} |
|
116 |
|||
117 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |