| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
/////////////////////////////////////////////////////////////////////////////// |
||
2 |
// BSD 3-Clause License |
||
3 |
// |
||
4 |
// Copyright (C) 2021-2022, LAAS-CNRS, University of Edinburgh |
||
5 |
// Copyright note valid unless otherwise stated in individual files. |
||
6 |
// All rights reserved. |
||
7 |
/////////////////////////////////////////////////////////////////////////////// |
||
8 |
|||
9 |
#include <pinocchio/algorithm/frames.hpp> |
||
10 |
|||
11 |
#include "crocoddyl/multibody/residuals/frame-translation.hpp" |
||
12 |
|||
13 |
namespace crocoddyl { |
||
14 |
|||
15 |
template <typename Scalar> |
||
16 |
340 |
ResidualModelFrameTranslationTpl<Scalar>::ResidualModelFrameTranslationTpl( |
|
17 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
18 |
const Vector3s& xref, const std::size_t nu) |
||
19 |
: Base(state, 3, nu, true, false, false), |
||
20 |
id_(id), |
||
21 |
xref_(xref), |
||
22 |
✓✗✓✗ |
340 |
pin_model_(state->get_pinocchio()) { |
23 |
✗✓ | 340 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
24 |
id) { |
||
25 |
throw_pretty( |
||
26 |
"Invalid argument: " |
||
27 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
28 |
} |
||
29 |
340 |
} |
|
30 |
|||
31 |
template <typename Scalar> |
||
32 |
3 |
ResidualModelFrameTranslationTpl<Scalar>::ResidualModelFrameTranslationTpl( |
|
33 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
34 |
const Vector3s& xref) |
||
35 |
: Base(state, 3, true, false, false), |
||
36 |
id_(id), |
||
37 |
xref_(xref), |
||
38 |
✓✗✓✗ |
3 |
pin_model_(state->get_pinocchio()) { |
39 |
✗✓ | 3 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
40 |
id) { |
||
41 |
throw_pretty( |
||
42 |
"Invalid argument: " |
||
43 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
44 |
} |
||
45 |
3 |
} |
|
46 |
|||
47 |
template <typename Scalar> |
||
48 |
690 |
ResidualModelFrameTranslationTpl<Scalar>::~ResidualModelFrameTranslationTpl() {} |
|
49 |
|||
50 |
template <typename Scalar> |
||
51 |
8189 |
void ResidualModelFrameTranslationTpl<Scalar>::calc( |
|
52 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
53 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
54 |
// Compute the frame translation w.r.t. the reference frame |
||
55 |
8189 |
Data* d = static_cast<Data*>(data.get()); |
|
56 |
8189 |
pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_); |
|
57 |
✓✗ | 8189 |
data->r = d->pinocchio->oMf[id_].translation() - xref_; |
58 |
8189 |
} |
|
59 |
|||
60 |
template <typename Scalar> |
||
61 |
264 |
void ResidualModelFrameTranslationTpl<Scalar>::calcDiff( |
|
62 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
63 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
64 |
264 |
Data* d = static_cast<Data*>(data.get()); |
|
65 |
|||
66 |
// Compute the derivatives of the frame translation |
||
67 |
264 |
const std::size_t nv = state_->get_nv(); |
|
68 |
264 |
pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_, |
|
69 |
264 |
pinocchio::LOCAL, d->fJf); |
|
70 |
✓✗✓✗ ✓✗ |
264 |
d->Rx.leftCols(nv).noalias() = |
71 |
✓✗✓✗ |
264 |
d->pinocchio->oMf[id_].rotation() * d->fJf.template topRows<3>(); |
72 |
; |
||
73 |
264 |
} |
|
74 |
|||
75 |
template <typename Scalar> |
||
76 |
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
||
77 |
9538 |
ResidualModelFrameTranslationTpl<Scalar>::createData( |
|
78 |
DataCollectorAbstract* const data) { |
||
79 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
||
80 |
✓✗ | 9538 |
data); |
81 |
} |
||
82 |
|||
83 |
template <typename Scalar> |
||
84 |
63 |
void ResidualModelFrameTranslationTpl<Scalar>::print(std::ostream& os) const { |
|
85 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
126 |
const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[", |
86 |
"]"); |
||
87 |
63 |
os << "ResidualModelFrameTranslation {frame=" << pin_model_->frames[id_].name |
|
88 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
126 |
<< ", tref=" << xref_.transpose().format(fmt) << "}"; |
89 |
63 |
} |
|
90 |
|||
91 |
template <typename Scalar> |
||
92 |
pinocchio::FrameIndex ResidualModelFrameTranslationTpl<Scalar>::get_id() const { |
||
93 |
return id_; |
||
94 |
} |
||
95 |
|||
96 |
template <typename Scalar> |
||
97 |
const typename MathBaseTpl<Scalar>::Vector3s& |
||
98 |
ResidualModelFrameTranslationTpl<Scalar>::get_reference() const { |
||
99 |
return xref_; |
||
100 |
} |
||
101 |
|||
102 |
template <typename Scalar> |
||
103 |
void ResidualModelFrameTranslationTpl<Scalar>::set_id( |
||
104 |
const pinocchio::FrameIndex id) { |
||
105 |
id_ = id; |
||
106 |
} |
||
107 |
|||
108 |
template <typename Scalar> |
||
109 |
void ResidualModelFrameTranslationTpl<Scalar>::set_reference( |
||
110 |
const Vector3s& translation) { |
||
111 |
xref_ = translation; |
||
112 |
} |
||
113 |
|||
114 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |