add an optional ramp on the input values

motor_controllerTypes.hpp

@@ -1,18 +1,19 @@
 #ifndef motor_controller_TYPES_HPP
 #define motor_controller_TYPES_HPP
 
+#include <base/Float.hpp>
 #include <base/JointState.hpp>
 #include <motor_controller/PID.hpp>
 
 namespace motor_controller {
     /** Configuration of the controller for a single actuator */
     struct ActuatorSettings
     {
-        base::JointState::MODE output_mode;
+        base::JointState::MODE output_mode = base::JointState::UNSET;
         PIDSettings pid;
 
-        ActuatorSettings()
-            : output_mode(base::JointState::UNSET) {}
+        /** Max allowed variation in input command in unit/s */
+        float ramp = base::infinity<float>();
     };
 }
 

tasks/PIDTask.cpp

@@ -37,6 +37,7 @@ bool PIDTask::configureHook()
     mInputCommand.resize(size);
     mOutputCommand.resize(size);
     mPIDState.resize(size);
+    mRampState.resize(size);
     return true;
 }
 
@@ -46,6 +47,10 @@ bool PIDTask::startHook()
         return false;
     }
 
+    for (auto& state : mRampState) {
+        state.first = base::Time();
+    }
+
     for (size_t i = 0; i < mSettings.size(); ++i) {
         mPIDs[i].reset();
         mPIDs[i].setPIDSettings(mSettings[i].pid);
@@ -103,6 +108,7 @@ void PIDTask::updateHook()
             return exception(INVALID_INPUT_COMMAND);
         }
         float input_target = mInputCommand[i].getField(input_domain);
+        float ramped_input_target = applyRamp(i, base::Time::now(), input_target);
         float input_state  = mStatus[i].getField(input_domain);
 
         if (base::isUnknown(input_state)) {
@@ -118,7 +124,7 @@ void PIDTask::updateHook()
         float pid_output = computePIDOutput(i,
                 output_domain,
                 input_state,
-                input_target,
+                ramped_input_target,
                 mStatus.time);
 
         mOutputCommand[i] = base::JointState();
@@ -130,6 +136,37 @@ void PIDTask::updateHook()
     _pid_states.write(mPIDState);
 }
 
