1 |
|
|
// |
2 |
|
|
// Copyright (c) 2022 INRIA |
3 |
|
|
// |
4 |
|
|
// This file is part of tsid |
5 |
|
|
// tsid is free software: you can redistribute it |
6 |
|
|
// and/or modify it under the terms of the GNU Lesser General Public |
7 |
|
|
// License as published by the Free Software Foundation, either version |
8 |
|
|
// 3 of the License, or (at your option) any later version. |
9 |
|
|
// tsid is distributed in the hope that it will be |
10 |
|
|
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty |
11 |
|
|
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
12 |
|
|
// General Lesser Public License for more details. You should have |
13 |
|
|
// received a copy of the GNU Lesser General Public License along with |
14 |
|
|
// tsid If not, see |
15 |
|
|
// <http://www.gnu.org/licenses/>. |
16 |
|
|
// |
17 |
|
|
|
18 |
|
|
#include "tsid/bindings/python/fwd.hpp" |
19 |
|
|
#include <pinocchio/bindings/python/utils/deprecation.hpp> |
20 |
|
|
#include "tsid/bindings/python/math/utils.hpp" |
21 |
|
|
#include "tsid/math/utils.hpp" |
22 |
|
|
|
23 |
|
|
namespace tsid { |
24 |
|
|
namespace python { |
25 |
|
|
static math::Vector SE3ToVector_1(const pinocchio::SE3& M) { |
26 |
|
|
math::Vector res(12); |
27 |
|
|
math::SE3ToVector(M, res); |
28 |
|
|
return res; |
29 |
|
|
} |
30 |
|
|
|
31 |
|
|
static pinocchio::SE3 vectorToSE3_1(math::RefVector vec) { |
32 |
|
|
pinocchio::SE3 res; |
33 |
|
|
math::vectorToSE3(vec, res); |
34 |
|
|
return res; |
35 |
|
|
} |
36 |
|
|
|
37 |
|
7 |
void exposeMathUtils() { |
38 |
|
|
namespace bp = boost::python; |
39 |
|
|
using namespace math; |
40 |
|
|
|
41 |
✓✗ |
7 |
bp::def("SE3ToVector", &SE3ToVector, bp::args("M", "vec"), |
42 |
|
|
"Convert the input SE3 object M to a 12D vector of floats " |
43 |
|
|
"[X,Y,Z,R11,R12,R13,R14,...] vec"); |
44 |
✓✗ |
7 |
bp::def("SE3ToVector", &SE3ToVector_1, bp::args("M"), |
45 |
|
|
"Convert the input SE3 object M to a 12D vector of floats " |
46 |
|
|
"[X,Y,Z,R11,R12,R13,R14,...] and return the vector"); |
47 |
✓✗ |
7 |
bp::def("vectorToSE3", &vectorToSE3, bp::args("vec", "M"), |
48 |
|
|
"Convert the input 12D vector of floats [X,Y,Z,R11,R12,R13,R14,...] " |
49 |
|
|
"vec to a SE3 object M"); |
50 |
✓✗ |
7 |
bp::def("vectorToSE3", &vectorToSE3_1, bp::args("vec"), |
51 |
|
|
"Convert the input 12D vector of floats [X,Y,Z,R11,R12,R13,R14,...] " |
52 |
|
|
"vec to a SE3 object and return the SE3 object"); |
53 |
|
7 |
} |
54 |
|
|
} // namespace python |
55 |
|
|
} // namespace tsid |