Set default values of empty Parameters correctly (epfl-lasa#298)

* Add virtual destructor for State * Fix constructor and ostream operator in Ellipsoid * Default construct value of parameters if not provided * Add empty constructor for parameter of ellipsoid * Update C++ tests for parameters * Add missing Shape and Ellipsoid constructors in Python * Default construct values of parameters in Python * Update Python parameter tests * Fix incorrect bracket * 6.0.1 -> 6.0.2 * Update CHANGELOG
aica-technology · Jun 24, 2022 · 3c6a242 · 3c6a242
1 parent 5a7e1fe
commit 3c6a242
Show file tree

Hide file tree

Showing 15 changed files with 348 additions and 121 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -15,6 +15,7 @@ Release Versions:
 ## Upcoming changes (in development)
 
 - Extend clproto tests (#297)
+- Set default values of empty Parameters correctly (#298)
 
 ## 6.0.0
 

diff --git a/VERSION b/VERSION
@@ -1 +1 @@
-6.0.1
+6.0.2
diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf
@@ -38,7 +38,7 @@ PROJECT_NAME           = "Control Libraries"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER = 6.0.1
+PROJECT_NUMBER = 6.0.2
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(clproto VERSION 6.0.1)
+project(clproto VERSION 6.0.2)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h
@@ -9,17 +9,17 @@
 namespace py_parameter {
 
 struct ParameterValues {
-  int int_value;
-  std::vector<int> int_array_value;
-  double double_value;
-  std::vector<double> double_array_value;
-  bool bool_value;
-  std::vector<bool> bool_array_value;
-  std::string string_value;
-  std::vector<std::string> string_array_value;
+  int int_value = int();
+  std::vector<int> int_array_value = std::vector<int>();
+  double double_value = double();
+  std::vector<double> double_array_value = std::vector<double>();
+  bool bool_value = bool();
+  std::vector<bool> bool_array_value= std::vector<bool>();
+  std::string string_value = std::string();
+  std::vector<std::string> string_array_value = std::vector<std::string>();
   std::shared_ptr<State> state_pointer;
-  Eigen::MatrixXd matrix_value;
-  Eigen::VectorXd vector_value;
+  Eigen::MatrixXd matrix_value = Eigen::MatrixXd();
+  Eigen::VectorXd vector_value = Eigen::VectorXd();
 };
 
 class ParameterContainer : public ParameterInterface {

diff --git a/python/setup.py b/python/setup.py
@@ -10,7 +10,7 @@
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR'
 
-__version__ = "6.0.1"
+__version__ = "6.0.2"
 __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model']
 __include_dirs__ = ['include']
 

diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp
@@ -10,7 +10,30 @@ namespace py_parameter {
 
 ParameterContainer::ParameterContainer(
     const std::string& name, const ParameterType& type, const StateType& parameter_state_type
-) : ParameterInterface(name, type, parameter_state_type) {}
+) : ParameterInterface(name, type, parameter_state_type) {
+  if (type == ParameterType::STATE) {
+    switch (parameter_state_type) {
+      case StateType::CARTESIAN_STATE:
+        values.state_pointer = std::make_shared<CartesianState>();
+        break;
+      case StateType::CARTESIAN_POSE:
+        values.state_pointer = std::make_shared<CartesianPose>();
+        break;
+      case StateType::JOINT_STATE:
+        values.state_pointer = std::make_shared<JointState>();
+        break;
+      case StateType::JOINT_POSITIONS:
+        values.state_pointer = std::make_shared<JointPositions>();
+        break;
+      case StateType::GEOMETRY_ELLIPSOID:
+        values.state_pointer = std::make_shared<Ellipsoid>();
+        break;
+      default:
+        throw std::invalid_argument("The desired StateType for parameter " + this->get_name() + " is not supported.");
+        break;
+    }
+  }
+}
 
 ParameterContainer::ParameterContainer(
     const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type

diff --git a/python/source/state_representation/bind_geometry.cpp b/python/source/state_representation/bind_geometry.cpp
@@ -8,6 +8,7 @@
 void shape(py::module_& m) {
   py::class_<Shape, std::shared_ptr<Shape>, State> c(m, "Shape");
 
+  c.def(py::init<const StateType&>(), "Constructor with a type", "type"_a);
   c.def(py::init<const StateType&, const std::string&, const std::string&>(), "Constructor with name but empty state.", "type"_a, "name"_a, "reference_frame"_a=std::string("world"));
   c.def(py::init<const Shape&>(), "Copy constructor from another Shape.", "shape"_a);
 
@@ -38,6 +39,7 @@ void shape(py::module_& m) {
 void ellipsoid(py::module_& m) {
   py::class_<Ellipsoid, std::shared_ptr<Ellipsoid>, Shape> c(m, "Ellipsoid");
 
+  c.def(py::init(), "Empty constructor.");
   c.def(py::init<const std::string&, const std::string&>(), "Constructor with name but empty state.", "name"_a, "reference_frame"_a=std::string("world"));
   c.def(py::init<const Ellipsoid&>(), "Copy constructor from another Ellipsoid.", "ellipsoid"_a);
 

diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py
@@ -42,6 +42,7 @@ def test_param_int(self):
         self.assertEqual(param.get_name(), "int")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), 0)
         param.set_value(1)
         self.assertEqual(param.get_value(), 1)
         param1 = sr.Parameter("int", 1, sr.ParameterType.INT)
@@ -53,6 +54,7 @@ def test_param_int_array(self):
         self.assertEqual(param.get_name(), "int_array")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT_ARRAY)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), [])
         values = [2, 3, 4]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
@@ -65,6 +67,7 @@ def test_param_double(self):
         self.assertEqual(param.get_name(), "double")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), 0.0)
         param.set_value(1.5)
         self.assertEqual(param.get_value(), 1.5)
         param1 = sr.Parameter("double", 1.5, sr.ParameterType.DOUBLE)
@@ -76,6 +79,7 @@ def test_param_double_array(self):
         self.assertEqual(param.get_name(), "double_array")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE_ARRAY)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), [])
         values = [2.2, 3.3, 4.4]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
