GCC Code Coverage Report

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