| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2022, LAAS-CNRS |
||
5 |
// Copyright note valid unless otherwise stated in individual files. |
||
6 |
// All rights reserved. |
||
7 |
/////////////////////////////////////////////////////////////////////////////// |
||
8 |
|||
9 |
#include <crocoddyl/core/utils/exception.hpp> |
||
10 |
#include <pinocchio/algorithm/frames-derivatives.hpp> |
||
11 |
#include <pinocchio/algorithm/frames.hpp> |
||
12 |
|||
13 |
#include "sobec/crocomplements/residual-fly-high.hpp" |
||
14 |
|||
15 |
namespace sobec { |
||
16 |
using namespace crocoddyl; |
||
17 |
template <typename Scalar> |
||
18 |
521 |
ResidualModelFlyHighTpl<Scalar>::ResidualModelFlyHighTpl( |
|
19 |
boost::shared_ptr<StateMultibody> state, |
||
20 |
const pinocchio::FrameIndex frame_id, const Scalar slope, |
||
21 |
const std::size_t nu) |
||
22 |
: Base(state, 2, nu, true, true, false), |
||
23 |
frame_id(frame_id), |
||
24 |
slope(slope), |
||
25 |
✓✗✓✗ |
521 |
pin_model_(*state->get_pinocchio()) {} |
26 |
|||
27 |
template <typename Scalar> |
||
28 |
ResidualModelFlyHighTpl<Scalar>::ResidualModelFlyHighTpl( |
||
29 |
boost::shared_ptr<StateMultibody> state, |
||
30 |
const pinocchio::FrameIndex frame_id, const Scalar slope) |
||
31 |
: Base(state, 2, true, true, false), |
||
32 |
frame_id(frame_id), |
||
33 |
slope(slope), |
||
34 |
pin_model_(*state->get_pinocchio()) {} |
||
35 |
|||
36 |
template <typename Scalar> |
||
37 |
1042 |
ResidualModelFlyHighTpl<Scalar>::~ResidualModelFlyHighTpl() {} |
|
38 |
|||
39 |
template <typename Scalar> |
||
40 |
14308 |
void ResidualModelFlyHighTpl<Scalar>::calc( |
|
41 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
42 |
const Eigen::Ref<const VectorXs>& /*x*/, |
||
43 |
const Eigen::Ref<const VectorXs>&) { |
||
44 |
// Compute the residual residual give the reference CoM velocity |
||
45 |
|||
46 |
14308 |
Data* d = static_cast<Data*>(data.get()); |
|
47 |
|||
48 |
14308 |
pinocchio::updateFramePlacement(pin_model_, *d->pinocchio, frame_id); |
|
49 |
✓✗✓✗ ✓✗ |
14308 |
data->r = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, frame_id, |
50 |
pinocchio::LOCAL_WORLD_ALIGNED) |
||
51 |
.linear() |
||
52 |
.head(2); |
||
53 |
14308 |
d->ez = exp(-d->pinocchio->oMf[frame_id].translation()[2] * slope); |
|
54 |
14308 |
data->r *= d->ez; |
|
55 |
14308 |
} |
|
56 |
|||
57 |
template <typename Scalar> |
||
58 |
7139 |
void ResidualModelFlyHighTpl<Scalar>::calcDiff( |
|
59 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
60 |
const Eigen::Ref<const VectorXs>& /*x*/, |
||
61 |
const Eigen::Ref<const VectorXs>&) { |
||
62 |
7139 |
Data* d = static_cast<Data*>(data.get()); |
|
63 |
7139 |
const std::size_t nv = state_->get_nv(); |
|
64 |
|||
65 |
/* Let' s do a little bit of maths ... |
||
66 |
* r = v/e with e=exp(z/2) |
||
67 |
* r' = v'/e - v/e z'/2 = v'/e - r/2 z' |
||
68 |
* |
||
69 |
* Wrong, we should consider l_v the local velocity, with o_v = oRl l_v |
||
70 |
* Then v=R l_v |
||
71 |
* v' = R l_v' + R' l_v = R l_v' - l_v x Jr |
||
72 |
* Then r' = v'/e - r/2 z' = R l_v'/e - l_v x Jr/e - r/2 z' |
||
73 |
*/ |
||
74 |
|||
75 |
7139 |
pinocchio::getFrameVelocityDerivatives(pin_model_, *d->pinocchio, frame_id, |
|
76 |
7139 |
pinocchio::LOCAL, d->l_dnu_dq, |
|
77 |
✓✗ | 7139 |
d->l_dnu_dv); |
78 |
✓✗✓✗ ✓✗ |
7139 |
const Vector3s& v = pinocchio::getFrameVelocity(pin_model_, *d->pinocchio, |
79 |
frame_id, pinocchio::LOCAL) |
||
80 |
.linear(); |
||
81 |
✓✗ | 7139 |
const Matrix3s& R = d->pinocchio->oMf[frame_id].rotation(); |
82 |
|||
83 |
// First compute LWA derivatives of the velocity |
||
84 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
7139 |
d->vxJ.noalias() = pinocchio::skew(-v) * d->l_dnu_dv.template bottomRows<3>(); |
85 |
✓✗✓✗ |
7139 |
d->vxJ += d->l_dnu_dq.template topRows<3>(); |
86 |
✓✗✓✗ |
7139 |
d->o_dv_dq = R * d->vxJ; |
87 |
✓✗✓✗ ✓✗ |
7139 |
d->o_dv_dv = R * d->l_dnu_dv.template topRows<3>(); |
88 |
|||
89 |
// First term with derivative of v |
||
90 |
✓✗✓✗ ✓✗ |
7139 |
data->Rx.leftCols(nv) = d->o_dv_dq.template topRows<2>(); |
91 |
✓✗✓✗ ✓✗ |
7139 |
data->Rx.rightCols(nv) = d->o_dv_dv.template topRows<2>(); |
92 |
✓✗ | 7139 |
data->Rx *= d->ez; |
93 |
|||
94 |
// Second term with derivative of z |
||
95 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
7139 |
data->Rx.leftCols(nv).row(0) -= data->r[0] * slope * d->o_dv_dv.row(2); |
96 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
7139 |
data->Rx.leftCols(nv).row(1) -= data->r[1] * slope * d->o_dv_dv.row(2); |
97 |
7139 |
} |
|
98 |
|||
99 |
template <typename Scalar> |
||
100 |
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
||
101 |
651 |
ResidualModelFlyHighTpl<Scalar>::createData(DataCollectorAbstract* const data) { |
|
102 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
||
103 |
✓✗ | 651 |
data); |
104 |
} |
||
105 |
|||
106 |
template <typename Scalar> |
||
107 |
const typename pinocchio::FrameIndex& |
||
108 |
ResidualModelFlyHighTpl<Scalar>::get_frame_id() const { |
||
109 |
return frame_id; |
||
110 |
} |
||
111 |
|||
112 |
template <typename Scalar> |
||
113 |
void ResidualModelFlyHighTpl<Scalar>::set_frame_id( |
||
114 |
const pinocchio::FrameIndex& fid) { |
||
115 |
frame_id = fid; |
||
116 |
} |
||
117 |
|||
118 |
} // namespace sobec |
| Generated by: GCOVR (Version 4.2) |