@@ -88,17 +92,19 @@ def test_param_bool(self):
         self.assertEqual(param.get_name(), "bool")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
-        param.set_value(False)
         self.assertEqual(param.get_value(), False)
-        param1 = sr.Parameter("bool", False, sr.ParameterType.BOOL)
-        self.assertEqual(param1.get_value(), False)
+        param.set_value(True)
+        self.assertEqual(param.get_value(), True)
+        param1 = sr.Parameter("bool", True, sr.ParameterType.BOOL)
+        self.assertEqual(param1.get_value(), True)
 
     def test_param_bool_array(self):
         param = sr.Parameter("bool_array", sr.ParameterType.BOOL_ARRAY)
         self.assertTrue(param.is_empty())
         self.assertEqual(param.get_name(), "bool_array")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL_ARRAY)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), [])
         values = [True, False, False]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
@@ -111,6 +117,7 @@ def test_param_string(self):
         self.assertEqual(param.get_name(), "string")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), "")
         param.set_value("parameter")
         self.assertEqual(param.get_value(), "parameter")
         param1 = sr.Parameter("string", "parameter", sr.ParameterType.STRING)
@@ -122,6 +129,7 @@ def test_param_string_array(self):
         self.assertEqual(param.get_name(), "string_array")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING_ARRAY)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assertEqual(param.get_value(), [])
         values = ["test", "parameter", "bindings"]
         param.set_value(values)
         [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))]
@@ -134,6 +142,8 @@ def test_param_cartesian_state(self):
         self.assertEqual(param.get_name(), "cartesian_state")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_STATE)
+        self.assertTrue(param.get_value().is_empty())
+        self.assertEqual(param.get_value().get_type(), sr.StateType.CARTESIAN_STATE)
         values = sr.CartesianState.Random("test")
         param.set_value(values)
         self.cartesian_equal(param.get_value(), values)
@@ -146,6 +156,8 @@ def test_param_cartesian_pose(self):
         self.assertEqual(param.get_name(), "cartesian_pose")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_POSE)
+        self.assertTrue(param.get_value().is_empty())
+        self.assertEqual(param.get_value().get_type(), sr.StateType.CARTESIAN_POSE)
         values = sr.CartesianPose.Random("test")
         param.set_value(values)
         self.cartesian_equal(param.get_value(), values)
@@ -158,6 +170,8 @@ def test_param_joint_state(self):
         self.assertEqual(param.get_name(), "joint_state")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_STATE)
+        self.assertTrue(param.get_value().is_empty())
+        self.assertEqual(param.get_value().get_type(), sr.StateType.JOINT_STATE)
         values = sr.JointState.Random("test", 3)
         param.set_value(values)
         self.joint_equal(param.get_value(), values)
@@ -170,6 +184,9 @@ def test_param_joint_positions(self):
         self.assertEqual(param.get_name(), "joint_positions")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_POSITIONS)
