| 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-placement.hpp" |
||
12 |
|||
13 |
namespace crocoddyl { |
||
14 |
|||
15 |
template <typename Scalar> |
||
16 |
895 |
ResidualModelFramePlacementTpl<Scalar>::ResidualModelFramePlacementTpl( |
|
17 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
18 |
const SE3& pref, const std::size_t nu) |
||
19 |
: Base(state, 6, nu, true, false, false), |
||
20 |
id_(id), |
||
21 |
pref_(pref), |
||
22 |
oMf_inv_(pref.inverse()), |
||
23 |
✓✗✓✗ ✓✗ |
895 |
pin_model_(state->get_pinocchio()) { |
24 |
✗✓ | 895 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
25 |
id) { |
||
26 |
throw_pretty( |
||
27 |
"Invalid argument: " |
||
28 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
29 |
} |
||
30 |
895 |
} |
|
31 |
|||
32 |
template <typename Scalar> |
||
33 |
10 |
ResidualModelFramePlacementTpl<Scalar>::ResidualModelFramePlacementTpl( |
|
34 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
35 |
const SE3& pref) |
||
36 |
: Base(state, 6, true, false, false), |
||
37 |
id_(id), |
||
38 |
pref_(pref), |
||
39 |
oMf_inv_(pref.inverse()), |
||
40 |
✓✗✓✗ ✓✗ |
10 |
pin_model_(state->get_pinocchio()) { |
41 |
✗✓ | 10 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
42 |
id) { |
||
43 |
throw_pretty( |
||
44 |
"Invalid argument: " |
||
45 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
46 |
} |
||
47 |
10 |
} |
|
48 |
|||
49 |
template <typename Scalar> |
||
50 |
1814 |
ResidualModelFramePlacementTpl<Scalar>::~ResidualModelFramePlacementTpl() {} |
|
51 |
|||
52 |
template <typename Scalar> |
||
53 |
37436 |
void ResidualModelFramePlacementTpl<Scalar>::calc( |
|
54 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
55 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
56 |
37436 |
Data* d = static_cast<Data*>(data.get()); |
|
57 |
|||
58 |
// Compute the frame placement w.r.t. the reference frame |
||
59 |
37436 |
pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_); |
|
60 |
37436 |
d->rMf = oMf_inv_ * d->pinocchio->oMf[id_]; |
|
61 |
✓✗✓✗ |
37436 |
data->r = pinocchio::log6(d->rMf).toVector(); |
62 |
37436 |
} |
|
63 |
|||
64 |
template <typename Scalar> |
||
65 |
8023 |
void ResidualModelFramePlacementTpl<Scalar>::calcDiff( |
|
66 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
67 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
68 |
8023 |
Data* d = static_cast<Data*>(data.get()); |
|
69 |
|||
70 |
// Compute the derivatives of the frame placement |
||
71 |
8023 |
const std::size_t nv = state_->get_nv(); |
|
72 |
8023 |
pinocchio::Jlog6(d->rMf, d->rJf); |
|
73 |
8023 |
pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_, |
|
74 |
8023 |
pinocchio::LOCAL, d->fJf); |
|
75 |
✓✗✓✗ ✓✗ |
8023 |
data->Rx.leftCols(nv).noalias() = d->rJf * d->fJf; |
76 |
8023 |
} |
|
77 |
|||
78 |
template <typename Scalar> |
||
79 |
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
||
80 |
46410 |
ResidualModelFramePlacementTpl<Scalar>::createData( |
|
81 |
DataCollectorAbstract* const data) { |
||
82 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
||
83 |
✓✗ | 46410 |
data); |
84 |
} |
||
85 |
|||
86 |
template <typename Scalar> |
||
87 |
63 |
void ResidualModelFramePlacementTpl<Scalar>::print(std::ostream& os) const { |
|
88 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
126 |
const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[", |
89 |
"]"); |
||
90 |
✓✗ | 63 |
typename SE3::Quaternion qref; |
91 |
✓✗✓✗ |
63 |
pinocchio::quaternion::assignQuaternion(qref, pref_.rotation()); |
92 |
63 |
os << "ResidualModelFramePlacement {frame=" << pin_model_->frames[id_].name |
|
93 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
189 |
<< ", tref=" << pref_.translation().transpose().format(fmt) |
94 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
63 |
<< ", qref=" << qref.coeffs().transpose().format(fmt) << "}"; |
95 |
63 |
} |
|
96 |
|||
97 |
template <typename Scalar> |
||
98 |
pinocchio::FrameIndex ResidualModelFramePlacementTpl<Scalar>::get_id() const { |
||
99 |
return id_; |
||
100 |
} |
||
101 |
|||
102 |
template <typename Scalar> |
||
103 |
const pinocchio::SE3Tpl<Scalar>& |
||
104 |
ResidualModelFramePlacementTpl<Scalar>::get_reference() const { |
||
105 |
return pref_; |
||
106 |
} |
||
107 |
|||
108 |
template <typename Scalar> |
||
109 |
void ResidualModelFramePlacementTpl<Scalar>::set_id( |
||
110 |
const pinocchio::FrameIndex id) { |
||
111 |
id_ = id; |
||
112 |
} |
||
113 |
|||
114 |
template <typename Scalar> |
||
115 |
void ResidualModelFramePlacementTpl<Scalar>::set_reference( |
||
116 |
const SE3& placement) { |
||
117 |
pref_ = placement; |
||
118 |
oMf_inv_ = placement.inverse(); |
||
119 |
} |
||
120 |
|||
121 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |