Ignore data fields when copying an empty state (#71)

* Add resize helper to JointState * Resizing in initialize is not necessary * Avoid using virtual functions in JointState constructor * Improve JointState copy constructor Only set the state variables if the state is not empty * Update copy constructors in derived joint states * Reduce Clion warnings and add FIXME * Improve JointState copy constructor And only set state variables if state is not empty * Update copy constructor of derived Cartesian states * Test copy construction of parameters * Fix copy construction of ParameterContainer * Fix ParameterContainer translation helpers * Test copy construction of Python parameters * Add FIXMEs * 6.3.42 -> 6.3.43 * Update CHANGELOG * Update FIXMEs
aica-technology · Feb 8, 2023 · b10bf07 · b10bf07
1 parent abec0d2
commit b10bf07
Show file tree

Hide file tree

Showing 24 changed files with 107 additions and 76 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -37,6 +37,7 @@ Release Versions:
 - Bind operators of joint states (#65)
 - Add initialize method to Parameter class (#68)
 - Allow Parameter construction with empty name (#67)
+- Ignore data fields when copying an empty state (#71)
 
 ## 6.3.1
 

diff --git a/VERSION b/VERSION
@@ -1 +1 @@
-6.3.42
+6.3.43
diff --git a/demos/CMakeLists.txt b/demos/CMakeLists.txt
@@ -15,7 +15,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-find_package(control_libraries 6.3.42 CONFIG REQUIRED)
+find_package(control_libraries 6.3.43 CONFIG REQUIRED)
 
 set(DEMOS_SCRIPTS
   task_space_control_loop

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.3.42
+PROJECT_NUMBER = 6.3.43
 
 # 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.3.42)
+project(clproto VERSION 6.3.43)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)

diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h
@@ -33,9 +33,9 @@ class ParameterContainer : public ParameterInterface {
   );
   ParameterContainer(const ParameterContainer& parameter);
 
-  void set_value(const py::object& value);
+  void set_value(py::object value);
 
-  py::object get_value();
+  py::object get_value() const;
 
   void initialize();
 

diff --git a/python/setup.py b/python/setup.py
@@ -11,7 +11,7 @@
 # names of the environment variables that define osqp and openrobots include directories
 osqp_path_var = 'OSQP_INCLUDE_DIR'
 
-__version__ = "6.3.42"
+__version__ = "6.3.43"
 __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
@@ -8,6 +8,8 @@
 
 namespace py_parameter {
 
+// FIXME: Improve exceptions in switch cases and try catch casts
+
 ParameterContainer::ParameterContainer(
     const std::string& name, const ParameterType& type, const StateType& parameter_state_type
 ) : ParameterInterface(name, type, parameter_state_type) {
@@ -42,10 +44,13 @@ ParameterContainer::ParameterContainer(
 }
 
 ParameterContainer::ParameterContainer(const ParameterContainer& parameter) :
-    ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()),
-    values(parameter.values) {}
+    ParameterInterface(parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type()) {
+  if (parameter) {
+    set_value(parameter.get_value());
+  }
+}
 
-void ParameterContainer::set_value(const py::object& value) {
+void ParameterContainer::set_value(py::object value) {
   switch (this->get_parameter_type()) {
     case ParameterType::INT:
       values.int_value = value.cast<int>();
@@ -105,7 +110,7 @@ void ParameterContainer::set_value(const py::object& value) {
   this->set_empty(false);
 }
 
-py::object ParameterContainer::get_value() {
+py::object ParameterContainer::get_value() const {
   switch (this->get_parameter_type()) {
     case ParameterType::INT:
       return py::cast(values.int_value);
@@ -160,6 +165,10 @@ void ParameterContainer::initialize() {
 }
 
 ParameterContainer interface_ptr_to_container(const std::shared_ptr<ParameterInterface>& parameter) {
+  if (parameter->is_empty()) {
+    return ParameterContainer(
+        parameter->get_name(), parameter->get_parameter_type(), parameter->get_parameter_state_type());
+  }
   switch (parameter->get_parameter_type()) {
     case ParameterType::INT:
       return ParameterContainer(
@@ -233,6 +242,10 @@ ParameterContainer interface_ptr_to_container(const std::shared_ptr<ParameterInt
 }
 
 std::shared_ptr<ParameterInterface> container_to_interface_ptr(const ParameterContainer& parameter) {
+  if (parameter.is_empty()) {
+    return make_shared_parameter_interface(
+        parameter.get_name(), parameter.get_parameter_type(), parameter.get_parameter_state_type());
+  }
   switch (parameter.get_parameter_type()) {
     case ParameterType::INT:
       return make_shared_parameter(parameter.get_name(), parameter.values.int_value);

diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py
@@ -44,6 +44,10 @@ def test_param_int(self):
         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)
+
+        new_param = sr.Parameter(param)
+        self.assertTrue(new_param.is_empty())
+
         param.set_value(1)
         self.assertTrue(param)
         self.assertFalse(param.is_empty())
@@ -52,6 +56,10 @@ def test_param_int(self):
         self.assertFalse(param1.is_empty())
         self.assertEqual(param1.get_value(), 1)
 
+        new_param = sr.Parameter(param1)
+        self.assertFalse(new_param.is_empty())
+        self.assertEqual(param1.get_value(), new_param.get_value())
+
         # FIXME (#50): Use parametrized pytests for these tests
         param1.initialize()
         self.assertTrue(param1.is_empty())

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.3.42)
+project(control_libraries VERSION 6.3.43)
 
 # Build options
 option(BUILD_TESTING "Build all tests." OFF)

diff --git a/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp b/source/state_representation/include/state_representation/space/cartesian/CartesianState.hpp
@@ -68,7 +68,7 @@ class CartesianState : public SpatialState {
   static CartesianState Identity(const std::string& name, const std::string& reference = "world");
 
   /**
-   * @brief Constructor for a random Cartsian state
+   * @brief Constructor for a random Cartesian state
    */
   static CartesianState Random(const std::string& name, const std::string& reference = "world");
 

diff --git a/source/state_representation/include/state_representation/space/joint/JointState.hpp b/source/state_representation/include/state_representation/space/joint/JointState.hpp
@@ -565,6 +565,12 @@ class JointState : public State {
   std::string to_string() const override;
 
 private:
+  /**
+   * @brief Resize the data vectors to a new size
+   * @param size The desired size
+   */
+  void resize(unsigned int size);
+
   std::vector<std::string> names_;///< names of the joints
   Eigen::VectorXd positions_;     ///< joints positions
   Eigen::VectorXd velocities_;    ///< joints velocities

diff --git a/source/state_representation/src/space/Jacobian.cpp b/source/state_representation/src/space/Jacobian.cpp
@@ -58,6 +58,7 @@ Jacobian::Jacobian(const std::string& robot_name,
   this->set_data(data);
 }
 
+// FIXME: improve copy constructor after Jacobian refactor
 Jacobian::Jacobian(const Jacobian& jacobian) :
     State(jacobian),
     joint_names_(jacobian.joint_names_),

diff --git a/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp b/source/state_representation/src/space/cartesian/CartesianAcceleration.cpp
@@ -38,11 +38,11 @@ CartesianAcceleration::CartesianAcceleration(
 }
 
 CartesianAcceleration::CartesianAcceleration(const CartesianState& state) : CartesianState(state) {
-  // set all the state variables to 0 except linear and angular velocities
   this->set_type(StateType::CARTESIAN_ACCELERATION);
-  this->set_zero();
-  this->set_acceleration(state.get_acceleration());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_acceleration(state.get_acceleration());
+  }
 }
 
 CartesianAcceleration::CartesianAcceleration(const CartesianAcceleration& acceleration) :

diff --git a/source/state_representation/src/space/cartesian/CartesianPose.cpp b/source/state_representation/src/space/cartesian/CartesianPose.cpp
@@ -44,11 +44,11 @@ CartesianPose::CartesianPose(
 }
 
 CartesianPose::CartesianPose(const CartesianState& state) : CartesianState(state) {
-  // set all the state variables to 0 except position and orientation
   this->set_type(StateType::CARTESIAN_POSE);
-  this->set_zero();
-  this->set_pose(state.get_pose());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_pose(state.get_pose());
+  }
 }
 
 CartesianPose::CartesianPose(const CartesianPose& pose) : CartesianPose(static_cast<const CartesianState&>(pose)) {}

diff --git a/source/state_representation/src/space/cartesian/CartesianState.cpp b/source/state_representation/src/space/cartesian/CartesianState.cpp
@@ -51,16 +51,10 @@ CartesianState::CartesianState(const std::string& name, const std::string& refer
 }
 
 CartesianState::CartesianState(const CartesianState& state) :
-    SpatialState(state),
-    position_(state.position_),
-    orientation_(state.orientation_),
-    linear_velocity_(state.linear_velocity_),
-    angular_velocity_(state.angular_velocity_),
-    linear_acceleration_(state.linear_acceleration_),
-    angular_acceleration_(state.angular_acceleration_),
-    force_(state.force_),
-    torque_(state.torque_) {
-  this->set_type(StateType::CARTESIAN_STATE);
+    CartesianState(state.get_name(), state.get_reference_frame()) {
+  if (state) {
+    this->set_state_variable(state.get_state_variable(CartesianStateVariable::ALL), CartesianStateVariable::ALL);
+  }
 }
 
 CartesianState CartesianState::Identity(const std::string& name, const std::string& reference) {

diff --git a/source/state_representation/src/space/cartesian/CartesianTwist.cpp b/source/state_representation/src/space/cartesian/CartesianTwist.cpp
@@ -38,11 +38,11 @@ CartesianTwist::CartesianTwist(
 }
 
 CartesianTwist::CartesianTwist(const CartesianState& state) : CartesianState(state) {
-  // set all the state variables to 0 except linear and angular velocities
   this->set_type(StateType::CARTESIAN_TWIST);
-  this->set_zero();
-  this->set_twist(state.get_twist());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_twist(state.get_twist());
+  }
 }
 
 CartesianTwist::CartesianTwist(const CartesianTwist& twist) :

diff --git a/source/state_representation/src/space/cartesian/CartesianWrench.cpp b/source/state_representation/src/space/cartesian/CartesianWrench.cpp
@@ -36,11 +36,11 @@ CartesianWrench::CartesianWrench(
 }
 
 CartesianWrench::CartesianWrench(const CartesianState& state) : CartesianState(state) {
-  // set all the state variables to 0 except force and torque
   this->set_type(StateType::CARTESIAN_WRENCH);
-  this->set_zero();
-  this->set_wrench(state.get_wrench());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_wrench(state.get_wrench());
+  }
 }
 
 CartesianWrench::CartesianWrench(const CartesianWrench& wrench) :

diff --git a/source/state_representation/src/space/joint/JointAccelerations.cpp b/source/state_representation/src/space/joint/JointAccelerations.cpp
@@ -33,11 +33,11 @@ JointAccelerations::JointAccelerations(const std::string& robot_name,
 }
 
 JointAccelerations::JointAccelerations(const JointState& state) : JointState(state) {
-  // set all the state variables to 0 except accelerations
   this->set_type(StateType::JOINT_ACCELERATIONS);
-  this->set_zero();
-  this->set_accelerations(state.get_accelerations());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_accelerations(state.get_accelerations());
+  }
 }
 
 JointAccelerations::JointAccelerations(const JointAccelerations& accelerations) :

diff --git a/source/state_representation/src/space/joint/JointPositions.cpp b/source/state_representation/src/space/joint/JointPositions.cpp
@@ -34,11 +34,11 @@ JointPositions::JointPositions(
 }
 
 JointPositions::JointPositions(const JointState& state) : JointState(state) {
-  // set all the state variables to 0 except positions
   this->set_type(StateType::JOINT_POSITIONS);
-  this->set_zero();
-  this->set_positions(state.get_positions());
-  this->set_empty(state.is_empty());
+  if (state) {
+    this->set_zero();
+    this->set_positions(state.get_positions());
+  }
 }
 
 JointPositions::JointPositions(const JointPositions& positions) :

diff --git a/source/state_representation/src/space/joint/JointState.cpp b/source/state_representation/src/space/joint/JointState.cpp
@@ -32,29 +32,27 @@ static unsigned int get_state_variable_size_factor(const JointStateVariable& sta
 
 JointState::JointState() : State() {
   this->set_type(StateType::JOINT_STATE);
-  this->initialize();
 }
 
 JointState::JointState(const std::string& robot_name, unsigned int nb_joints) : State(robot_name), names_(nb_joints) {
   this->set_type(StateType::JOINT_STATE);
   this->set_names(nb_joints);
-  this->initialize();
+  this->resize(nb_joints);
+  this->set_zero();
 }
 
 JointState::JointState(const std::string& robot_name, const std::vector<std::string>& joint_names) :
     State(robot_name), names_(joint_names) {
   this->set_type(StateType::JOINT_STATE);
-  this->initialize();
+  this->resize(joint_names.size());
+  this->set_zero();
 }
 
 JointState::JointState(const JointState& state) :
-    State(state),
-    names_(state.names_),
-    positions_(state.positions_),
-    velocities_(state.velocities_),
-    accelerations_(state.accelerations_),
-    torques_(state.torques_) {
-  this->set_type(StateType::JOINT_STATE);
+    JointState(state.get_name(), state.names_) {
+  if (state) {
+    this->set_state_variable(state.get_state_variable(JointStateVariable::ALL), JointStateVariable::ALL);
+  }
 }
 
 JointState JointState::Zero(const std::string& robot_name, unsigned int nb_joints) {
@@ -91,6 +89,13 @@ JointState& JointState::operator=(const JointState& state) {
   return *this;
 }
 
+void JointState::resize(unsigned int size) {
+  this->positions_.resize(size);
+  this->velocities_.resize(size);
+  this->accelerations_.resize(size);
+  this->torques_.resize(size);
+}
+
 Eigen::VectorXd JointState::get_state_variable(const JointStateVariable& state_variable_type) const {
   switch (state_variable_type) {
     case JointStateVariable::POSITIONS:
@@ -351,7 +356,7 @@ void JointState::clamp_state_variable(
     const Eigen::ArrayXd& noise_ratio_array
 ) {
   Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type);
-  int expected_size = state_variable.size();
+  long expected_size = state_variable.size();
   if (max_absolute_value_array.size() != expected_size) {
     throw IncompatibleSizeException(
         "Array of max values is of incorrect size: expected " + std::to_string(expected_size) + ", given "
@@ -379,7 +384,7 @@ void JointState::clamp_state_variable(
     double max_absolute_value, const JointStateVariable& state_variable_type, double noise_ratio
 ) {
   Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type);
-  int expected_size = state_variable.size();
+  long expected_size = state_variable.size();
   this->clamp_state_variable(
       max_absolute_value * Eigen::ArrayXd::Ones(expected_size), state_variable_type,
       noise_ratio * Eigen::ArrayXd::Ones(expected_size));
@@ -422,13 +427,6 @@ double dist(const JointState& s1, const JointState& s2, const JointStateVariable
 
 void JointState::initialize() {
   this->State::initialize();
-  // resize
-  unsigned int size = this->names_.size();
-  this->positions_.resize(size);
-  this->velocities_.resize(size);
-  this->accelerations_.resize(size);
-  this->torques_.resize(size);
-  // set to zeros
   this->set_zero();
 }
 
@@ -455,16 +453,17 @@ void JointState::set_zero() {
   this->velocities_.setZero();
   this->accelerations_.setZero();
   this->torques_.setZero();
+  // FIXME: reset timestamp
 }
 
 std::vector<double> JointState::to_std_vector() const {
   Eigen::VectorXd data = this->data();
-  return std::vector<double>(data.data(), data.data() + data.size());
+  return {data.data(), data.data() + data.size()};
 }
 
 void JointState::multiply_state_variable(const Eigen::MatrixXd& lambda, const JointStateVariable& state_variable_type) {
   Eigen::VectorXd state_variable = this->get_state_variable(state_variable_type);
-  int expected_size = state_variable.size();
+  long expected_size = state_variable.size();
   if (lambda.rows() != expected_size || lambda.cols() != expected_size) {
     throw IncompatibleSizeException(
         "Gain matrix is of incorrect size: expected " + std::to_string(expected_size) + "x"