diff --git a/python/include/parameter_container.hpp b/python/include/parameter_container.hpp index 3b777c471..f4f57f637 100644 --- a/python/include/parameter_container.hpp +++ b/python/include/parameter_container.hpp @@ -67,4 +67,6 @@ interface_ptr_to_container_list(const std::list> container_to_interface_ptr_list(const std::list& parameters); +void copy_parameter_value(const ParameterContainer& source_parameter, ParameterContainer& target_parameter); + }// namespace py_parameter \ No newline at end of file diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp index 708af9bab..510ea55ce 100644 --- a/python/source/common/parameter_container.cpp +++ b/python/source/common/parameter_container.cpp @@ -1,5 +1,6 @@ #include "parameter_container.hpp" +#include #include #include #include @@ -333,4 +334,56 @@ container_to_interface_ptr_list(const std::list& parameters) } return parameter_list; } + +void copy_parameter_value(const ParameterContainer& source_parameter, ParameterContainer& target_parameter) { + if (target_parameter.get_parameter_type() != source_parameter.get_parameter_type()) { + throw exceptions::IncompatibleStatesException( + "Source parameter " + source_parameter.get_name() + + " to be copied does not have the same type as target parameter " + target_parameter.get_name() + "(" + + get_parameter_type_name(source_parameter.get_parameter_type()) + " vs. " + + get_parameter_type_name(target_parameter.get_parameter_type()) + ")" + ); + } + switch (target_parameter.get_parameter_type()) { + case ParameterType::BOOL: + case ParameterType::BOOL_ARRAY: + case ParameterType::INT: + case ParameterType::INT_ARRAY: + case ParameterType::DOUBLE: + case ParameterType::DOUBLE_ARRAY: + case ParameterType::STRING: + case ParameterType::STRING_ARRAY: + case ParameterType::VECTOR: + case ParameterType::MATRIX: + target_parameter.set_value(source_parameter.get_value()); + return; + case ParameterType::STATE: + if (target_parameter.get_parameter_state_type() != source_parameter.get_parameter_state_type()) { + throw exceptions::IncompatibleStatesException( + "Source parameter " + source_parameter.get_name() + + " to be copied does not have the same parameter state type as target parameter " + + target_parameter.get_name() + "(" + get_state_type_name(source_parameter.get_parameter_state_type()) + + " vs. " + get_state_type_name(target_parameter.get_parameter_state_type()) + ")" + ); + } + switch (target_parameter.get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + case StateType::CARTESIAN_POSE: + case StateType::JOINT_STATE: + case StateType::JOINT_POSITIONS: + case StateType::GEOMETRY_ELLIPSOID: + target_parameter.set_value(source_parameter.get_value()); + return; + default: + break; + } + break; + default: + break; + } + throw exceptions::IncompatibleStatesException( + "Could not copy the value from source parameter " + source_parameter.get_name() + " into target parameter " + + target_parameter.get_name() + ); +} }// namespace py_parameter diff --git a/python/source/state_representation/bind_parameters.cpp b/python/source/state_representation/bind_parameters.cpp index 13a725e09..73e3a6cae 100644 --- a/python/source/state_representation/bind_parameters.cpp +++ b/python/source/state_representation/bind_parameters.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -125,23 +126,25 @@ void parameter(py::module_& m) { } return buffer.str(); }); + + m.def("copy_parameter_value", &py_parameter::copy_parameter_value, "Copy the value from one parameter to another", "source_parameter"_a, "target_parameter"_a); } void parameter_map(py::module_& m) { py::class_, PyParameterMap> c(m, "ParameterMap"); c.def(py::init(), "Empty constructor"); + c.def( + py::init([](const std::list& parameters) { + auto parameter_list = container_to_interface_ptr_list(parameters); + return ParameterMap(parameter_list); + }), "Construct the parameter map with an initial list of parameters", "parameters"_a); c.def( py::init([](const std::map& parameters) { auto parameter_map = container_to_interface_ptr_map(parameters); return ParameterMap(parameter_map); - }), "Construct the parameter map with an initial list of parameters", "parameters"_a + }), "Construct the parameter map with an initial map of parameters", "parameters"_a ); - c.def( - py::init([](const std::list& parameters) { - auto parameter_list = container_to_interface_ptr_list(parameters); - return ParameterMap(parameter_list); - }), "Construct the parameter map with an initial map of parameters", "parameters"_a); c.def( "get_parameter", [](ParameterMap& self, const std::string& name) -> ParameterContainer { @@ -182,9 +185,27 @@ void parameter_map(py::module_& m) { c.def("remove_parameter", &ParameterMap::remove_parameter, "Remove a parameter from the parameter map.", "name"_a); } +void strict_parameter_map(py::module_& m) { + py::class_, ParameterMap> c(m, "StrictParameterMap"); + + c.def(py::init(), "Empty constructor"); + c.def( + py::init([](const std::list& parameters) { + auto parameter_list = container_to_interface_ptr_list(parameters); + return StrictParameterMap(parameter_list); + }), "Construct the parameter map with an initial list of parameters", "parameters"_a + ); + c.def( + py::init([](const std::map& parameters) { + auto parameter_map = container_to_interface_ptr_map(parameters); + return StrictParameterMap(parameter_map); + }), "Construct the parameter map with an initial map of parameters", "parameters"_a); +} + void bind_parameters(py::module_& m) { parameter_type(m); parameter_interface(m); parameter(m); parameter_map(m); + strict_parameter_map(m); } \ No newline at end of file diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py index a8afee0cb..2e72ed1ea 100755 --- a/python/test/state_representation/test_parameters.py +++ b/python/test/state_representation/test_parameters.py @@ -2,6 +2,7 @@ import numpy as np import state_representation as sr +from state_representation import copy_parameter_value from numpy.testing import assert_array_almost_equal from ..conftest import Helpers @@ -73,6 +74,11 @@ def test_parameter_construction(name, value, parameter_type, state_type, test_fu assert param.is_empty() with pytest.raises(sr.exceptions.EmptyStateError): param.get_value() + with pytest.raises(sr.exceptions.EmptyStateError): + copy_parameter_value(param, new_param) + + copy_parameter_value(param1, param) + test_func(value, param.get_value()) def param_map_equal(param_dict, param_map): @@ -131,3 +137,11 @@ def test_param_map(): m.remove_parameter("int") with pytest.raises(sr.exceptions.InvalidParameterError): m.get_parameter("int") + +def test_strict_param_map(): + param_map = sr.StrictParameterMap() + param_map.set_parameter(sr.Parameter("int", 1, sr.ParameterType.INT)) + param_map.set_parameter("int", 2, sr.ParameterType.INT) + assert_value_equal(param_map.get_parameter_value("int"), 2) + with pytest.raises(sr.exceptions.InvalidParameterError): + param_map.set_parameter(sr.Parameter("int", 1.0, sr.ParameterType.DOUBLE)) diff --git a/source/dynamical_systems/src/PointAttractor.cpp b/source/dynamical_systems/src/PointAttractor.cpp index 588113b8f..0f5d38a25 100644 --- a/source/dynamical_systems/src/PointAttractor.cpp +++ b/source/dynamical_systems/src/PointAttractor.cpp @@ -169,6 +169,8 @@ void PointAttractor::validate_and_set_parameter(const std::shared_pt this->set_attractor(parameter->get_parameter_value()); } else if (parameter->get_parameter_state_type() == StateType::JOINT_POSITIONS) { this->set_attractor(parameter->get_parameter_value()); + } else { + throw state_representation::exceptions::InvalidParameterException("Parameter 'attractor' has incorrect type"); } } else if (parameter->get_name() == "gain") { this->set_gain(parameter, this->attractor_->get_value().get_size());