GCC Code Coverage Report

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