-
Notifications
You must be signed in to change notification settings - Fork 1.2k
/
system_scalar_converter.h
251 lines (222 loc) · 10.2 KB
/
system_scalar_converter.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
#pragma once
#include <functional>
#include <memory>
#include <typeindex>
#include <typeinfo>
#include <unordered_map>
#include <utility>
#include "drake/common/autodiff.h"
#include "drake/common/drake_assert.h"
#include "drake/common/drake_copyable.h"
#include "drake/common/symbolic/expression.h"
#include "drake/systems/framework/scalar_conversion_traits.h"
#include "drake/systems/framework/system_type_tag.h"
namespace drake {
namespace systems {
#if !defined(DRAKE_DOXYGEN_CXX)
// We need a bunch of forward declarations here.
template <typename T> class System;
class SystemScalarConverter;
namespace system_scalar_converter_internal {
template <typename T, typename U>
void AddPydrakeConverterFunction(
SystemScalarConverter*, const std::function<System<T>*(const System<U>&)>&);
} // namespace system_scalar_converter_internal
#endif
/// Helper class to convert a System<U> into a System<T>, intended for internal
/// use by the System framework, not directly by users.
///
/// For user-facing documentation see @ref system_scalar_conversion.
///
/// Because it is not templated on a System subclass, this class can be used by
/// LeafSystem without any direct knowledge of the subtypes being converted.
/// In other words, it enables a runtime flavor of the CRTP.
///
/// Throughout this class, the following template naming convention applies:
///
/// @tparam S is the System subclass that this object will convert from and to.
/// @tparam U the source scalar type (to convert from), which must be one of
/// the @ref default_scalars "default scalars".
/// @tparam T the resulting scalar type (to convert into), which must be one of
/// the @ref default_scalars "default scalars".
///
/// @note Conversions where `T` and `U` types are the same are not supported.
/// Template functions such as IsConvertible<T, U>() are still callable, but
/// will always return false, null, etc.
class SystemScalarConverter {
public:
DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(SystemScalarConverter);
/// (Advanced) Creates a converter that supports no conversions. The single-
/// argument constructor below is the overload intended for users.
SystemScalarConverter();
/// Creates a converter that uses S's scalar-converting copy constructor to
/// perform system scalar conversion. That constructor takes the form of:
///
/// @code
/// template <typename T>
/// class FooSystem final : public LeafSystem<T> {
/// template <typename U>
/// explicit FooSystem(const FooSystem<U>& other);
/// };
/// @endcode
///
/// By default, the converter supports conversions to and from all of the
/// @ref default_scalars "default scalars", but systems may specialize the
/// scalar_conversion::Traits to disable support for some or all of these
/// conversions. Conversions where `T` and `U` types are the same are not
/// supported.
///
/// This an implicit conversion constructor (not marked `explicit`), in order
/// to make calling code substantially more readable, with relatively little
/// risk of an unwanted accidental conversion happening.
///
/// See @ref system_scalar_conversion for additional overview documentation.
template <template <typename> class S>
// NOLINTNEXTLINE(runtime/explicit)
SystemScalarConverter(SystemTypeTag<S>) {
AddConstructors<true, S>();
}
/// (Advanced) Creates a converter similar to the single-argument constructor,
/// with the built-in checks for guaranteed subtype preservation of the System
/// turned off. In general, subtype preservation is an important invariant of
/// scalar conversion, so be extremely cautious about disabling it.
template <template <typename> class S>
static SystemScalarConverter MakeWithoutSubtypeChecking() {
SystemScalarConverter result;
result.AddConstructors<false, S>();
return result;
}
/// Returns true iff no conversions are supported. (In other words, whether
/// this is a default-constructed object.)
bool empty() const { return funcs_.empty(); }
/// Removes from this converter all pairs where `other.IsConvertible<T, U>`
/// is false. The subtype `S` need not be the same between this and `other`.
void RemoveUnlessAlsoSupportedBy(const SystemScalarConverter& other);
/// Removes from this converter the ability to convert from System<U> to
/// System<T>.
template <typename T, typename U>
void Remove();
/// Returns true iff this object can convert a System<U> into a System<T>,
/// i.e., whether Convert() will return non-null.
template <typename T, typename U>
bool IsConvertible() const;
/// Returns true iff this object can convert a System<U> into a System<T>,
/// i.e., whether Convert() will return non-null.
bool IsConvertible(
const std::type_info& t_info,
const std::type_info& u_info) const;
/// Converts a System<U> into a System<T>. This is the API that LeafSystem
/// uses to provide a default implementation of DoToAutoDiffXd, etc.
/// Returns null when IsConvertible() is false.
template <typename T, typename U>
std::unique_ptr<System<T>> Convert(const System<U>& other) const;
private:
// Allow the pydrake helper to call Insert().
template <typename T, typename U>
friend void system_scalar_converter_internal::AddPydrakeConverterFunction(
SystemScalarConverter*,
const std::function<System<T>*(const System<U>&)>&);
// Like ConverterFunc, but with the args and return value decayed into void*.
using ErasedConverterFunc = std::function<void*(const void*)>;
// A pair of types {T, U}, usable as an unordered_map key.
struct Key : std::pair<std::type_index, std::type_index> {
Key(const std::type_info&, const std::type_info&);
};
struct KeyHasher {
size_t operator()(const Key&) const;
};
// Calls MaybeAddConstructor for all supported pairs of scalar types.
template <bool subtype_preservation, template <typename> class S>
void AddConstructors() {
// N.B. When changing the pairs of supported types below, be sure to also
// change the `ConversionPairs` type pack in `DefineFrameworkPySystems`
// and the factory function immediately below.
using Expression = symbolic::Expression;
// From double to all other types.
MaybeAddConstructor<subtype_preservation, S, AutoDiffXd, double>();
MaybeAddConstructor<subtype_preservation, S, Expression, double>();
// From AutoDiffXd to all other types.
MaybeAddConstructor<subtype_preservation, S, double, AutoDiffXd>();
MaybeAddConstructor<subtype_preservation, S, Expression, AutoDiffXd>();
// From Expression to all other types.
MaybeAddConstructor<subtype_preservation, S, double, Expression>();
MaybeAddConstructor<subtype_preservation, S, AutoDiffXd, Expression>();
}
// Adds a converter for an S<U> into an S<T> using S's scalar-converting copy
// constructor, unless the traits have disabled the conversion.
template <bool subtype_preservation,
template <typename> class S, typename T, typename U>
void MaybeAddConstructor();
// Given typeid(T), typeid(U), returns a converter. If no converter has been
// added yet, returns nullptr.
const ErasedConverterFunc* Find(
const std::type_info&, const std::type_info&) const;
// Given typeid(T), typeid(U), adds a converter.
void Insert(
const std::type_info&, const std::type_info&,
const ErasedConverterFunc&);
// Maps from {T, U} to the function that converts from U into T.
std::unordered_map<Key, ErasedConverterFunc, KeyHasher> funcs_;
};
#if !defined(DRAKE_DOXYGEN_CXX)
template <typename T, typename U>
std::unique_ptr<System<T>> SystemScalarConverter::Convert(
const System<U>& other) const {
// Lookup the lambda that Add() stored and call it.
System<T>* result = nullptr;
const ErasedConverterFunc* converter = Find(typeid(T), typeid(U));
if (converter) {
result = static_cast<System<T>*>((*converter)(&other));
}
return std::unique_ptr<System<T>>(result);
}
namespace system_scalar_converter_internal {
// Throws an exception that `other` cannot be converted from S<U> to S<T>.
[[noreturn]] void ThrowConversionMismatch(
const std::type_info& s_t_info,
const std::type_info& s_u_info,
const std::type_info& other_info);
// N.B. This logic should be reflected in `TemplateSystem._make` in the file
// `scalar_conversion.py`.
template <bool subtype_preservation,
template <typename> class S, typename T, typename U>
static std::unique_ptr<System<T>> Make(const System<U>& other) {
// We conditionally require that system scalar conversion maintain the exact
// system type. Fail fast if `other` is not of exact type S<U>.
if (subtype_preservation &&
(std::type_index{typeid(other)} != std::type_index{typeid(S<U>)})) {
ThrowConversionMismatch(typeid(S<T>), typeid(S<U>), typeid(other));
}
const S<U>& my_other = dynamic_cast<const S<U>&>(other);
auto result = std::make_unique<S<T>>(my_other);
// We manually propagate the name from the old System to the new. The name
// is the only extrinsic property of the System and LeafSystem base classes
// that is stored within the System itself.
result->set_name(other.get_name());
return result;
}
} // namespace system_scalar_converter_internal
template <bool subtype_preservation,
template <typename> class S, typename T, typename U>
void SystemScalarConverter::MaybeAddConstructor() {
using Traits = typename scalar_conversion::Traits<S>;
if constexpr (Traits::template supported<T, U>::value) {
// The lambda is typed as `void* => void*` in order to have a non-templated
// signature and thus fit into a homogeneously-typed std::unordered_map.
auto func = [](const void* const other_system_u) -> void* {
DRAKE_ASSERT(other_system_u != nullptr);
const System<U>& other = *static_cast<const System<U>*>(other_system_u);
// Dispatch to an overload based on whether S<U> ==> S<T> is supported.
// (At runtime, this block is only executed for supported conversions,
// but at compile time, Make will be instantiated unconditionally.)
std::unique_ptr<System<T>> result =
system_scalar_converter_internal::
Make<subtype_preservation, S, T, U>(other);
return result.release();
};
Insert(typeid(T), typeid(U), func);
}
}
#endif // DRAKE_DOXYGEN_CXX
} // namespace systems
} // namespace drake