GCC Code Coverage Report

Directory: ./
File: include/pinocchio/algorithm/utils/force.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 30 31 96.8%
Branches: 64 110 58.2%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020 INRIA
3 //
4
5 #ifndef __pinocchio_algorithm_utils_force_hpp__
6 #define __pinocchio_algorithm_utils_force_hpp__
7
8 #include "pinocchio/spatial/se3.hpp"
9 #include "pinocchio/spatial/force.hpp"
10 #include "pinocchio/multibody/fwd.hpp"
11
12 namespace pinocchio
13 {
14 ///
15 /// @copydoc changeReferenceFrame(const SE3Tpl<Scalar,Options> &,const ForceDense<MotionIn>
16 /// &,const ReferenceFrame,const ReferenceFrame) \param[out] f_out Resulting force quantity.
17 ///
18 template<typename Scalar, int Options, typename ForceIn, typename ForceOut>
19 18 void changeReferenceFrame(
20 const SE3Tpl<Scalar, Options> & placement,
21 const ForceDense<ForceIn> & f_in,
22 const ReferenceFrame rf_in,
23 const ReferenceFrame rf_out,
24 ForceDense<ForceOut> & f_out)
25 {
26
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 12 times.
 18 if (rf_in == rf_out)
27 {
28 6 f_out = f_in;
29 6 return;
30 }
31
32 // case: LOCAL/WORLD and WORLD/LOCAL
33
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 8 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
 12 if (rf_in == LOCAL && rf_out == WORLD)
34 {
35
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 f_out = placement.act(f_in);
36 2 return;
37 }
38
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
 10 if (rf_in == WORLD && rf_out == LOCAL)
39 {
40
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 f_out = placement.actInv(f_in);
41 2 return;
42 }
43
44 // case: LOCAL/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/LOCAL
45
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 8 if (rf_in == LOCAL && rf_out == LOCAL_WORLD_ALIGNED)
46 {
47
5/10
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 2 f_out.angular().noalias() = placement.rotation() * f_in.angular();
48
5/10
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
 2 f_out.linear().noalias() = placement.rotation() * f_in.linear();
49 2 return;
50 }
51
4/4
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
 6 if (rf_in == LOCAL_WORLD_ALIGNED && rf_out == LOCAL)
52 {
53
6/12
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
 2 f_out.angular().noalias() = placement.rotation().transpose() * f_in.angular();
54
6/12
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
 2 f_out.linear().noalias() = placement.rotation().transpose() * f_in.linear();
55 2 return;
56 }
57
58 // case: WORLD/LOCAL_WORLD_ALIGNED and LOCAL_WORLD_ALIGNED/WORLD
59
3/4
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 4 if (rf_in == WORLD && rf_out == LOCAL_WORLD_ALIGNED)
60 {
61
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 f_out.linear() = f_in.linear();
62
7/14
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
 2 f_out.angular().noalias() = f_in.angular() + f_in.linear().cross(placement.translation());
63 2 return;
64 }
65
2/4
✓ Branch 0 taken 2 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
 2 if (rf_in == LOCAL_WORLD_ALIGNED && rf_out == WORLD)
66 {
67
2/4
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 2 f_out.linear() = f_in.linear();
68
7/14
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2 times.
✗ Branch 21 not taken.
 2 f_out.angular().noalias() = f_in.angular() - f_in.linear().cross(placement.translation());
69 2 return;
70 }
71
72 ✗ assert(false && "This must never happened.");
73 }
74
75 ///
76 ///  \brief Change the expression of a given Force vector from one reference frame to another
77 /// reference frame.
78 ///
79 ///  \example If ones has an initial f_in Force expressed locally (rf_in=LOCAL) in a Frame
80 /// localized at a given placement value,   ones may want to retrieve its value inside
81 /// another reference frame e.g. the world (rf_out=WORLD).
82 ///
83 /// \param[in] placement Placement of the frame having force f_in
84 /// \param[in] f_in Input force quantity.
85 /// \param[in] rf_in Reference frame in which m_in is expressed
86 /// \param[in] rf_out Reference frame in which the result m_out is expressed
87 /// \param[out] f_out Resulting force quantity.
88 ///
89 template<typename Scalar, int Options, typename ForceIn>
90 18 typename ForceIn::ForcePlain changeReferenceFrame(
91 const SE3Tpl<Scalar, Options> & placement,
92 const ForceDense<ForceIn> & f_in,
93 const ReferenceFrame rf_in,
94 const ReferenceFrame rf_out)
95 {
96 typedef typename ForceIn::ForcePlain ReturnType;
97 18 ReturnType res;
98
99 18 changeReferenceFrame(placement, f_in, rf_in, rf_out, res);
100
101 18 return res;
102 }
103
104 } // namespace pinocchio
105
106 #endif // #ifndef __pinocchio_algorithm_utils_force_hpp__
107