GCC Code Coverage Report

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