GCC Code Coverage Report

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