Crocoddyl
Loading...
Searching...
No Matches
conversions.hpp
1
2// BSD 3-Clause License
3//
4// Copyright (C) 2024-2026, Heriot-Watt University
5// Copyright note valid unless otherwise stated in individual files.
6// All rights reserved.
8
9#ifndef CROCODDYL_UTILS_CONVERSIONS_HPP_
10#define CROCODDYL_UTILS_CONVERSIONS_HPP_
11
12#include <memory>
13#include <vector>
14
15#ifdef CROCODDYL_WITH_CODEGEN
16#include <cppad/cg/support/cppadcg_eigen.hpp>
17#include <cppad/cppad.hpp>
18#endif
19
20#include "crocoddyl/core/mathbase.hpp"
21
22namespace crocoddyl {
23
24template <typename Scalar>
25struct ScalarSelector {
26 typedef typename std::conditional<std::is_floating_point<Scalar>::value,
27 Scalar, double>::type type;
28};
29
30// Casting between floating-point types
31template <typename NewScalar, typename Scalar>
32static typename std::enable_if<std::is_floating_point<NewScalar>::value &&
33 std::is_floating_point<Scalar>::value,
34 NewScalar>::type
35scalar_cast(const Scalar& x) {
36 return static_cast<NewScalar>(x);
37}
38
39#ifdef CROCODDYL_WITH_CODEGEN
40
41// Casting to CppAD types from floating-point types
42template <typename NewScalar, typename Scalar>
43static typename std::enable_if<
44 std::is_floating_point<Scalar>::value &&
45 (std::is_same<NewScalar, CppAD::AD<CppAD::cg::CG<double>>>::value ||
46 std::is_same<NewScalar, CppAD::AD<CppAD::cg::CG<float>>>::value),
47 NewScalar>::type
48scalar_cast(const Scalar& x) {
49 return static_cast<NewScalar>(x);
50}
51
52// Casting to floating-point types from CppAD types
53template <typename NewScalar, typename Scalar>
54static inline typename std::enable_if<std::is_floating_point<Scalar>::value,
55 NewScalar>::type
56scalar_cast(const CppAD::AD<CppAD::cg::CG<Scalar>>& x) {
57 return static_cast<NewScalar>(CppAD::Value(x).getValue());
58}
59
60#endif // CROCODDYL_WITH_CODEGEN
61
62template <typename NewScalar, typename Scalar>
63std::vector<NewScalar> vector_cast(const std::vector<Scalar>& in) {
64 std::vector<NewScalar> out;
65 out.reserve(in.size()); // Optimize allocation
66 for (const auto& obj : in) {
67 out.push_back(scalar_cast<NewScalar>(obj));
68 }
69 return out;
70}
71
72template <typename NewScalar, typename Scalar, int Rows, int Cols, int Options,
73 int MaxRows, int MaxCols>
74std::vector<Eigen::Matrix<NewScalar, Rows, Cols, Options, MaxRows, MaxCols>>
75vector_cast(const std::vector<
76 Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols>>& in) {
77 std::vector<Eigen::Matrix<NewScalar, Rows, Cols, Options, MaxRows, MaxCols>>
78 out;
79 out.reserve(in.size()); // Optimize allocation
80 for (const auto& obj : in) {
81 out.push_back(obj.template cast<NewScalar>());
82 }
83 return out;
84}
85
86template <typename NewScalar, typename Scalar,
87 template <typename> class ItemTpl>
88std::vector<ItemTpl<NewScalar>> vector_cast(
89 const std::vector<ItemTpl<Scalar>>& in) {
90 std::vector<ItemTpl<NewScalar>> out;
91 out.reserve(in.size()); // Optimize allocation
92 for (const auto& obj : in) {
93 out.push_back(obj.template cast<NewScalar>());
94 }
95 return out;
96}
97
98template <typename NewScalar, typename Scalar,
99 template <typename> class ItemTpl>
100std::vector<std::shared_ptr<ItemTpl<NewScalar>>> vector_cast(
101 const std::vector<std::shared_ptr<ItemTpl<Scalar>>>& in) {
102 std::vector<std::shared_ptr<ItemTpl<NewScalar>>> out;
103 out.reserve(in.size()); // Optimize allocation
104 for (const auto& obj : in) {
105 out.push_back(std::static_pointer_cast<ItemTpl<NewScalar>>(
106 obj->template cast<NewScalar>()));
107 }
108 return out;
109}
110
111template <typename Scalar, int Rows, int Cols, int Options, int MaxRows,
112 int MaxCols>
113Eigen::Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> matrix_random_cast(
114 const Eigen::Index rows, const Eigen::Index cols) {
115 using MatrixDouble =
116 Eigen::Matrix<double, Rows, Cols, Options, MaxRows, MaxCols>;
117 return MatrixDouble::Random(rows, cols).template cast<Scalar>();
118}
119
120template <typename Scalar, int Rows, int Options, int MaxRows>
121Eigen::Matrix<Scalar, Rows, 1, Options, MaxRows, 1> vector_random_cast(
122 const Eigen::Index size) {
123 using VectorDouble = Eigen::Matrix<double, Rows, 1, Options, MaxRows, 1>;
124 return VectorDouble::Random(size).template cast<Scalar>();
125}
126
127} // namespace crocoddyl
128
129#ifdef CROCODDYL_WITH_CODEGEN
130
131// Specialize Eigen's internal cast_impl for your specific types
132namespace Eigen {
133namespace internal {
134
135template <>
136struct cast_impl<CppAD::AD<CppAD::cg::CG<double>>, float> {
137 EIGEN_DEVICE_FUNC static inline float run(
138 const CppAD::AD<CppAD::cg::CG<double>>& x) {
139 // Perform the conversion. This example extracts the value from the AD type.
140 // You might need to adjust this depending on the specific implementation of
141 // CppAD::cg::CG<double>.
142 return static_cast<float>(CppAD::Value(x).getValue());
143 }
144};
145
146template <>
147struct cast_impl<CppAD::AD<CppAD::cg::CG<double>>, double> {
148 EIGEN_DEVICE_FUNC static inline double run(
149 const CppAD::AD<CppAD::cg::CG<double>>& x) {
150 return CppAD::Value(x).getValue();
151 }
152};
153
154template <>
155struct cast_impl<CppAD::AD<CppAD::cg::CG<float>>, float> {
156 EIGEN_DEVICE_FUNC static inline float run(
157 const CppAD::AD<CppAD::cg::CG<float>>& x) {
158 return CppAD::Value(x).getValue();
159 }
160};
161
162template <>
163struct cast_impl<CppAD::AD<CppAD::cg::CG<float>>, double> {
164 EIGEN_DEVICE_FUNC static inline double run(
165 const CppAD::AD<CppAD::cg::CG<float>>& x) {
166 // Perform the conversion. This example extracts the value from the AD type.
167 // You might need to adjust this depending on the specific implementation of
168 // CppAD::cg::CG<float>.
169 return static_cast<float>(CppAD::Value(x).getValue());
170 }
171};
172
173// Convert from CppAD::AD<CppAD::cg::CG<float>> to
174// CppAD::AD<CppAD::cg::CG<double>>
175template <>
176struct cast_impl<CppAD::AD<CppAD::cg::CG<float>>,
177 CppAD::AD<CppAD::cg::CG<double>>> {
178 EIGEN_DEVICE_FUNC static inline CppAD::AD<CppAD::cg::CG<double>> run(
179 const CppAD::AD<CppAD::cg::CG<float>>& x) {
180 return CppAD::AD<CppAD::cg::CG<double>>(
181 CppAD::cg::CG<double>(CppAD::Value(x).getValue()));
182 }
183};
184
185// Convert from CppAD::AD<CppAD::cg::CG<double>> to
186// CppAD::AD<CppAD::cg::CG<float>>
187template <>
188struct cast_impl<CppAD::AD<CppAD::cg::CG<double>>,
189 CppAD::AD<CppAD::cg::CG<float>>> {
190 EIGEN_DEVICE_FUNC static inline CppAD::AD<CppAD::cg::CG<float>> run(
191 const CppAD::AD<CppAD::cg::CG<double>>& x) {
192 return CppAD::AD<CppAD::cg::CG<float>>(
193 CppAD::cg::CG<float>(static_cast<float>(CppAD::Value(x).getValue())));
194 }
195};
196
197} // namespace internal
198} // namespace Eigen
199
200#endif
201
202#endif // CROCODDYL_UTILS_CONVERSIONS_HPP_