Dynamical Systems Refactor

* Refactor and clarify "DS reference frame" logic

* Refactor test structure and add more coverage

* Add proper README for Base and Linear DS

---

Reimplement DS base frame logic

* Refactor reference_frame_ member and associated set/get functions
to be named base_frame_ in DynamicalSystem class

* Revise DS::evaluate function to always and only return the result
in the same reference frame as the input, accepting either:
* * The base frame of the DS
* * The _reference frame_ of the base frame of the DS

* Add basic constructor to DynamicalSystem to take a string
argument, from which it automatically makes an Identity state
for the base frame

* Add logic to Linear DS set_attractor to assert that it matches
the DS base frame

* Add logic to Linear DS to override set_base_frame and update
the attractor accordingly

* Extend test suite for Linear DS to cover setters for attractor
and base frame, and verify behaviour of stacked/fixed/moving frames

* Revise Circular DS and tests

* Override set_base_frame method to update circle center frame

* Add explicit tests for Circular to cover reference frames

* Minor reformat, use namespace in sourcefile to reduce verbosity

---

Refactor DS tests

* Add test fixtures to linear and circular DS tests

* Refine test cases:
* * In circular test, actually check for convergence!
* * In linear test, add tests for diagonal gains
and for stacked reference frames

* Refactor test structure to use one top-level test runner
test_dynamical_system, with test sources grouped by test_
prefix in the tests subfolder.

* Update the CMakeLists accordingly.

* Rename linear and circular tests to have the test_ prefix

---

Update DS README

* Add more information of base frame and reference frames

* Add practical example of set_base_frame in a control loop

source/dynamical_systems/CMakeLists.txt

