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