+static float clamp(float input, float min, float max) {
+    if (input > max) {
+        return max;
+    }
+    else if (input < min) {
+        return min;
+    }
+    else {
+        return input;
+    }
+}
+
+float PIDTask::applyRamp(int idx, base::Time time, float input) {
+    float ramp = mSettings[idx].ramp;
+    float ramped_input = input;
+
+    if (!mRampState[idx].first.isNull()) {
+        float lastInput = mRampState[idx].second;
+        float dt = (time - mRampState[idx].first).toSeconds();
+        float allowed_range = ramp * dt;
+        ramped_input = clamp(
+            input,
+            lastInput - allowed_range,
+            lastInput + allowed_range
+        );
+    }
+
+    mRampState[idx] = std::make_pair(time, ramped_input);
+    return ramped_input;
+}
+
 float PIDTask::computePIDOutput(int idx,
         JointState::MODE output_domain,
         float state,

tasks/PIDTask.hpp

@@ -33,6 +33,10 @@ namespace motor_controller {
         base::commands::Joints mOutputCommand;
         std::vector<motor_controller::PIDState> mPIDState;
 
+        typedef std::pair<base::Time, float> RampState;
+        std::vector<RampState> mRampState;
+        float applyRamp(int idx, base::Time time, float input);
+
         /** Computes an output command
          *
          * The default implementation simply calls the PID controller for the

test/pid_task_test.rb

@@ -13,46 +13,6 @@
         )
     end
 
-    describe 'input validation' do
-        before do
-            settings = Types.motor_controller.ActuatorSettings.new
-            settings.zero!
-            settings.output_mode = :RAW
-            @task.properties.settings = [settings]
-            syskit_configure_and_start(@task)
-        end
-
-        it 'fails if the expected set of actuators do not match '\
-        'the number of entries in the status' do
-            command = Types.base.samples.Joints.new(
-                elements: [Types.base.JointState.new]
-            )
-            status = Types.base.samples.Joints.new
-
-            expect_execution do
-                syskit_write task.in_command_port, command
-                syskit_write task.status_samples_port, status
-            end.to do
-                emit task.wrong_status_size_event
-            end
-        end
-
-        it 'fails if the expected set of actuators do not match '\
-        'the number of entries in the command' do
-            command = Types.base.samples.Joints.new
-            status = Types.base.samples.Joints.new(
-                elements: [Types.base.JointState.new]
-            )
-
-            expect_execution do
-                syskit_write task.in_command_port, command
-                syskit_write task.status_samples_port, status
-            end.to do
-                emit task.wrong_input_command_size_event
-            end
-        end
-    end
-
     describe 'the application of the PID controller' do
         def do_test_input_field_selection(
             mode, position, speed, effort, raw, acceleration
@@ -143,6 +103,36 @@ def do_test_input_field_selection(
             syskit_configure_and_start(@task)
         end
 
+        it 'fails if the expected set of actuators do not match '\
+           'the number of entries in the status' do
+            command = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.new]
+            )
+            status = Types.base.samples.Joints.new
+
+            expect_execution do
+                syskit_write task.in_command_port, command
+                syskit_write task.status_samples_port, status
+            end.to do
+                emit task.wrong_status_size_event
+            end
+        end
+
+        it 'fails if the expected set of actuators do not match '\
+           'the number of entries in the command' do
+            command = Types.base.samples.Joints.new
+            status = Types.base.samples.Joints.new(
+                elements: [Types.base.JointState.new]
+            )
+
+            expect_execution do
+                syskit_write task.in_command_port, command
+                syskit_write task.status_samples_port, status
+            end.to do
+                emit task.wrong_input_command_size_event
+            end
+        end
+
         it 'emits invalid_input_command if the command has no fields set' do
             command = Types.base.samples.Joints.new(
                 elements: [Types.base.JointState.Effort(Float::NAN)]
@@ -312,4 +302,177 @@ def do_test_input_field_selection(
             assert cmd.elements[0].raw.nan?
         end
     end
+
+    describe "ramp" do
+        before do
+            @settings = Types.motor_controller.ActuatorSettings.new
+            @settings.zero!
+            @settings.output_mode = :EFFORT
+            @settings.pid.K = 1
+            @settings.pid.Ts = 1
+            @settings.pid.B = 1
+
+            @command = Types.base.samples.Joints.new(
+                time: Time.now,
+                elements: [Types.base.JointState.Effort(0)]
+            )
+            @status = Types.base.samples.Joints.new(
+                time: Time.now,
+                elements: [Types.base.JointState.Effort(0)]
+            )
+        end
+
+        it "does not do any positive limiting if the ramp is infinite" do
+            @settings.ramp = Float::INFINITY
+            _, cmd_writer = configure_and_start(@task)
+
+            write_cmd_effort cmd_writer, 1_000_000
+
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            assert_in_delta 1_000_000, cmd.elements[0].effort, 1e-3
+        end
+
+        it "does not do any negative limiting if the ramp is infinite" do
+            @settings.ramp = Float::INFINITY
+            _, cmd_writer = configure_and_start(@task)
+
+            write_cmd_effort cmd_writer, -1_000_000
+
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            assert_in_delta -1_000_000, cmd.elements[0].effort, 1e-3
+        end
+
+        it "ramps up the control input according to the rate set in the settings" do
+            @settings.ramp = 10
+            _, cmd_writer = configure_and_start(@task)
+            start_time = Time.now
+
+            write_cmd_effort cmd_writer, 100
+
+            10.times do
+                before = Time.now
+                cmd = expect_execution do
+                    syskit_write @task.status_samples_port, @status
+                end.to { have_one_new_sample task.out_command_port }
+                after = Time.now
+
+                assert_in_interval(
+                    cmd.elements[0].effort,
+                    (before - start_time) * 10 * 0.9,
+                    (after - start_time) * 10 * 1.1
+                )
+                sleep 0.2
+            end
+        end
+
+        it "ramps down the control input according to the rate set in the settings" do
+            @settings.ramp = 10
+            _, cmd_writer = configure_and_start(@task)
+            start_time = Time.now
+
+            write_cmd_effort cmd_writer, -100
+
+            10.times do
+                before = Time.now
+                cmd = expect_execution do
+                    syskit_write @task.status_samples_port, @status
+                end.to { have_one_new_sample task.out_command_port }
+                after = Time.now
+
+                assert_in_interval(
+                    cmd.elements[0].effort,
+                    (after - start_time) * -10 * 1.1,
+                    (before - start_time) * -10 * 0.9
+                )
+                sleep 0.2
+            end
+        end
+
+        it "ramps from the last generated command on command update" do
+            @settings.ramp = 10
+            _, cmd_writer = configure_and_start(@task)
+            start_time = Time.now
+
+            write_cmd_effort cmd_writer, 100
+            sleep 0.1
+
+            before = Time.now
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            after = Time.now
+
+            assert_in_interval(
+                cmd.elements[0].effort,
+                (before - start_time) * 10 * 0.9,
+                (after - start_time) * 10 * 1.1
+            )
+
+            start_cmd = cmd.elements[0].effort
+            write_cmd_effort cmd_writer, (start_cmd + 0.05)
+            sleep 0.1
+
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            assert_in_delta start_cmd + 0.05, cmd.elements[0].effort, 1e-4
+        end
+
+        it "sticks to the target once reached (positive)" do
+            @settings.ramp = 10
+            _, cmd_writer = configure_and_start(@task)
+
+            write_cmd_effort cmd_writer, 10
+            sleep 1.1
+
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            assert_equal 10, cmd.elements[0].effort
+        end
+
+        it "sticks to the target once reached (negative)" do
+            @settings.ramp = 10
+            _, cmd_writer = configure_and_start(@task)
+
+            write_cmd_effort cmd_writer, -10
+            sleep 1.1
+
+            cmd = expect_execution do
+                syskit_write @task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+            assert_equal -10, cmd.elements[0].effort
+        end
+
+        def configure_and_start(task)
+            task.properties.settings = [@settings]
+            syskit_configure_and_start(task)
+
+            cmd_writer = @task.in_command_port.writer
+            expect_execution.to { achieve { cmd_writer.ready? } }
+            cmd_writer.write(@command)
+            sleep 0.1
+
+            sample = expect_execution do
+                syskit_write task.status_samples_port, @status
+            end.to { have_one_new_sample task.out_command_port }
+
+            [sample, cmd_writer]
+        end
+
+        def write_cmd_effort(writer, effort)
+            @command.elements[0].effort = effort
+            writer.write @command
+            sleep 0.1
+        end
+
+        def assert_in_interval(value, min, max)
+            flunk("#{value} to be in [#{min}, #{max}]") if value <= min
+            flunk("#{value} to be in [#{min}, #{max}]") if value >= max
+        end
+    end
 end