GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/frame-placement.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 38 55 69.1%
Branches: 41 108 38.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, 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 891 ResidualModelFramePlacementTpl<Scalar>::ResidualModelFramePlacementTpl(
17 std::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 891 id_(id),
21 891 pref_(pref),
22
1/2
✓ Branch 1 taken 891 times.
✗ Branch 2 not taken.
 891 oMf_inv_(pref.inverse()),
23
3/6
✓ Branch 2 taken 891 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 891 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 891 times.
✗ Branch 11 not taken.
 1782 pin_model_(state->get_pinocchio()) {
24
2/4
✓ Branch 2 taken 891 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 891 times.
 891 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 891 }
31
32 template <typename Scalar>
33 10 ResidualModelFramePlacementTpl<Scalar>::ResidualModelFramePlacementTpl(
34 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
35 const SE3& pref)
36 : Base(state, 6, true, false, false),
37 10 id_(id),
38 10 pref_(pref),
39
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 oMf_inv_(pref.inverse()),
40
3/6
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 10 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 10 times.
✗ Branch 11 not taken.
 20 pin_model_(state->get_pinocchio()) {
41
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
 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 37812 void ResidualModelFramePlacementTpl<Scalar>::calc(
51 const std::shared_ptr<ResidualDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
53 37812 Data* d = static_cast<Data*>(data.get());
54
55 // Compute the frame placement w.r.t. the reference frame
56 37812 pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_);
57
1/2
✓ Branch 3 taken 37812 times.
✗ Branch 4 not taken.
 37812 d->rMf = oMf_inv_ * d->pinocchio->oMf[id_];
58
2/4
✓ Branch 2 taken 37812 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 37812 times.
✗ Branch 7 not taken.
 37812 data->r = pinocchio::log6(d->rMf).toVector();
59 37812 }
60
61 template <typename Scalar>
62 8185 void ResidualModelFramePlacementTpl<Scalar>::calcDiff(
63 const std::shared_ptr<ResidualDataAbstract>& data,
64 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
65 8185 Data* d = static_cast<Data*>(data.get());
66
67 // Compute the derivatives of the frame placement
68 8185 const std::size_t nv = state_->get_nv();
69 8185 pinocchio::Jlog6(d->rMf, d->rJf);
70 8185 pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_,
71 8185 pinocchio::LOCAL, d->fJf);
72
3/6
✓ Branch 3 taken 8185 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 8185 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 8185 times.
✗ Branch 10 not taken.
 8185 data->Rx.leftCols(nv).noalias() = d->rJf * d->fJf;
73 8185 }
74
75 template <typename Scalar>
76 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
77 46302 ResidualModelFramePlacementTpl<Scalar>::createData(
78 DataCollectorAbstract* const data) {
79 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
80
1/2
✓ Branch 2 taken 46302 times.
✗ Branch 3 not taken.
 46302 data);
81 }
82
83 template <typename Scalar>
84 template <typename NewScalar>
85 ResidualModelFramePlacementTpl<NewScalar>
86 ✗ ResidualModelFramePlacementTpl<Scalar>::cast() const {
87 typedef ResidualModelFramePlacementTpl<NewScalar> ReturnType;
88 typedef StateMultibodyTpl<NewScalar> StateType;
89 ✗ ReturnType ret(
90 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
91 ✗ id_, pref_.template cast<NewScalar>(), nu_);
92 ✗ return ret;
93 }
94
95 template <typename Scalar>
96 63 void ResidualModelFramePlacementTpl<Scalar>::print(std::ostream& os) const {
97
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", "", "", "[",
98 "]");
99
1/2
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
 63 typename SE3::Quaternion qref;
100
2/4
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 63 times.
✗ Branch 5 not taken.
 63 pinocchio::quaternion::assignQuaternion(qref, pref_.rotation());
101 63 os << "ResidualModelFramePlacement {frame=" << pin_model_->frames[id_].name
102
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=" << pref_.translation().transpose().format(fmt)
103
5/10
✓ 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.
 63 << ", qref=" << qref.coeffs().transpose().format(fmt) << "}";
104 63 }
105
106 template <typename Scalar>
107 ✗ pinocchio::FrameIndex ResidualModelFramePlacementTpl<Scalar>::get_id() const {
108 ✗ return id_;
109 }
110
111 template <typename Scalar>
112 const pinocchio::SE3Tpl<Scalar>&
113 ✗ ResidualModelFramePlacementTpl<Scalar>::get_reference() const {
114 ✗ return pref_;
115 }
116
117 template <typename Scalar>
118 ✗ void ResidualModelFramePlacementTpl<Scalar>::set_id(
119 const pinocchio::FrameIndex id) {
120 ✗ id_ = id;
121 }
122
123 template <typename Scalar>
124 ✗ void ResidualModelFramePlacementTpl<Scalar>::set_reference(
125 const SE3& placement) {
126 ✗ pref_ = placement;
127 ✗ oMf_inv_ = placement.inverse();
128 }
129
130 } // namespace crocoddyl
131