| 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-derivatives.hpp> |
||
10 |
#include <pinocchio/algorithm/frames.hpp> |
||
11 |
#include <pinocchio/algorithm/kinematics-derivatives.hpp> |
||
12 |
|||
13 |
#include "crocoddyl/multibody/residuals/frame-velocity.hpp" |
||
14 |
|||
15 |
namespace crocoddyl { |
||
16 |
|||
17 |
template <typename Scalar> |
||
18 |
596 |
ResidualModelFrameVelocityTpl<Scalar>::ResidualModelFrameVelocityTpl( |
|
19 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
20 |
const Motion& velocity, const pinocchio::ReferenceFrame type, |
||
21 |
const std::size_t nu) |
||
22 |
: Base(state, 6, nu, true, true, false), |
||
23 |
id_(id), |
||
24 |
vref_(velocity), |
||
25 |
type_(type), |
||
26 |
✓✗✓✗ |
596 |
pin_model_(state->get_pinocchio()) { |
27 |
✗✓ | 596 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
28 |
id) { |
||
29 |
throw_pretty( |
||
30 |
"Invalid argument: " |
||
31 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
32 |
} |
||
33 |
596 |
} |
|
34 |
|||
35 |
template <typename Scalar> |
||
36 |
3 |
ResidualModelFrameVelocityTpl<Scalar>::ResidualModelFrameVelocityTpl( |
|
37 |
boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id, |
||
38 |
const Motion& velocity, const pinocchio::ReferenceFrame type) |
||
39 |
: Base(state, 6, true, true, false), |
||
40 |
id_(id), |
||
41 |
vref_(velocity), |
||
42 |
type_(type), |
||
43 |
✓✗✓✗ |
3 |
pin_model_(state->get_pinocchio()) { |
44 |
✗✓ | 3 |
if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <= |
45 |
id) { |
||
46 |
throw_pretty( |
||
47 |
"Invalid argument: " |
||
48 |
<< "the frame index is wrong (it does not exist in the robot)"); |
||
49 |
} |
||
50 |
3 |
} |
|
51 |
|||
52 |
template <typename Scalar> |
||
53 |
1202 |
ResidualModelFrameVelocityTpl<Scalar>::~ResidualModelFrameVelocityTpl() {} |
|
54 |
|||
55 |
template <typename Scalar> |
||
56 |
21150 |
void ResidualModelFrameVelocityTpl<Scalar>::calc( |
|
57 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
58 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
59 |
21150 |
Data* d = static_cast<Data*>(data.get()); |
|
60 |
|||
61 |
// Compute the frame velocity w.r.t. the reference frame |
||
62 |
✓✗✓✗ ✓✗ |
21150 |
data->r = (pinocchio::getFrameVelocity(*pin_model_.get(), *d->pinocchio, id_, |
63 |
type_) - |
||
64 |
21150 |
vref_) |
|
65 |
.toVector(); |
||
66 |
21150 |
} |
|
67 |
|||
68 |
template <typename Scalar> |
||
69 |
3533 |
void ResidualModelFrameVelocityTpl<Scalar>::calcDiff( |
|
70 |
const boost::shared_ptr<ResidualDataAbstract>& data, |
||
71 |
const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) { |
||
72 |
// Get the partial derivatives of the local frame velocity |
||
73 |
3533 |
Data* d = static_cast<Data*>(data.get()); |
|
74 |
3533 |
const std::size_t nv = state_->get_nv(); |
|
75 |
✓✗ | 3533 |
pinocchio::getFrameVelocityDerivatives(*pin_model_.get(), *d->pinocchio, id_, |
76 |
✓✗ | 3533 |
type_, data->Rx.leftCols(nv), |
77 |
3533 |
data->Rx.rightCols(nv)); |
|
78 |
3533 |
} |
|
79 |
|||
80 |
template <typename Scalar> |
||
81 |
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
||
82 |
26103 |
ResidualModelFrameVelocityTpl<Scalar>::createData( |
|
83 |
DataCollectorAbstract* const data) { |
||
84 |
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
||
85 |
✓✗ | 26103 |
data); |
86 |
} |
||
87 |
|||
88 |
template <typename Scalar> |
||
89 |
63 |
void ResidualModelFrameVelocityTpl<Scalar>::print(std::ostream& os) const { |
|
90 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
126 |
const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[", |
91 |
"]"); |
||
92 |
63 |
os << "ResidualModelFrameVelocity {frame=" << pin_model_->frames[id_].name |
|
93 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
126 |
<< ", vref=" << vref_.toVector().transpose().format(fmt) << "}"; |
94 |
63 |
} |
|
95 |
|||
96 |
template <typename Scalar> |
||
97 |
pinocchio::FrameIndex ResidualModelFrameVelocityTpl<Scalar>::get_id() const { |
||
98 |
return id_; |
||
99 |
} |
||
100 |
|||
101 |
template <typename Scalar> |
||
102 |
const pinocchio::MotionTpl<Scalar>& |
||
103 |
ResidualModelFrameVelocityTpl<Scalar>::get_reference() const { |
||
104 |
return vref_; |
||
105 |
} |
||
106 |
|||
107 |
template <typename Scalar> |
||
108 |
pinocchio::ReferenceFrame ResidualModelFrameVelocityTpl<Scalar>::get_type() |
||
109 |
const { |
||
110 |
return type_; |
||
111 |
} |
||
112 |
|||
113 |
template <typename Scalar> |
||
114 |
void ResidualModelFrameVelocityTpl<Scalar>::set_id( |
||
115 |
const pinocchio::FrameIndex id) { |
||
116 |
id_ = id; |
||
117 |
} |
||
118 |
|||
119 |
template <typename Scalar> |
||
120 |
void ResidualModelFrameVelocityTpl<Scalar>::set_reference( |
||
121 |
const Motion& velocity) { |
||
122 |
vref_ = velocity; |
||
123 |
} |
||
124 |
|||
125 |
template <typename Scalar> |
||
126 |
void ResidualModelFrameVelocityTpl<Scalar>::set_type( |
||
127 |
const pinocchio::ReferenceFrame type) { |
||
128 |
type_ = type; |
||
129 |
} |
||
130 |
|||
131 |
} // namespace crocoddyl |
| Generated by: GCOVR (Version 4.2) |