GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/impulses/impulse-3d.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 72 79 91.1%
Branches: 73 157 46.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
12
13 #include "crocoddyl/multibody/impulses/impulse-3d.hpp"
14
15 namespace crocoddyl {
16
17 template <typename Scalar>
18 209 ImpulseModel3DTpl<Scalar>::ImpulseModel3DTpl(
19 std::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
20 const pinocchio::ReferenceFrame type)
21
1/2
✓ Branch 2 taken 209 times.
✗ Branch 3 not taken.
 209 : Base(state, type, 3) {
22 209 id_ = id;
23 209 }
24
25 template <typename Scalar>
26 5039 void ImpulseModel3DTpl<Scalar>::calc(
27 const std::shared_ptr<ImpulseDataAbstract>& data,
28 const Eigen::Ref<const VectorXs>&) {
29 5039 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
30
2/4
✓ Branch 1 taken 5039 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 5039 times.
✗ Branch 6 not taken.
 5039 pinocchio::updateFramePlacement<Scalar>(*state_->get_pinocchio().get(),
31 5039 *d->pinocchio, id_);
32
2/4
✓ Branch 2 taken 5039 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 5039 times.
✗ Branch 7 not taken.
 5039 pinocchio::getFrameJacobian(*state_->get_pinocchio().get(), *d->pinocchio,
33 5039 id_, pinocchio::LOCAL, d->fJf);
34
35
2/3
✓ Branch 0 taken 2677 times.
✓ Branch 1 taken 2362 times.
✗ Branch 2 not taken.
 5039 switch (type_) {
36 2677 case pinocchio::ReferenceFrame::LOCAL:
37
2/4
✓ Branch 2 taken 2677 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2677 times.
✗ Branch 7 not taken.
 2677 data->Jc = d->fJf.template topRows<3>();
38 2677 break;
39 2362 case pinocchio::ReferenceFrame::WORLD:
40 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
41
2/4
✓ Branch 2 taken 2362 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2362 times.
✗ Branch 6 not taken.
 2362 data->Jc.noalias() =
42
3/6
✓ Branch 2 taken 2362 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 2362 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2362 times.
✗ Branch 11 not taken.
 2362 d->pinocchio->oMf[id_].rotation() * d->fJf.template topRows<3>();
43 2362 break;
44 }
45 5039 }
46
47 template <typename Scalar>
48 448 void ImpulseModel3DTpl<Scalar>::calcDiff(
49 const std::shared_ptr<ImpulseDataAbstract>& data,
50 const Eigen::Ref<const VectorXs>&) {
51 448 std::shared_ptr<Data> d = std::static_pointer_cast<Data>(data);
52
53 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
54 448 const pinocchio::JointIndex joint =
55
1/2
✓ Branch 2 taken 448 times.
✗ Branch 3 not taken.
 448 state_->get_pinocchio()->frames[d->frame].parentJoint;
56 #else
57 const pinocchio::JointIndex joint =
58 state_->get_pinocchio()->frames[d->frame].parent;
59 #endif
60
2/4
✓ Branch 1 taken 448 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 448 times.
✗ Branch 6 not taken.
 448 pinocchio::getJointVelocityDerivatives(*state_->get_pinocchio().get(),
61 448 *d->pinocchio, joint, pinocchio::LOCAL,
62 448 d->v_partial_dq, d->v_partial_dv);
63
4/8
✓ Branch 3 taken 448 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 448 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 448 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 448 times.
✗ Branch 14 not taken.
 448 d->dv0_local_dq.noalias() = d->fXj.template topRows<3>() * d->v_partial_dq;
64
65
2/3
✓ Branch 0 taken 236 times.
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
 448 switch (type_) {
66 236 case pinocchio::ReferenceFrame::LOCAL:
67
1/2
✓ Branch 3 taken 236 times.
✗ Branch 4 not taken.
 236 data->dv0_dq = d->dv0_local_dq;
68 236 break;
69 212 case pinocchio::ReferenceFrame::WORLD:
70 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
71
2/4
✓ Branch 3 taken 212 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
 212 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
72
2/4
✓ Branch 1 taken 212 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
 424 d->v0 = pinocchio::getFrameVelocity(*state_->get_pinocchio().get(),
73 212 *d->pinocchio, id_,
74 pinocchio::LOCAL_WORLD_ALIGNED)
75
2/4
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 212 times.
✗ Branch 6 not taken.
 424 .linear();
76
1/2
✓ Branch 3 taken 212 times.
✗ Branch 4 not taken.
 212 pinocchio::skew(d->v0, d->v0_skew);
77
3/6
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 212 times.
✗ Branch 10 not taken.
 212 d->v0_world_skew.noalias() = d->v0_skew * oRf;
78
3/6
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 212 times.
✗ Branch 10 not taken.
 212 data->dv0_dq.noalias() = oRf * d->dv0_local_dq;
79
2/4
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 212 times.
✗ Branch 6 not taken.
 212 data->dv0_dq.noalias() -=
80
2/4
✓ Branch 2 taken 212 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 212 times.
✗ Branch 7 not taken.
 212 d->v0_world_skew * d->fJf.template bottomRows<3>();
81 212 break;
82 212 }
83 448 }
84
85 template <typename Scalar>
86 4997 void ImpulseModel3DTpl<Scalar>::updateForce(
87 const std::shared_ptr<ImpulseDataAbstract>& data, const VectorXs& force) {
88
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 4997 times.
 4997 if (force.size() != 3) {
89 ✗ throw_pretty(
90 "Invalid argument: " << "lambda has wrong dimension (it should be 3)");
91 }
92 4997 Data* d = static_cast<Data*>(data.get());
93
1/2
✓ Branch 3 taken 4997 times.
✗ Branch 4 not taken.
 4997 data->f.linear() = force;
94
1/2
✓ Branch 3 taken 4997 times.
✗ Branch 4 not taken.
 4997 data->f.angular().setZero();
95
2/3
✓ Branch 0 taken 2649 times.
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
 4997 switch (type_) {
96 2649 case pinocchio::ReferenceFrame::LOCAL:
97
2/4
✓ Branch 2 taken 2649 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2649 times.
✗ Branch 7 not taken.
 2649 data->fext = d->jMf.act(data->f);
98
1/2
✓ Branch 2 taken 2649 times.
✗ Branch 3 not taken.
 2649 data->dtau_dq.setZero();
99 2649 break;
100 2348 case pinocchio::ReferenceFrame::WORLD:
101 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
102
2/4
✓ Branch 2 taken 2348 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2348 times.
✗ Branch 6 not taken.
 2348 const Eigen::Ref<const Matrix3s> oRf = d->pinocchio->oMf[id_].rotation();
103
5/10
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2348 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2348 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2348 times.
✗ Branch 14 not taken.
 2348 d->f_local.linear().noalias() = oRf.transpose() * force;
104
2/4
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
 2348 d->f_local.angular().setZero();
105
2/4
✓ Branch 2 taken 2348 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2348 times.
✗ Branch 7 not taken.
 2348 data->fext = data->jMf.act(d->f_local);
106
2/4
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
 2348 pinocchio::skew(d->f_local.linear(), d->f_skew);
107
4/8
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2348 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2348 times.
✗ Branch 11 not taken.
 2348 d->fJf_df.noalias() = d->f_skew * d->fJf.template bottomRows<3>();
108
4/8
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 2348 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2348 times.
✗ Branch 12 not taken.
 2348 data->dtau_dq.noalias() =
109
2/4
✓ Branch 1 taken 2348 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2348 times.
✗ Branch 5 not taken.
 2348 -d->fJf.template topRows<3>().transpose() * d->fJf_df;
110 2348 break;
111 2348 }
112 4997 }
113
114 template <typename Scalar>
115 std::shared_ptr<ImpulseDataAbstractTpl<Scalar> >
116 3497 ImpulseModel3DTpl<Scalar>::createData(pinocchio::DataTpl<Scalar>* const data) {
117 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
118
1/2
✓ Branch 2 taken 3497 times.
✗ Branch 3 not taken.
 3497 data);
119 }
120
121 template <typename Scalar>
122 template <typename NewScalar>
123 ✗ ImpulseModel3DTpl<NewScalar> ImpulseModel3DTpl<Scalar>::cast() const {
124 typedef ImpulseModel3DTpl<NewScalar> ReturnType;
125 typedef StateMultibodyTpl<NewScalar> StateType;
126 ✗ ReturnType ret(
127 ✗ std::make_shared<StateType>(state_->template cast<NewScalar>()), id_,
128 ✗ type_);
129 ✗ return ret;
130 }
131
132 template <typename Scalar>
133 15 void ImpulseModel3DTpl<Scalar>::print(std::ostream& os) const {
134 15 os << "ImpulseModel3D {frame=" << state_->get_pinocchio()->frames[id_].name
135 30 << ", type=" << type_ << "}";
136 15 }
137
138 } // namespace crocoddyl
139