@@ -32,21 +32,14 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 if (BUILD_TESTING)
-  add_executable(runTestLinear tests/testLinear.cpp)
-  target_link_libraries(runTestLinear
+  add_executable(test_dynamical_systems test_dynamical_systems.cpp)
+  file(GLOB_RECURSE MODULE_TEST_SOURCES tests test_*.cpp)
+  target_sources(test_dynamical_systems PRIVATE ${MODULE_TEST_SOURCES})
+  target_link_libraries(test_dynamical_systems
     ${PROJECT_NAME}
     state_representation
     ${GTEST_LIBRARIES}
     pthread
   )
-  add_test(NAME runTestLinear COMMAND runTestLinear)
-
-  add_executable(runTestCircular tests/testCircular.cpp)
-  target_link_libraries(runTestCircular
-    ${PROJECT_NAME}
-    state_representation
-    ${GTEST_LIBRARIES}
-    pthread
-  )
-  add_test(NAME runTestCircular COMMAND runTestCircular)
+  add_test(NAME test_dynamical_systems COMMAND test_dynamical_systems)
 endif ()

source/dynamical_systems/README.md

@@ -1,2 +1,210 @@
 # dynamical_systems
 
+This library provides a set of classes to represent **Dynamical Systems**: 
+functions which map a state to a state derivative.
+Those are a set of helpers functions to handle common concepts in robotics such as transformations between frames and 
+the link between them and the robot state.
+This description covers most of the functionalities starting from the spatial transformations.
+
+## Table of contents:
+* [Base DynamicalSystem](#base-dynamicalsystem)
+  * [Base frame](#base-frame)
+* [Linear](#linear)
+  * [Evaluating the Linear DS](#evaluating-the-linear-ds)
+  * [Configuring the Linear DS](#configuring-the-linear-ds)
+  * [Reference frames](#reference-frames)
+* [Circular](#circular)
+
+## Base DynamicalSystem
+
+The base class DynamicalSystem defines the main public interface pattern which the derived classes follow.
+
+It is templated for any state type, though the intention is to use the **State Representation** library
+(for example, `CartesianState` or `JointState`).
+
+The main function is:
+```c++
+template<class S>
+S DynamicalSystem<S>::evaluate(const S& state) const
+```
+where `S` is the state representation type.
+The purpose of the evaluate function is to calculate a state derivative from a state.
+The current implementations support calculating a velocity from a position in either joint space or Cartesian space,
+though higher derivatives and other space types can be extended in a similar fashion.
+
+In the case of a `CartesianState`, only the position and orientation is evaluated, and the returned object
+updates only the linear and angular velocity properties.
+In the case of `JointState`, only the joint position is evaluated, and the returned object contains only the
+joint velocity.
+
+### Base frame
+
+The DynamicalSystem base class has a private `base_frame` property, which can be thought of as the DS origin. 
+The functions `get_base_frame()` and `set_base_frame(const S& state)` can be used to access or modify this base frame.
+
+The DynamicalSystem can be constructed with a `state` to set the base frame, or with string frame name. In 
+the latter case the base frame is set as a null / Identity frame with the specified name.
+For example, `DynamicalSystem<CartesianState>("base")` will create a dynamical system with the null base frame "base",
+expressed in its own frame "base".
+
+In most cases, the constructor for the base DynamicalSystem should not be used directly,
+and rather the derived DS classes should construct the base accordingly.
+
+However, the `set_base_frame()` method remains useful in combination with derived classes.
+See the section on [Linear DS reference frames](#reference-frames) for examples.
+
+## Linear
+
+The Linear DS can be thought of as a point attractor, with a velocity that is linearly proportional
+to the distance of the current state from the attractor.
+It is currently implemented for the `CartesianState` and `JointState` types.
+
+The Linear DS is constructed with a state as an argument; this becomes the attractor.
+```c++
+state_representation::CartesianState cartesianAttractor("A");
+DynamicalSystem::Linear<state_representation::CartesianState> linear(cartesianAttractor);
+
+state_representation::JointState jointAttractor("B");
+DynamicalSystem::Linear<state_representation::JointState> linear(jointAttractor);
+```
+
+### Configuring the Linear DS
+
+To change the strength of the attractor, a gain can be passed as the second argument during construction or
+passed to the `set_gain()` member function. The gain defines the proportionality between
+a distance unit and a velocity unit, and is internally stored as a square matrix with a size corresponding
+to the degrees of freedom in the state representation. For example, the `CartesianState` has six degrees of freedom
+(XYZ in linear and angular space), while the `JointState` would have as many degrees of freedom as joints.
+The gain can be defined as a matrix directly, as a diagonal vector of the appropriate
+length, or as a scalar (which sets the value along the diagonal elements of the matrix).
+```c++
+// set a gain (scalar, vector or matrix during construction)
+double gain = 10;
+state_representation::CartesianState csA("A");
+DynamicalSystem::Linear<state_representation::CartesianState> linear(csA, gain);
+
+// or set / update the gain for the created object
+std::vector<double> gains = {1, 2, 3, 4, 5, 6};
+linear.set_gain(gains);
+
+// update the attractor
+state_representation::CartesianState csB("B");
+linear.set_attractor(csB);
+```
+
+### Evaluating the Linear DS
+
+To get the velocity from a state, simply call the `evaluate()` function.
+
+```c++
+state_representation::CartesianState csA("A"), csB("B");
+DynamicalSystem::Linear<state_representation::CartesianState> linear(csA);
+
+// note: the return type of evaluate is a CartesianState, but
+// it can be directly assigned to a CartesianTwist because the =operator
+// has been defined for that purpose
+state_representation::CartesianState twist = linear.evaluate(csB);
+```
+
+The returned velocity will always be expressed in the same reference frame as the input state.
+
+### Reference frames
+
+The following section applies for the `CartesianState` type.
+
+The `evaulate()` function will always return a twist expressed in the same reference frame as the input state,
+provided that the input state is compatible with the DS.
+
+The input state must be expressed in one of two supported reference frames:
+1. The reference frame of the input is the DS base frame
+2. The reference frame of the input matches the reference frame of the DS base frame
+
+The following snippet illustrates the difference in these two options.
+```c++
+// create a linear DS with attractor B in frame A
+state_representation::CartesianState BinA("B", "A");
+DynamicalSystems::Linear<state_representation::CartesianState> linearDS(BinA);
+
+linearDS.get_attractor().get_name();             // "B"
+linearDS.get_attractor().get_reference_frame();  // "A"
+linearDS.get_base_frame().get_name();            // "A"
+linearDS.get_base_frame().get_reference_frame(); // "A"
+
+// evaluate a point C in frame A
+state_representation::CartesianState CinA("C", "A");
+auto twist0 = linearDS.evaluate(CinA); // valid, twist is expressed in frame A
+
+// set the base from of the DS to be A expressed in the world frame
+state_representation::CartesianState AinWorld("A", "world");
+linearDS.set_base_frame(AinWorld);
+
+linearDS.get_attractor().get_name();             // "B"
+linearDS.get_attractor().get_reference_frame();  // "A"
+linearDS.get_base_frame().get_name();            // "A"
+linearDS.get_base_frame().get_reference_frame(); // "world"
+
+// option 1: reference frame of the input is the same as the base frame 
+// -> reference frame of the input: "A"
+// -> DS base frame: "A"
+// -> reference frame of the output: "A"
+auto twist1 = linearDS.evaluate(CinA);
+
+// option 2: reference frame of the input is the same as the base frame 
+// -> reference frame of the input: "world"
+// -> DS base frame reference frame: "world"
+// -> reference frame of the output: "world"
+auto CinWorld = AinWorld * CinA;
+auto twist2 = linearDS.evaluate(CinWorld);
+
+// as a note, you can mix and match the approach as necessary.
+// the following is using option 1 with an additional external operation
+// to yield a final result equivalent to option 2.
+auto twist3 =  AinWorld * linearDS.evaluate(CinA);
+// twist2 === twist3
+```
+
+Note that the base frame can have its own velocity or other state properties, which 
+are automatically combined with the DS result with respect to the common reference frame.
+
+Setting the base frame of the DS has some benefits. In some cases, the state variable to be
+evaluated is not directly expressed in the frame of the DS.
+Similarly the output twist may need to be expressed in a different reference frame.
+
+As a practical example, consider a case where the state of an end-effector is
+reported in the reference frame of a robot, while a linear attractor is expressed
+in some moving task frame.
+The robot controller expects a twist expressed in the robot frame.
+By updating the DS base frame with respect to the robot frame, 
+any pre-transformation of the end-effector state or post-transformation of the twist can be avoided.
+```c++
+state_representation::CartesianState EE("end_effector", "robot");
+state_representation::CartesianState attractor("attractor", "task");
+state_representation::CartesianState taskInRobot("task", "robot");
+
+DynamicalSystems::Linear<state_representation::CartesianState> linearDS(attractor);
+
+// control loop
+while (...) {
+  // update the EE state from robot feedback 
+  EE.set_pose(...);
+
+  // update the state of the task with respect to the robot (for example, from optitrack)
+  taskInRobot.set_pose(...);
+  taskInRobot.set_linear_velocity(...);
+  taskInRobot.set_angular_velocity(...);
+
+  // now update the DS base frame
+  linearDS.set_base_frame(taskInRobot);
+
+  // find the twist in the robot reference frame 
+  // directly from the end-effector position in the robot reference frame
+  auto twist = linearDS.evaluate(EE);
+
+  // send twist command to controller
+  update_controller(twist);
+}
+```
+
+## Circular
+
+TODO

source/dynamical_systems/include/dynamical_systems/Circular.hpp

@@ -76,6 +76,12 @@ class Circular : public DynamicalSystem<state_representation::CartesianState> {
    */
   void set_center(const state_representation::CartesianPose& center);
 
+  /**
+   * @brief Setter of the DS base frame as a new value
+   * @param base_frame the new base frame
+   */
+  void set_base_frame(const state_representation::CartesianState& base_frame) override;
+
   /**
    * @brief Getter of the planar gain attribute
    * @return The gain value in the plane of the circle

source/dynamical_systems/include/dynamical_systems/DynamicalSystem.hpp

@@ -15,10 +15,10 @@ namespace DynamicalSystems {
  * @class DynamicalSystem
  * @brief Abstract class to define a DynamicalSystem either in joint or cartesian spaces
  */
-template <class S>
+template<class S>
 class DynamicalSystem {
 private:
-  S reference_frame_;
+  S base_frame_;
 
 protected:
   /**
@@ -39,9 +39,15 @@ class DynamicalSystem {
 
   /**
    * @brief Constructor with a provided reference frame
-   * @param reference_frame the reference frame in which the dynamics is computed
+   * @param base_frame the reference frame in which the dynamics is computed
    */
-  explicit DynamicalSystem(const S& reference_frame);
+  explicit DynamicalSystem(const S& base_frame);
+
+  /**
+   * @brief Constructor with a provided reference frame name
+   * @param base_frame the name of the reference frame in which the dynamics is computed
+   */
+  explicit DynamicalSystem(const std::string& base_frame);
 
   /**
    * @brief Evaluate the value of the dynamical system at a given state
@@ -56,27 +62,27 @@ class DynamicalSystem {
    */
   virtual std::list<std::shared_ptr<state_representation::ParameterInterface>> get_parameters() const;
 
-  const S& get_reference_frame() const;
+  const S& get_base_frame() const;
 
-  void set_reference_frame(const S& reference_frame);
+  virtual void set_base_frame(const S& base_frame);
 };
 
-template <class S>
-DynamicalSystem<S>::DynamicalSystem(const S& reference_frame) : reference_frame_(reference_frame) {}
+template<class S>
+DynamicalSystem<S>::DynamicalSystem(const S& base_frame) : base_frame_(base_frame) {}
 
-template <class S>
+template<class S>
 std::list<std::shared_ptr<state_representation::ParameterInterface>> DynamicalSystem<S>::get_parameters() const {
   std::list<std::shared_ptr<state_representation::ParameterInterface>> param_list;
   return param_list;
 }
 
-template <class S>
-inline const S& DynamicalSystem<S>::get_reference_frame() const {
-  return this->reference_frame_;
+template<class S>
+inline const S& DynamicalSystem<S>::get_base_frame() const {
+  return this->base_frame_;
 }
 
-template <class S>
-inline void DynamicalSystem<S>::set_reference_frame(const S& reference_frame) {
-  this->reference_frame_ = reference_frame;
+template<class S>
+inline void DynamicalSystem<S>::set_base_frame(const S& base_frame) {
+  this->base_frame_ = base_frame;
 }
 }// namespace DynamicalSystems