first draft; need to show expected controller output

Signed-off-by: Michael Anderson <anderson@mbari.org>

docs/docs/Tutorials/ROS2/PythonLinearDamperExample.md

@@ -34,7 +34,8 @@ mbari_wec_linear_damper_py
     └── test_pep257.py
 ```
 
-with the files modified according to the previous tutorial
+with the files modified according to the previous tutorial. The package for this tutorial is named
+`mbari_wec_linear_damper_py`.
 
 ---
 
@@ -43,7 +44,12 @@ with the files modified according to the previous tutorial
 In this tutorial you will implement a simple linear damper controller for the piston in the WEC
 Power-Take-Off (PTO). Given motor RPM, it outputs desired motor winding current (interpolated from
 RPM->Torque lookup table) to generate a torque to resist piston velocity with a damping force.
-Configurable gains (scale/retract factor) are applied before output.
+Configurable gains (scale/retract factor) are applied before output. In the end, you will have a
+working linear damper controller that is very close to the controller running on both the physical
+and simulated buoy.
+
+A full example starting from the template may be found
+[here](https://github.com/mbari-org/mbari_wec_template_py/tree/linear_damper_example).
 
 ### Parameters
 
@@ -53,14 +59,172 @@ Parameters for the controller are:
   Constant to convert desired torque to applied motor winding current
 <br/>
 <br/>
-- `n_spec`: Motor RPM Breakpoints  
-  `N` (RPM) is the input to the controller and `n_spec` are the x-components of the breakpoints
-  (`n_spec`, `torque_spec` / `torque_constant`) for the interpolant,
-  `f(n_spec) = torque_spec / torque_constant`
+- `n_spec`: Input Motor Speed (RPM) Breakpoints  
+  \(N\) (RPM) is the input to the controller and `n_spec` are the x-components of the breakpoints
+  \(\left(n\_spec, \frac{torque\_spec}{torque_constant}\right)\) for the interpolant,  
+<br/>
+  \(\hat{f}_{I}(n\_spec) = \frac{torque\_spec}{torque\_constant} \approx f_{I}(N) = I\)
 <br/>
 <br/>
-- `torque_spec`: Desired Motor Torque (N-m) Breakpoints  
+- `torque_spec`: Desired Output Motor Torque (N-m) Breakpoints  
   Torque (N-m) is the eventual desired output of the controller given an input `N` (motor RPM) and
   `torque_spec` / `torque_constant` (Amps) are the y-components of the breakpoints for the
   interpolant. The controller actually outputs motor winding current (Amps) to generate a torque
   in the opposite direction of piston velocity to generate a damping force.
+
+These can be configured using the `config/controller.yaml` file.
+
+``` yaml linenums="1" title="config/controller.yaml"
+/linear_damper:
+  ros__parameters:
+    torque_constant: 0.438
+    n_spec: [0.0, 300.0, 600.0, 1000.0, 1700.0, 4400.0, 6790.0]
+    torque_spec: [0.0, 0.0, 0.8, 2.9, 5.6, 9.8, 16.6]
+```
+
+As you can see, as motor speed increases, so does the damping torque. For low RPM (up to 300),
+there is no damping.
+
+Initialize these variables in `ControlPolicy` in `mbari_wec_linear_damper_py/controller.py`. This
+example makes use of `numpy.array`.
+
+``` py linenums="24" title="mbari_wec_linear_damper_py/controller.py"
+class ControlPolicy(object):
+    """
+    Simple Linear Damper Control Policy.
+    Implements a simple linear damper controller for the piston in the WEC
+    Power-Take-Off (PTO). Given motor RPM, outputs desired motor winding current (interpolated
+    from RPM->Torque lookup table) to resist piston velocity. Configurable gains
+    (scale/retract factor) are applied before output.
+    """
+
+    def __init__(self):
+        # Define any parameter variables here
+        self.Torque_constant = 0.438  # N-m/Amps
+        # Desired damping Torque vs RPM relationship
+        self.N_Spec = np.array([0.0, 300.0, 600.0, 1000.0, 1700.0, 4400.0, 6790.0])  # RPM
+        self.Torque_Spec = np.array([0.0, 0.0, 0.8, 2.9, 5.6, 9.8, 16.6])  # N-m
+```
+
+Update the dependent variable, `I_Spec`, and create the interpolator, `windcurr_interp1d`, which
+uses `interp1d` from `scipy.interpolate`.
+
+``` py linenums="43" title="mbari_wec_linear_damper_py/controller.py"
+    def update_params(self):
+        """Update dependent variables after reading in params."""
+        # Convert to Motor Winding Current vs RPM and generate interpolator for f(RPM) = I
+        self.I_Spec = self.Torque_Spec / self.Torque_constant  # Amps
+        self.windcurr_interp1d = interpolate.interp1d(self.N_Spec, self.I_Spec,
+                                                      fill_value=self.I_Spec[-1],
+                                                      bounds_error=False)
+```
+
+Finally, in the `Controller` class, declare/get/set/update these parameters from ROS 2 (as set in
+`config/controller.yaml`).
+
+``` py linenums="118" title="mbari_wec_linear_damper_py/controller.py"
+    def set_params(self):
+        """Use ROS2 declare_parameter and get_parameter to set policy params."""
+        self.declare_parameter('torque_constant', self.policy.Torque_constant)
+        self.policy.Torque_constant = \
+            self.get_parameter('torque_constant').get_parameter_value().double_value
+
+        self.declare_parameter('n_spec', self.policy.N_Spec.tolist())
+        self.policy.N_Spec = \
+            np.array(self.get_parameter('n_spec').get_parameter_value().double_array_value)
+
+        self.declare_parameter('torque_spec', self.policy.Torque_Spec.tolist())
+        self.policy.Torque_Spec = \
+            np.array(self.get_parameter('torque_spec').get_parameter_value().double_array_value)
+
+        # recompute any dependent variables
+        self.policy.update_params()
+        self.get_logger().info(str(self.policy))
+```
+
+The example includes a helper function, `__str__`, in the `ControlPolicy` class to report the
+parameters used.
+
+``` py linenums="66" title="mbari_wec_linear_damper_py/controller.py"
+    def __str__(self):
+        return """ControlPolicy:
+\tTorque_constant: {tc}
+\tN_Spec: {nspec}
+\tTorque_Spec: {tspec}
+\tI_Spec: {ispec}""".format(tc=self.Torque_constant,
+                            nspec=self.N_Spec,
+                            tspec=self.Torque_Spec,
+                            ispec=self.I_Spec)
+```
+
+### Control Policy Target
+
+To implement the torque control control policy, we use the `target` function in `ControlPolicy`.
+This is where we accept feedback data and return a command value. In this case, we need the motor
+`rpm`, and the gains applied to the winding current damping, `scale_factor` and `retract_factor`.
+Typical values for these gains are
+
+- scale_factor = 1
+- retract_factor = 0.6
+
+``` py linenums="52" title="mbari_wec_linear_damper_py/controller.py"
+    def target(self, rpm, scale_factor, retract_factor):
+        """Calculate target value from feedback inputs."""
+        N = abs(rpm)
+        I = self.windcurr_interp1d(N)
+
+        # Apply damping gain
+        I *= scale_factor
+
+        # Hysteresis due to gravity assist
+        if rpm > 0.0:
+            I *= -retract_factor
+
+        return float(I)
+```
+
+So, as you can see we apply a positive damping torque when `N` is negative (piston extending), and
+a positive damping torque when `N` is positive (piston retracting). The damping torque required is
+reduced when retracting.
+
+### Controller
+
+All that is left is to connect the necessary feedback data to the `ControlPolicy`. In this case,
+`rpm`, `scale`, and `retract` are present in `buoy_interfaces.msg.PCRecord` on the `/power_data`
+topic published by the Power Controller running on the buoy.
+
+To access the data, all that is required is to define the callback `def power_callback(self, data)`
+in the `Controller` class, and pass the data to `self.policy.target` to get the desired winding
+current command. Various commands are available, and this time we will be using  
+`self.send_pc_wind_curr_command(wind_curr, blocking=False)`
+
+``` py linenums="107" title="mbari_wec_linear_damper_py/controller.py"
+    def power_callback(self, data):
+        """Provide feedback of '/power_data' topic from Power Controller."""
+        # Update class variables, get control policy target, send commands, etc.
+        wind_curr = self.policy.target(data.rpm, data.scale, data.retract)
+
+        self.get_logger().info('WindingCurrent:' +
+                               f' f({data.rpm:.02f}, {data.scale:.02f}, {data.retract:.02f})' +
+                               f' = {wind_curr:.02f}')
+
+        self.send_pc_wind_curr_command(wind_curr, blocking=False)
+```
+
+## Try It Out
+
+We will be using `ros2 launch` and `launch/controller.launch.py` to run our new controller.
+
+To run the controller along with the simulation, first source your workspace. Then, launch the
+sim:  
+`$ ros2 launch buoy_gazebo mbari_wec.launch.py`  
+and click the play button.
+
+Then, launch your controller:  
+`$ ros2 launch mbari_wec_linear_damper_py controller.launch.py`
+
+You should see output similar to 
+
+```
+...
+```