GCC Code Coverage Report

Directory: ./
File: bindings/python/utils/conversions.cpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 47 47 100.0%
Branches: 91 176 51.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2019-2021 CNRS INRIA
3 //
4
5 #include <sstream>
6
7 #include "pinocchio/bindings/python/fwd.hpp"
8 #include "pinocchio/bindings/python/spatial/se3.hpp"
9
10 namespace pinocchio
11 {
12 namespace python
13 {
14 namespace bp = boost::python;
15
16 struct XYZQUATConverter
17 {
18 typedef context::Scalar Scalar;
19 typedef context::SE3 SE3;
20 typedef context::VectorXs VectorXs;
21 enum
22 {
23 Options = context::Options
24 };
25 typedef Eigen::Matrix<Scalar, 7, 1, Options> Vector7s;
26 typedef Eigen::Map<SE3::Quaternion> QuatMap;
27 typedef Eigen::Map<const SE3::Quaternion> QuatConstMap;
28
29 3 static VectorXs fromSE3(const SE3 & M)
30 {
31
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Vector7s res;
32
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 res.head<3>() = M.translation();
33
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
 3 QuatMap(res.tail<4>().data()) = M.rotation();
34
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 return res;
35 }
36
37 2 static bp::tuple fromSE3tuple(const SE3 & M)
38 {
39
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 const SE3::Quaternion q(M.rotation());
40 return bp::make_tuple(
41
11/22
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2 times.
✗ Branch 32 not taken.
 4 M.translation()(0), M.translation()(1), M.translation()(2), q.x(), q.y(), q.z(), q.w());
42 }
43
44 template<typename TupleOrList>
45 12 static SE3 toSE3fromTupleOrList(const TupleOrList & v)
46 {
47
48
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 12 bp::ssize_t size = bp::len(v);
49
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 5 times.
 12 if (size != 7)
50 {
51
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 throw std::invalid_argument(
52 "Wrong size: v(" + std::to_string(size) + ") should have 7 elements");
53 }
54
55 // bp::extract<SE3::Scalar> to_double;
56
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v0 = bp::extract<Scalar>(v[0]);
57
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v1 = bp::extract<Scalar>(v[1]);
58
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v2 = bp::extract<Scalar>(v[2]);
59
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v3 = bp::extract<Scalar>(v[3]);
60
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v4 = bp::extract<Scalar>(v[4]);
61
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v5 = bp::extract<Scalar>(v[5]);
62
4/8
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5 times.
✗ Branch 11 not taken.
 10 const Scalar & v6 = bp::extract<Scalar>(v[6]);
63
64
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 SE3::Quaternion q(v6, v3, v4, v5);
65
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 10 SE3::Vector3 t(v0, v1, v2);
66
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 10 return SE3(q.matrix(), t);
67 }
68
69 template<typename Vector7Like>
70 SE3 XYZQUATToSE3_ei(const Vector7Like & v)
71 {
72 if (v.rows() != 7 || v.cols() != 1)
73 {
74 std::ostringstream shape;
75 shape << "(" << v.rows() << ", " << v.cols() << ")";
76 throw std::invalid_argument(
77 "Wrong size: v" + shape.str() + " but should have the following shape (7, 1)");
78 }
79 QuatConstMap q(v.template tail<4>().data());
80 return SE3(q.matrix(), v.template head<3>());
81 }
82
83 template<typename Vector7Like>
84 4 static SE3 toSE3(const Vector7Like & v)
85 {
86
5/6
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3 times.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 3 times.
 4 if (v.rows() != 7 || v.cols() != 1)
87 {
88
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::ostringstream shape;
89
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 1 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 1 times.
✗ Branch 16 not taken.
 1 shape << "(" << v.rows() << ", " << v.cols() << ")";
90
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 1 throw std::invalid_argument(
91 "Wrong size: v" + shape.str() + " but should have the following shape (7, 1)");
92 1 }
93
94
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 3 PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(Vector7Like, v, 7, 1);
95
2/4
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 QuatConstMap q(v.template tail<4>().data());
96
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
 3 return SE3(q.matrix(), v.template head<3>());
97 }
98
99 69 static void expose()
100 {
101
102 69 const char * doc1 = "Convert the input SE3 object to a numpy array.";
103
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def("SE3ToXYZQUAT", fromSE3, "M", doc1);
104 69 const char * doc1_tuple =
105 "Convert the input SE3 object to a 7D tuple of floats [X,Y,Z,x,y,z,w].";
106
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def("SE3ToXYZQUATtuple", fromSE3tuple, "M", doc1_tuple);
107
108 69 const char * doc2 =
109 "Reverse function of SE3ToXYZQUAT: convert [X,Y,Z,x,y,z,w] to an SE3 element.";
110
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
111 "XYZQUATToSE3", static_cast<SE3 (*)(const bp::tuple &)>(toSE3fromTupleOrList<bp::tuple>),
112
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 bp::arg("tuple"), doc2);
113
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
114 "XYZQUATToSE3", static_cast<SE3 (*)(const bp::list &)>(toSE3fromTupleOrList<bp::list>),
115
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 bp::arg("list"), doc2);
116
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 69 bp::def(
117
1/2
✓ Branch 1 taken 69 times.
✗ Branch 2 not taken.
 138 "XYZQUATToSE3", static_cast<SE3 (*)(const VectorXs &)>(toSE3<VectorXs>), bp::arg("array"),
118 doc2);
119 69 }
120 };
121
122 69 void exposeConversions()
123 {
124 69 XYZQUATConverter::expose();
125 69 }
126
127 } // namespace python
128 } // namespace pinocchio
129