GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/frame-translation.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 33 44 75.0%
Branches: 27 74 36.5%
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 345 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 345 id_(id),
21 345 xref_(xref),
22
2/4
✓ Branch 2 taken 345 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 345 times.
✗ Branch 7 not taken.
 345 pin_model_(state->get_pinocchio()) {
23
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 345 times.
 345 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 345 }
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 3 id_(id),
37 3 xref_(xref),
38
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
 3 pin_model_(state->get_pinocchio()) {
39
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
 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 700 ResidualModelFrameTranslationTpl<Scalar>::~ResidualModelFrameTranslationTpl() {}
49
50 template <typename Scalar>
51 8225 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 8225 Data* d = static_cast<Data*>(data.get());
56 8225 pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_);
57
1/2
✓ Branch 5 taken 8225 times.
✗ Branch 6 not taken.
 8225 data->r = d->pinocchio->oMf[id_].translation() - xref_;
58 8225 }
59
60 template <typename Scalar>
61 296 void ResidualModelFrameTranslationTpl<Scalar>::calcDiff(
62 const boost::shared_ptr<ResidualDataAbstract>& data,
63 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
64 296 Data* d = static_cast<Data*>(data.get());
65
66 // Compute the derivatives of the frame translation
67 296 const std::size_t nv = state_->get_nv();
68 296 pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_,
69 296 pinocchio::LOCAL, d->fJf);
70
3/6
✓ Branch 1 taken 296 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 296 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 296 times.
✗ Branch 8 not taken.
 296 d->Rx.leftCols(nv).noalias() =
71
2/4
✓ Branch 3 taken 296 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 296 times.
✗ Branch 7 not taken.
 296 d->pinocchio->oMf[id_].rotation() * d->fJf.template topRows<3>();
72 ;
73 296 }
74
75 template <typename Scalar>
76 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
77 9554 ResidualModelFrameTranslationTpl<Scalar>::createData(
78 DataCollectorAbstract* const data) {
79 ✗ return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
80
1/2
✓ Branch 2 taken 9554 times.
✗ Branch 3 not taken.
 9554 data);
81 }
82
83 template <typename Scalar>
84 63 void ResidualModelFrameTranslationTpl<Scalar>::print(std::ostream& os) const {
85
7/14
✓ Branch 2 taken 63 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 63 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 63 times.
✗ Branch 11 not taken.
✓ Branch 14 taken 63 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 63 times.
✗ Branch 19 not taken.
✓ Branch 22 taken 63 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 63 times.
✗ Branch 26 not taken.
 126 const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
86 "]");
87 63 os << "ResidualModelFrameTranslation {frame=" << pin_model_->frames[id_].name
88
7/14
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 63 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 63 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 63 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 63 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 63 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 63 times.
✗ Branch 21 not taken.
 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
115