Ring DS

source/dynamical_systems/CMakeLists.txt

@@ -10,6 +10,7 @@ set(CORE_SOURCES
   src/DynamicalSystem.cpp
   src/Circular.cpp
   src/Linear.cpp
+  src/Ring.cpp
 )
 
 add_library(${PROJECT_NAME} SHARED

source/dynamical_systems/README.md

@@ -10,10 +10,13 @@ This description covers most of the functionalities starting from the spatial tr
 * [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)
+  * [Evaluating the Linear DS](#evaluating-the-linear-ds)
   * [Reference frames](#reference-frames)
 * [Circular](#circular)
+  * [Configuring the Circular DS](#configuring-the-circular-ds)
+* [Ring](#ring)
+  * [Configuring the Ring DS](#configuring-the-ring-ds)
 
 ## Base DynamicalSystem
 
@@ -39,7 +42,7 @@ joint velocity.
 
 ### Base frame
 
-The DynamicalSystem base class has a private `base_frame` property, which can be thought of as the DS origin. 
+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 
@@ -55,7 +58,7 @@ 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
+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.
 
@@ -70,6 +73,12 @@ DynamicalSystem::Linear<state_representation::JointState> linear(jointAttractor)
 
 ### Configuring the Linear DS
 
+The Linear DS has the following core parameters:
+- **attractor**; the CartesianState or JointState type object defining the attractor pose relative to the DS base frame
+- **gain**; the proportional gain acting towards the attractor
+
+Each parameter has corresponding `set_` and `get_` functions.
+
 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
@@ -208,4 +217,114 @@ while (...) {
 
 ## Circular
 
-TODO
+The **Circular** DS is a limit cycle that rotates around a center point in an elliptical orbit,
+converging to a desired radius on a plane.
+
+This DS is defined only for the `CartesianState` type.
+
+The Circular DS can be constructed with a `CartesianState` state and radius as an argument;
+the state position defines the center of the limit cycle, while the state orientation defines the inclination
+of the limit cycle plane. The radius has a default value of 1.
+
+The internal representation of the limit cycle is a `state_representation::Ellipsoid` type. When a state is used
+in the constructor, the limit cycle has a constant radius.
+If an elliptical limit cycle is desired, the DS can be constructed directly from an `Ellipsoid` type.
+
+```c++
+// construct the circular DS limit cycle using a CartesianState center
+state_representation::CartesianState center("center");
+
+// default constructor (radius = 1)
+DynamicalSystems::Circular circularDS1(center);
+
+// constructor with radius
+double radius = 2.0;
+DynamicalSystems::Circular circularDS2(center, radius);
+
+// construct the circular DS limit cycle directly using an Ellipsoid type
+state_representation::Ellipsoid ellipse("limit_cycle");
+ellipse.set_center_state(center);
+ellipse.set_axis_lengths({radius, 2 * radius});
+DynamicalSystems::Circular ellipticalDS(ellipse);
+```
+
+### Configuring the Circular DS
+
+The Circular DS has the following core parameters:
+
+- **limit_cycle**; the Ellipsoid object defining the limit cycle center, shape and inclination 
+- **planar_gain**; the proportional gain acting in the local plane towards the limit cycle radius
+- **normal_gain**; the proportional gain acting towards the local plane
+- **circular_velocity**; the expected angular orbital velocity around the local origin
+
+Each parameter has corresponding `set_` and `get_` functions.
+
+The constructor takes additional optional arguments to define the gain and circular velocity.
+The scalar value for the gain sets both the planar and normal gain of the DS. 
+
+Note that the argument for the radius is only present when a CartesianState is provided,
+and not when an Ellipsoid is provided.
+```c++
+// construct the circular DS with optional parameters (default values are shown)
+double radius = 1.0;
+double gain = 1.0;
+double circular_velocity = M_PI / 2;
+
+// construction with a CartesianState center
+DynamicalSystems::Circular circularDS(center, radius, gain, circular_velocity);
+
+// construction with an Ellipsoid ellipse
+DynamicalSystems::Circular circularDS(ellipse, gain, circular_velocity);
+```
+
+In addition, the following helper are defined to configure the limit cycle behaviour.
+
+```c++
+.set_center(CartesianState("center"));  // sets the center position and orientation of the limit cycle
+.set_gain(1.0);                         // sets both planar and normal gain to the gain value
+.set_rotation_angle(0.0);               // sets a rotation offset around the local Z axis
+.set_radius(1.0);                       // sets both elliptical axes to the given radius (forces circle)
+.set_radiuses({1.0, 2.0});              // sets the length of each elliptical axis individually  
+```
+
+
+## Ring
+
+The **Ring** DS is similar to the **Circular** DS but is more parameterizable.
+In brief, the strength of the limit cycle can be configured with a width around the ring radius.
+
+It only supports the `CartesianState` type, and always acts in a circular ring. 
+
+```c++
+state_representation::CartesianState center("center");
+DynamicalSystems::Ring ringDS(center);
+```
+
+### Configuring the Ring DS
+
+The Ring DS has the following core parameters:
+
+- **center**; the ring center CartesianPose expressed in the DS base reference frame.
+This sets both the origin center and the inclination of the ring plane.
+- **rotation_offset**; the orientation offset of the orientational attractor in the ring frame. 
+- **radius**; the ring radius. [m]
+- **width**; the distance from the radius where the velocity has tangential components around the ring.
+Beyond this width, the velocity is always perpendicular towards the radius. [m]
+- **speed**; the desired linear speed when travelling along the circle radius. [m/s]
+- **field_strength**; the scale factor applied to the ring speed outside of the radius + width zone.
+- **normal_gain**; the scale factor for the speed normal to the ring plane.
+- **angular_gain**; the scale factor for angular velocity restitution.
+
+Each parameter has corresponding `set_` and `get_` functions.
+
+The constructor takes additional optional arguments to define the ring DS parameters.
+```c++
+// construct the ring DS with optional parameters (default values are shown)
+double radius = 1.0;
+double width = 0.5;
+double speed = 1.0;
+double field_strength = 1.0;
+double normal_gain = 1.0;
+double angular_gain = 1.0;
+DynamicalSystems::Ring ringDS(center, radius, width, speed, field_strength, normal_gain, angular_gain);
+```

source/dynamical_systems/include/dynamical_systems/Ring.hpp

@@ -0,0 +1,168 @@
+#pragma once
+
+#include "dynamical_systems/DynamicalSystem.hpp"
+#include "state_representation/parameters/Parameter.hpp"
+#include "state_representation/space/cartesian/CartesianPose.hpp"
+#include "state_representation/space/cartesian/CartesianState.hpp"
+#include <cmath>
+#include <vector>
+
+namespace DynamicalSystems {
+/**
+ * @class Ring
+ * @brief Represent a Ring dynamical system limit cycle to move around a radius within a fixed width
+ */
+class Ring : public DynamicalSystem<state_representation::CartesianState> {
+private:
+  typedef std::shared_ptr<state_representation::Parameter<double>> ptr_param_double_t;
+  std::shared_ptr<state_representation::Parameter<state_representation::CartesianPose>> center_; ///< ring center
+  std::shared_ptr<state_representation::Parameter<state_representation::CartesianPose>> rotation_offset_; ///< orientation of attractor in circle frame
+  ptr_param_double_t radius_; ///< ring radius [m]
+  ptr_param_double_t width_; ///< distance around radius where field rotates [m]
+  ptr_param_double_t speed_; ///< desired linear speed when travelling along the circle radius [m/s]
+  ptr_param_double_t field_strength_; ///< scale factor for desired speed outside of radius + width
+  ptr_param_double_t normal_gain_; ///< scale factor for the speed normal to the circular plane
+  ptr_param_double_t angular_gain_; ///< scale factor for angular velocity restitution
+
+  Eigen::Vector3d calculateLocalLinearVelocity(const state_representation::CartesianPose& pose,
+                                               double& localFieldStrength) const;
+  Eigen::Vector3d calculateLocalAngularVelocity(const state_representation::CartesianPose& pose,
+                                                const Eigen::Vector3d& linearVelocity,
+                                                double localFieldStrength) const;
+
+protected:
+  /**
+   * @brief Compute the dynamics of the input state.
+   * Internal function, to be redefined based on the
+   * type of dynamical system, called by the evaluate
+   * function
+   * @param state the input state
+   * @return the output state
+   */
+  state_representation::CartesianState compute_dynamics(const state_representation::CartesianState& state) const override;
+
+public:
+  /**
+   * @brief Default constructor with center and fixed radius
+   * @param center the center of the limit cycle
+   * @param radius radius of the limit cycle (default=1.)
+   */
+  explicit Ring(const state_representation::CartesianState& center,
+                    double radius = 1.0,
+                    double width = 0.5,
+                    double speed = 1.0,
+                    double field_strength = 1.0,
+                    double normal_gain = 1.0,
+                    double angular_gain = 1.0);
+
+  /**
+   * @brief Setter of the 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 Setter of the center as a new value
+   * @param center the new center
+   */
+  void set_center(const state_representation::CartesianPose& center);
+
+  /**
+   * @brief Setter of the attractor orientation relative to the circle reference frame
+   * @param rotation the rotation quaternion
+   */
+  void set_rotation_offset(const Eigen::Quaterniond& rotation);
+
+  /**
+  * @brief Setter of the radius of the ring
+  * @param radius the radius [m]
+  */
+  void set_radius(double radius);
+
+  /**
+  * @brief Setter of the field width around the ring radius
+  * @param width the width [m]
+  */
+  void set_width(double width);
+
+  /**
+  * @brief Setter of the linear speed of the ring DS
+  * @param speed the linear speed [m/s]
+  */
+  void set_speed(double speed);
+
+  /**
+  * @brief Setter of the field strength
+  * @param field_strength the field strength
+  */
+  void set_field_strength(double field_strength);
+
+  /**
+  * @brief Setter of the gain normal to the ring plane
+  * @param normal_gain the normal gain
+  */
+  void set_normal_gain(double normal_gain);
+
+  /**
+  * @brief Setter of the angular restitution gain
+  * @param angular_gain the angular gain
+  */
+  void set_angular_gain(double angular_gain);
+
+
+  /**
+   * @brief Getter of the center
+   * @return the center as a const reference
+   */
+  const state_representation::CartesianPose& get_center() const;
+
+  /**
+   * @brief Getter of the rotation offset in the center frame
+   * @return the rotation offset as a quaternion
+   */
+  Eigen::Quaterniond get_rotation_offset() const;
+
+  /**
+  * @brief Getter of the radius of the ring
+  * @return the radius [m]
+  */
+  double get_radius() const;
+
+  /**
+  * @brief Getter of the field width around the ring radius
+  * @return the width [m]
+  */
+  double get_width() const;
+
+  /**
+  * @brief Getter of the linear speed of the ring DS
+  * @return the linear speed [m/s]
+  */
+  double get_speed() const;
+
+  /**
+  * @brief Getter of the field strength
+  * @return the field strength
+  */
+  double get_field_strength() const;
+
+  /**
+  * @brief Getter of the gain normal to the ring plane
+  * @return the normal gain
+  */
+  double get_normal_gain() const;
+
+  /**
+  * @brief Getter of the angular restitution gain
+  * @return the angular gain
+  */
+  double get_angular_gain() const;
+
+  /**
+   * @brief Return a list of all the parameters of the dynamical system
+   * @return the list of parameters
+   */
+  std::list<std::shared_ptr<state_representation::ParameterInterface>> get_parameters() const override;
+};
+
+}// namespace DynamicalSystems