+        print(param.get_value())
+        self.assertTrue(param.get_value().is_empty())
+        self.assertEqual(param.get_value().get_type(), sr.StateType.JOINT_POSITIONS)
         values = sr.JointPositions.Random("test", 3)
         param.set_value(values)
         self.joint_equal(param.get_value(), values)
@@ -182,6 +199,8 @@ def test_param_ellipsoid(self):
         self.assertEqual(param.get_name(), "ellipse")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.GEOMETRY_ELLIPSOID)
+        self.assertTrue(param.get_value().is_empty())
+        self.assertEqual(param.get_value().get_type(), sr.StateType.GEOMETRY_ELLIPSOID)
         values = sr.Ellipsoid("test")
         param.set_value(values)
         self.assertTrue(param.get_value().get_name(), values.get_name())
@@ -194,6 +213,7 @@ def test_param_matrix(self):
         self.assertEqual(param.get_name(), "matrix")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.MATRIX)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assert_np_array_equal(param.get_value(), np.empty((0, 0)))
         values = np.random.rand(3, 2)
         param.set_value(values)
         self.assert_np_array_equal(param.get_value(), values)
@@ -206,6 +226,7 @@ def test_param_vector(self):
         self.assertEqual(param.get_name(), "vector")
         self.assertEqual(param.get_parameter_type(), sr.ParameterType.VECTOR)
         self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE)
+        self.assert_np_array_equal(param.get_value(), np.empty(0))
         values = np.random.rand(3)
         param.set_value(values)
         self.assert_np_array_equal(param.get_value(), values)

diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.15)
 
-project(control_libraries VERSION 6.0.1)
+project(control_libraries VERSION 6.0.2)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp
@@ -48,6 +48,11 @@ class State : public std::enable_shared_from_this<State> {
    */
   State(const State& state);
 
+  /**
+   * @brief Virtual destructor
+   */
+  virtual ~State() = default;
+
   /**
    * @brief Swap the values of the two States
    * @param state1 State to be swapped with 2

diff --git a/source/state_representation/src/geometry/Ellipsoid.cpp b/source/state_representation/src/geometry/Ellipsoid.cpp
@@ -13,9 +13,7 @@ Ellipsoid::Ellipsoid(const std::string& name, const std::string& reference_frame
 }
 
 Ellipsoid::Ellipsoid(const Ellipsoid& ellipsoid) :
-    Shape(ellipsoid), axis_lengths_(ellipsoid.axis_lengths_), rotation_angle_(0) {
-  this->set_filled();
-}
+    Shape(ellipsoid), axis_lengths_(ellipsoid.axis_lengths_), rotation_angle_(0) {}
 
 const std::list<CartesianPose> Ellipsoid::sample_from_parameterization(unsigned int nb_samples) const {
   if (this->get_center_state().is_empty()) {
@@ -185,11 +183,15 @@ void Ellipsoid::set_data(const std::vector<double>& data) {
 }
 
 std::ostream& operator<<(std::ostream& os, const Ellipsoid& ellipsoid) {
-  os << "Ellipsoid " << ellipsoid.get_name() << " of dimensions [";
-  os << ellipsoid.get_axis_length(0) << ", ";
-  os << ellipsoid.get_axis_length(1) << "]";
-  os << " with state:" << std::endl;
-  os << ellipsoid.get_center_state();
+  if (ellipsoid.is_empty()) {
+    os << "Empty Ellipsoid";
+  } else {
+    os << "Ellipsoid " << ellipsoid.get_name() << " of dimensions [";
+    os << ellipsoid.get_axis_length(0) << ", ";
+    os << ellipsoid.get_axis_length(1) << "]";
+    os << " with state:" << std::endl;
+    os << ellipsoid.get_center_state();
+  }
   return os;
 }
 }
diff --git a/source/state_representation/src/geometry/Shape.cpp b/source/state_representation/src/geometry/Shape.cpp
@@ -10,8 +10,12 @@ Shape::Shape(const StateType& type, const std::string& name, const std::string&
 Shape::Shape(const Shape& shape) : State(shape), center_state_(shape.center_state_) {}
 
 std::ostream& operator<<(std::ostream& os, const Shape& shape) {
-  os << "Shape " << shape.get_name() << " with state:" << std::endl;
-  os << shape.get_center_state();
+  if (shape.is_empty()) {
+    os << "Empty Shape";
+  } else {
+    os << "Shape " << shape.get_name() << " with state:" << std::endl;
+    os << shape.get_center_state();
+  }
   return os;
 }
 }