Control Libraries 6.3.4
Loading...
Searching...
No Matches
parameter_container.cpp
1#include "parameter_container.h"
2
3#include <state_representation/space/cartesian/CartesianState.hpp>
4#include <state_representation/space/cartesian/CartesianPose.hpp>
5#include <state_representation/space/joint/JointState.hpp>
6#include <state_representation/space/joint/JointPositions.hpp>
7#include <state_representation/geometry/Ellipsoid.hpp>
8
9namespace py_parameter {
10
11ParameterContainer::ParameterContainer(
12 const std::string& name, const ParameterType& type, const StateType& parameter_state_type
13) : ParameterInterface(name, type, parameter_state_type) {
14 if (type == ParameterType::STATE) {
15 switch (parameter_state_type) {
16 case StateType::CARTESIAN_STATE:
17 values.state_pointer = std::make_shared<CartesianState>();
18 break;
19 case StateType::CARTESIAN_POSE:
20 values.state_pointer = std::make_shared<CartesianPose>();
21 break;
22 case StateType::JOINT_STATE:
23 values.state_pointer = std::make_shared<JointState>();
24 break;
25 case StateType::JOINT_POSITIONS:
26 values.state_pointer = std::make_shared<JointPositions>();
27 break;
28 case StateType::GEOMETRY_ELLIPSOID:
29 values.state_pointer = std::make_shared<Ellipsoid>();
30 break;
31 default:
32 throw std::invalid_argument("The desired StateType for parameter " + this->get_name() + " is not supported.");
33 break;
34 }
35 }
36}
37
38ParameterContainer::ParameterContainer(
39 const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type
40) : ParameterInterface(name, type, parameter_state_type) {
41 set_value(value);
42}
43
44ParameterContainer::ParameterContainer(const ParameterContainer& parameter) :
45 ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()),
46 values(parameter.values) {}
47
48void ParameterContainer::set_value(const py::object& value) {
49 switch (this->get_parameter_type()) {
50 case ParameterType::INT:
51 values.int_value = value.cast<int>();
52 break;
53 case ParameterType::INT_ARRAY:
54 values.int_array_value = value.cast<std::vector<int>>();
55 break;
56 case ParameterType::DOUBLE:
57 values.double_value = value.cast<double>();
58 break;
59 case ParameterType::DOUBLE_ARRAY:
60 values.double_array_value = value.cast<std::vector<double>>();
61 break;
62 case ParameterType::BOOL:
63 values.bool_value = value.cast<bool>();
64 break;
65 case ParameterType::BOOL_ARRAY:
66 values.bool_array_value = value.cast<std::vector<bool>>();
67 break;
68 case ParameterType::STRING:
69 values.string_value = value.cast<std::string>();
70 break;
71 case ParameterType::STRING_ARRAY:
72 values.string_array_value = value.cast<std::vector<std::string>>();
73 break;
74 case ParameterType::STATE:
75 switch (this->get_parameter_state_type()) {
76 case StateType::CARTESIAN_STATE:
77 values.state_pointer = std::make_shared<CartesianState>(value.cast<CartesianState>());
78 break;
79 case StateType::CARTESIAN_POSE:
80 values.state_pointer = std::make_shared<CartesianPose>(value.cast<CartesianPose>());
81 break;
82 case StateType::JOINT_STATE:
83 values.state_pointer = std::make_shared<JointState>(value.cast<JointState>());
84 break;
85 case StateType::JOINT_POSITIONS:
86 values.state_pointer = std::make_shared<JointPositions>(value.cast<JointPositions>());
87 break;
88 case StateType::GEOMETRY_ELLIPSOID:
89 values.state_pointer = std::make_shared<Ellipsoid>(value.cast<Ellipsoid>());
90 break;
91 default:
92 throw std::invalid_argument("The StateType contained by parameter " + this->get_name() + " is unsupported.");
93 break;
94 }
95 break;
96 case ParameterType::MATRIX:
97 values.matrix_value = value.cast<Eigen::MatrixXd>();
98 break;
99 case ParameterType::VECTOR:
100 values.vector_value = value.cast<Eigen::VectorXd>();
101 break;
102 default:
103 throw std::invalid_argument("The ParameterType of parameter " + this->get_name() + " is invalid.");
104 }
105 this->set_filled();
106}
107
108py::object ParameterContainer::get_value() {
109 switch (this->get_parameter_type()) {
110 case ParameterType::INT:
111 return py::cast(values.int_value);
112 case ParameterType::INT_ARRAY:
113 return py::cast(values.int_array_value);
114 case ParameterType::DOUBLE:
115 return py::cast(values.double_value);
116 case ParameterType::DOUBLE_ARRAY:
117 return py::cast(values.double_array_value);
118 case ParameterType::BOOL:
119 return py::cast(values.bool_value);
120 case ParameterType::BOOL_ARRAY:
121 return py::cast(values.bool_array_value);
122 case ParameterType::STRING:
123 return py::cast(values.string_value);
124 case ParameterType::STRING_ARRAY:
125 return py::cast(values.string_array_value);
126 case ParameterType::STATE:
127 try {
128 switch (this->get_parameter_state_type()) {
129 case StateType::CARTESIAN_STATE:
130 return py::cast(*std::dynamic_pointer_cast<CartesianState>(values.state_pointer));
131 case StateType::CARTESIAN_POSE:
132 return py::cast(*std::dynamic_pointer_cast<CartesianPose>(values.state_pointer));
133 case StateType::JOINT_STATE:
134 return py::cast(*std::dynamic_pointer_cast<JointState>(values.state_pointer));
135 case StateType::JOINT_POSITIONS:
136 return py::cast(*std::dynamic_pointer_cast<JointPositions>(values.state_pointer));
137 case StateType::GEOMETRY_ELLIPSOID:
138 return py::cast(*std::dynamic_pointer_cast<Ellipsoid>(values.state_pointer));
139 default:
140 throw std::invalid_argument(
141 "The StateType contained by parameter " + this->get_name() + " is unsupported.");
142 }
143 } catch (const std::exception&) {
144 throw std::runtime_error("The ParameterType of parameter " + this->get_name() + " is invalid.");
145 }
146 break;
147 case ParameterType::MATRIX:
148 return py::cast(values.matrix_value);
149 case ParameterType::VECTOR:
150 return py::cast(values.vector_value);
151 default:
152 break;
153 }
154 throw std::invalid_argument("Could not get the value of parameter " + this->get_name());
155}
156
157ParameterContainer interface_ptr_to_container(const std::shared_ptr<ParameterInterface>& parameter) {
158 switch (parameter->get_parameter_type()) {
159 case ParameterType::INT:
160 return ParameterContainer(
161 parameter->get_name(), py::cast(parameter->get_parameter_value<int>()), ParameterType::INT);
162 case ParameterType::INT_ARRAY:
163 return ParameterContainer(
164 parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<int>>()),
165 ParameterType::INT_ARRAY);
166 case ParameterType::DOUBLE:
167 return ParameterContainer(
168 parameter->get_name(), py::cast(parameter->get_parameter_value<double>()), ParameterType::DOUBLE);
169 case ParameterType::DOUBLE_ARRAY:
170 return ParameterContainer(
171 parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<double>>()),
172 ParameterType::DOUBLE_ARRAY);
173 case ParameterType::BOOL:
174 return ParameterContainer(
175 parameter->get_name(), py::cast(parameter->get_parameter_value<bool>()), ParameterType::BOOL);
176 case ParameterType::BOOL_ARRAY:
177 return ParameterContainer(
178 parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<bool>>()),
179 ParameterType::BOOL_ARRAY);
180 case ParameterType::STRING:
181 return ParameterContainer(
182 parameter->get_name(), py::cast(parameter->get_parameter_value<std::string>()), ParameterType::STRING);
183 case ParameterType::STRING_ARRAY:
184 return ParameterContainer(
185 parameter->get_name(), py::cast(parameter->get_parameter_value<std::vector<std::string>>()),
186 ParameterType::STRING_ARRAY);
187 case ParameterType::STATE:
188 try {
189 switch (parameter->get_parameter_state_type()) {
190 case StateType::CARTESIAN_STATE:
191 return ParameterContainer(
192 parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianState>()), ParameterType::STATE,
193 StateType::CARTESIAN_STATE);
194 case StateType::CARTESIAN_POSE:
195 return ParameterContainer(
196 parameter->get_name(), py::cast(parameter->get_parameter_value<CartesianPose>()), ParameterType::STATE,
197 StateType::CARTESIAN_POSE);
198 case StateType::JOINT_STATE:
199 return ParameterContainer(
200 parameter->get_name(), py::cast(parameter->get_parameter_value<JointState>()), ParameterType::STATE,
201 StateType::JOINT_STATE);
202 case StateType::JOINT_POSITIONS:
203 return ParameterContainer(
204 parameter->get_name(), py::cast(parameter->get_parameter_value<JointPositions>()), ParameterType::STATE,
205 StateType::JOINT_POSITIONS);
206 case StateType::GEOMETRY_ELLIPSOID:
207 return ParameterContainer(
208 parameter->get_name(), py::cast(parameter->get_parameter_value<Ellipsoid>()), ParameterType::STATE,
209 StateType::GEOMETRY_ELLIPSOID);
210 default:
211 throw std::invalid_argument(
212 "The StateType contained by parameter " + parameter->get_name() + " is unsupported.");
213 }
214 } catch (const std::exception&) {
215 throw std::runtime_error("The ParameterType of parameter " + parameter->get_name() + " is invalid.");
216 }
217 break;
218 case ParameterType::MATRIX:
219 return ParameterContainer(
220 parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::MatrixXd>()), ParameterType::MATRIX);
221 case ParameterType::VECTOR:
222 return ParameterContainer(
223 parameter->get_name(), py::cast(parameter->get_parameter_value<Eigen::VectorXd>()), ParameterType::VECTOR);
224 default:
225 break;
226 }
227 throw std::invalid_argument("The conversion from this Parameter to a ParameterContainer is not supported.");
228}
229
230std::shared_ptr<ParameterInterface> container_to_interface_ptr(const ParameterContainer& parameter) {
231 switch (parameter.get_parameter_type()) {
232 case ParameterType::INT:
233 return make_shared_parameter(parameter.get_name(), parameter.values.int_value);
234 case ParameterType::INT_ARRAY:
235 return make_shared_parameter(parameter.get_name(), parameter.values.int_array_value);
236 case ParameterType::DOUBLE:
237 return make_shared_parameter(parameter.get_name(), parameter.values.double_value);
238 case ParameterType::DOUBLE_ARRAY:
239 return make_shared_parameter(parameter.get_name(), parameter.values.double_array_value);
240 case ParameterType::BOOL:
241 return make_shared_parameter(parameter.get_name(), parameter.values.bool_value);
242 case ParameterType::BOOL_ARRAY:
243 return make_shared_parameter(parameter.get_name(), parameter.values.bool_array_value);
244 case ParameterType::STRING:
245 return make_shared_parameter(parameter.get_name(), parameter.values.string_value);
246 case ParameterType::STRING_ARRAY:
247 return make_shared_parameter(parameter.get_name(), parameter.values.string_array_value);
248 case ParameterType::STATE:
249 try {
250 switch (parameter.get_parameter_state_type()) {
251 case StateType::CARTESIAN_STATE:
252 return make_shared_parameter(
253 parameter.get_name(), *std::dynamic_pointer_cast<CartesianState>(parameter.values.state_pointer));
254 case StateType::CARTESIAN_POSE:
255 return make_shared_parameter(
256 parameter.get_name(), *std::dynamic_pointer_cast<CartesianPose>(parameter.values.state_pointer));
257 case StateType::JOINT_STATE:
258 return make_shared_parameter(
259 parameter.get_name(), *std::dynamic_pointer_cast<JointState>(parameter.values.state_pointer));
260 case StateType::JOINT_POSITIONS:
261 return make_shared_parameter(
262 parameter.get_name(), *std::dynamic_pointer_cast<JointPositions>(parameter.values.state_pointer));
263 case StateType::GEOMETRY_ELLIPSOID:
264 return make_shared_parameter(
265 parameter.get_name(), *std::dynamic_pointer_cast<Ellipsoid>(parameter.values.state_pointer));
266 default:
267 throw std::invalid_argument(
268 "The StateType contained by parameter " + parameter.get_name() + " is unsupported.");
269 }
270 } catch (const std::exception&) {
271 throw std::runtime_error("The ParameterType of parameter " + parameter.get_name() + " is invalid.");
272 }
273 break;
274 case ParameterType::MATRIX:
275 return make_shared_parameter(parameter.get_name(), parameter.values.matrix_value);
276 case ParameterType::VECTOR:
277 return make_shared_parameter(parameter.get_name(), parameter.values.vector_value);
278 default:
279 break;
280 }
281 throw std::invalid_argument("The conversion from this ParameterContainer to a Parameter is not supported.");
282}
283
284std::map<std::string, ParameterContainer>
285interface_ptr_to_container_map(const std::map<std::string, std::shared_ptr<ParameterInterface>>& parameters) {
286 std::map<std::string, ParameterContainer> parameter_list;
287 for (const auto& param_it: parameters) {
288 parameter_list.insert(
289 std::pair<std::string, ParameterContainer>(param_it.first, interface_ptr_to_container(param_it.second)));
290 }
291 return parameter_list;
292}
293
294std::map<std::string, std::shared_ptr<ParameterInterface>>
295container_to_interface_ptr_map(const std::map<std::string, ParameterContainer>& parameters) {
296 std::map<std::string, std::shared_ptr<ParameterInterface>> parameter_list;
297 for (const auto& param_it: parameters) {
298 parameter_list.insert(
299 std::pair<std::string, std::shared_ptr<ParameterInterface>>(
300 param_it.first, container_to_interface_ptr(param_it.second)
301 )
302 );
303 }
304 return parameter_list;
305}
306
307std::list<ParameterContainer>
308interface_ptr_to_container_list(const std::list<std::shared_ptr<ParameterInterface>>& parameters) {
309 std::list<ParameterContainer> parameter_list;
310 for (const auto& param_it: parameters) {
311 parameter_list.emplace_back(interface_ptr_to_container(param_it));
312 }
313 return parameter_list;
314}
315
316std::list<std::shared_ptr<ParameterInterface>>
317container_to_interface_ptr_list(const std::list<ParameterContainer>& parameters) {
318 std::list<std::shared_ptr<ParameterInterface>> parameter_list;
319 for (const auto& param_it: parameters) {
320 parameter_list.emplace_back(container_to_interface_ptr(param_it));
321 }
322 return parameter_list;
323}
324}// namespace py_parameter
state_representation::CartesianPose
Class to define CartesianPose in cartesian space as 3D position and quaternion based orientation.
Definition: CartesianPose.hpp:17
state_representation::CartesianState
Class to represent a state in Cartesian space.
Definition: CartesianState.hpp:47
state_representation::Ellipsoid
Definition: Ellipsoid.hpp:15
state_representation::JointPositions
Class to define a positions of the joints.
Definition: JointPositions.hpp:13
state_representation::JointState
Class to define a state in joint space.
Definition: JointState.hpp:36
state_representation::ParameterInterface
Definition: ParameterInterface.hpp:16
state_representation::ParameterType
ParameterType
The parameter value types.
Definition: ParameterType.hpp:9
state_representation::StateType
StateType
The class types inheriting from State.
Definition: StateType.hpp:13
Generated by doxygen 1.9.5