GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/frame-rotation.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 37 54 68.5%
Branches: 38 104 36.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <pinocchio/algorithm/frames.hpp>
11
12 #include "crocoddyl/multibody/residuals/frame-rotation.hpp"
13
14 namespace crocoddyl {
15
16 template <typename Scalar>
17 353 ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
18 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
19 const Matrix3s& Rref, const std::size_t nu)
20 : Base(state, 3, nu, true, false, false),
21 353 id_(id),
22 353 Rref_(Rref),
23
2/4
✓ Branch 1 taken 353 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 353 times.
✗ Branch 5 not taken.
 353 oRf_inv_(Rref.transpose()),
24
3/6
✓ Branch 2 taken 353 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 353 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 353 times.
✗ Branch 11 not taken.
 706 pin_model_(state->get_pinocchio()) {
25
2/4
✓ Branch 2 taken 353 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 353 times.
 353 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
26 id) {
27 ✗ throw_pretty(
28 "Invalid argument: "
29 << "the frame index is wrong (it does not exist in the robot)");
30 }
31 353 }
32
33 template <typename Scalar>
34 3 ResidualModelFrameRotationTpl<Scalar>::ResidualModelFrameRotationTpl(
35 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
36 const Matrix3s& Rref)
37 : Base(state, 3, true, false, false),
38 3 id_(id),
39 3 Rref_(Rref),
40
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 oRf_inv_(Rref.transpose()),
41
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 6 pin_model_(state->get_pinocchio()) {
42
2/4
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 3 times.
 3 if (static_cast<pinocchio::FrameIndex>(state->get_pinocchio()->nframes) <=
43 id) {
44 ✗ throw_pretty(
45 "Invalid argument: "
46 << "the frame index is wrong (it does not exist in the robot)");
47 }
48 3 }
49
50 template <typename Scalar>
51 8219 void ResidualModelFrameRotationTpl<Scalar>::calc(
52 const std::shared_ptr<ResidualDataAbstract>& data,
53 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
54 8219 Data* d = static_cast<Data*>(data.get());
55
56 // Compute the frame rotation w.r.t. the reference frame
57 8219 pinocchio::updateFramePlacement(*pin_model_.get(), *d->pinocchio, id_);
58
2/4
✓ Branch 4 taken 8219 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8219 times.
✗ Branch 8 not taken.
 8219 d->rRf.noalias() = oRf_inv_ * d->pinocchio->oMf[id_].rotation();
59
1/2
✓ Branch 3 taken 8219 times.
✗ Branch 4 not taken.
 8219 data->r = pinocchio::log3(d->rRf);
60 8219 }
61
62 template <typename Scalar>
63 276 void ResidualModelFrameRotationTpl<Scalar>::calcDiff(
64 const std::shared_ptr<ResidualDataAbstract>& data,
65 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
66 276 Data* d = static_cast<Data*>(data.get());
67
68 // Compute the frame Jacobian at the error point
69 276 pinocchio::Jlog3(d->rRf, d->rJf);
70 276 pinocchio::getFrameJacobian(*pin_model_.get(), *d->pinocchio, id_,
71 276 pinocchio::LOCAL, d->fJf);
72
73 // Compute the derivatives of the frame rotation
74 276 const std::size_t nv = state_->get_nv();
75
4/8
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 276 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 276 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 276 times.
✗ Branch 13 not taken.
 276 data->Rx.leftCols(nv).noalias() = d->rJf * d->fJf.template bottomRows<3>();
76 276 }
77
78 template <typename Scalar>
79 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
80 9556 ResidualModelFrameRotationTpl<Scalar>::createData(
81 DataCollectorAbstract* const data) {
82 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
83
1/2
✓ Branch 2 taken 9556 times.
✗ Branch 3 not taken.
 9556 data);
84 }
85
86 template <typename Scalar>
87 template <typename NewScalar>
88 ResidualModelFrameRotationTpl<NewScalar>
89 ✗ ResidualModelFrameRotationTpl<Scalar>::cast() const {
90 typedef ResidualModelFrameRotationTpl<NewScalar> ReturnType;
91 typedef StateMultibodyTpl<NewScalar> StateType;
92 ✗ ReturnType ret(
93 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
94 ✗ id_, Rref_.template cast<NewScalar>(), nu_);
95 ✗ return ret;
96 }
97
98 template <typename Scalar>
99 63 void ResidualModelFrameRotationTpl<Scalar>::print(std::ostream& os) const {
100
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", "", "", "[",
101 "]");
102
1/2
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
 63 typename pinocchio::SE3Tpl<Scalar>::Quaternion qref;
103
1/2
✓ Branch 1 taken 63 times.
✗ Branch 2 not taken.
 63 pinocchio::quaternion::assignQuaternion(qref, Rref_);
104 63 os << "ResidualModelFrameRotation {frame=" << pin_model_->frames[id_].name
105
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 12 taken 63 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 63 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 63 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 63 times.
✗ Branch 22 not taken.
 126 << ", qref=" << qref.coeffs().transpose().format(fmt) << "}";
106 63 }
107
108 template <typename Scalar>
109 ✗ pinocchio::FrameIndex ResidualModelFrameRotationTpl<Scalar>::get_id() const {
110 ✗ return id_;
111 }
112
113 template <typename Scalar>
114 const typename MathBaseTpl<Scalar>::Matrix3s&
115 ✗ ResidualModelFrameRotationTpl<Scalar>::get_reference() const {
116 ✗ return Rref_;
117 }
118
119 template <typename Scalar>
120 ✗ void ResidualModelFrameRotationTpl<Scalar>::set_id(
121 const pinocchio::FrameIndex id) {
122 ✗ id_ = id;
123 }
124
125 template <typename Scalar>
126 ✗ void ResidualModelFrameRotationTpl<Scalar>::set_reference(
127 const Matrix3s& rotation) {
128 ✗ Rref_ = rotation;
129 ✗ oRf_inv_ = rotation.transpose();
130 }
131
132 } // namespace crocoddyl
133