| Directory: | ./ |
|---|---|
| File: | unittest/casadi/casadi-utils.hpp |
| Date: | 2025-02-12 21:03:38 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 6 | 6 | 100.0% |
| Branches: | 6 | 12 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2021 Inria | ||
| 3 | // | ||
| 4 | // Utils for tests, e.g. conversion between Eigen matrix | ||
| 5 | // and DM without copy. | ||
| 6 | #include "pinocchio/autodiff/casadi.hpp" | ||
| 7 | #include "pinocchio/autodiff/casadi-algo.hpp" | ||
| 8 | |||
| 9 | /// Without copy | ||
| 10 | template<typename Derived> | ||
| 11 | 128 | casadi::DM eigenToDM(const Eigen::MatrixBase<Derived> & x) | |
| 12 | { | ||
| 13 | typedef Eigen::Map<Derived> Map_t; | ||
| 14 |
1/2✓ Branch 3 taken 128 times.
✗ Branch 4 not taken.
|
128 | std::vector<double> x_vec((size_t)x.size()); |
| 15 |
3/6✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
✓ Branch 7 taken 128 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 128 times.
✗ Branch 11 not taken.
|
128 | Map_t(x_vec.data(), x.rows(), x.cols()) = x; |
| 16 |
1/2✓ Branch 1 taken 128 times.
✗ Branch 2 not taken.
|
128 | casadi::DM out(x_vec); |
| 17 |
1/2✓ Branch 3 taken 128 times.
✗ Branch 4 not taken.
|
256 | return reshape(out, x.rows(), x.cols()); |
| 18 | 128 | } | |
| 19 | |||
| 20 | template<typename Derived> | ||
| 21 | casadi::DM SE3toCasadiDM(const pinocchio::SE3Base<Derived> & M) | ||
| 22 | { | ||
| 23 | typedef pinocchio::SE3Base<Derived> SE3; | ||
| 24 | typedef typename Derived::Scalar Scalar; | ||
| 25 | typename SE3::HomogeneousMatrixType M_mat = M.toHomogeneousMatrix(); | ||
| 26 | std::vector<Scalar> flat_M_vec(M_mat.data(), M_mat.data() + M_mat.size()); | ||
| 27 | casadi::DM out{flat_M_vec}; | ||
| 28 | return reshape(out, 4, 4); | ||
| 29 | } | ||
| 30 |