fix angular velocity representation issue

Creates a new property that allows the user to say that euler
rate representation is being used and performs the necessary
transformation to the axis-angle representation. This property
is only used when position_control = false.

auv_control.orogen

@@ -144,6 +144,10 @@ task_context "WorldToAligned", subclasses: 'Base' do
     # efforts)
     property "position_control", "bool"
 
+    # Indicates how the angular velocity is being represented (true: euler-rate or
+    # false: axis-angle). This is NOT used for position control domain.
+    property "ang_vel_euler_rate", "bool", false
+
     # The system state. What is required depends on which parts of the state are
     # given as input
     input_port 'pose_samples', '/base/samples/RigidBodyState'

tasks/WorldToAligned.cpp

@@ -140,6 +140,12 @@ bool WorldToAligned::calcOutput(const LinearAngular6DCommandStatus &merged_comma
             output_command.angular(2)+=(2*M_PI);
 
     }
+    else if (_ang_vel_euler_rate.get())
+    {
+        output_command.angular = base::eulerRate2AngularVelocity(
+            output_command.angular.reverse(), currentPose.orientation
+        );
+    }
 
     // Finally, set the timestamp of the output
     output_command.time = merged_command.command.time;

test/auv_control::WorldToAligned.yml

@@ -1,4 +1,7 @@
 --- name:default
+# Indicates how the angular velocity is being represented (true: euler-rate or
+# false: axis-angle). This is NOT used for position control domain.
+ang_vel_euler_rate: false
 # This property defines which parts of the command input is expected to be
 # set once we merged all the declared input ports.
 expected_inputs:

test/test_world_to_aligned.rb

@@ -9,20 +9,35 @@
     writer 'world_to_aligned', 'cmd_in', :attr_name => 'cmd_in'
     writer 'world_to_aligned', 'pose_samples', :attr_name => 'pose_samples'
 
-    def generate_default_pose
+    def quaternion_from_euler(rpy)
+        # Euler angles respecting ZYX order
+        Eigen::Quaternion.from_euler(
+            Eigen::Vector3.new(rpy[2], rpy[1], rpy[0]), 2, 1, 0
+        )
+    end
+
+    def vector3d_from_array(array)
+        Eigen::Vector3.new(array[0], array[1], array[2])
+    end
+
+    def generate_rbs(
+        pos = [0, 0, 0], rpy = [0, 0, 0], lin_vel = [0, 0, 0], ang_vel = [0, 0, 0]
+    )
         pose_sample = world_to_aligned.pose_samples.new_sample
-        pose_sample.position = pose_sample.velocity = pose_sample.angular_velocity = Eigen::Vector3.new(0, 0, 0)
-        pose_sample.orientation = Eigen::Quaternion.Identity
+        pose_sample.position = vector3d_from_array(pos)
+        pose_sample.orientation = quaternion_from_euler(rpy)
+        pose_sample.velocity = vector3d_from_array(lin_vel)
+        pose_sample.angular_velocity = vector3d_from_array(ang_vel)
         pose_sample.time = Time.now
         pose_sample
     end
 
-    def generate_default_cmd
-        cmd_in = world_to_aligned.cmd_in.new_sample
-        cmd_in.linear = Eigen::Vector3.new(1, 0, 0)
-        cmd_in.angular = Eigen::Vector3.new(0, 0, 0)
-        cmd_in.time = Time.now
-        cmd_in
+    def generate_cmd(lin = [1, 0, 0], ang = [0, 0, 0])
+        set_point = world_to_aligned.cmd_in.new_sample
+        set_point.linear = vector3d_from_array(lin)
+        set_point.angular = vector3d_from_array(ang)
+        set_point.time = Time.now
+        set_point
     end
 
     it "should create a port with the given name prefixed with cmd_" do
@@ -38,8 +53,8 @@ def generate_default_cmd
         world_to_aligned.configure
         world_to_aligned.start
 
-        pose_sample = generate_default_pose
-        cmd = generate_default_cmd
+        pose_sample = generate_rbs
+        cmd = generate_cmd
 
         for i in 0..3
             pose_sample.time = Time.now
@@ -62,14 +77,14 @@ def generate_default_cmd
         world_to_aligned.configure
         world_to_aligned.start
 
-        pose_sample = generate_default_pose
-        cmd = generate_default_cmd
+        pose_sample = generate_rbs
+        cmd = generate_cmd
 
         pose_samples.write pose_sample
         cmd_in.write cmd
         cmd_out0 = assert_has_one_new_sample cmd_out, 0.1
         # sleep(world_to_aligned.timeout_in)
-        cmd_in.write generate_default_cmd
+        cmd_in.write generate_cmd
         assert_state_change(world_to_aligned) { |s| s == :CONTROLLING }
         cmd_out0 = assert_has_one_new_sample cmd_out, 0.1
         sleep(world_to_aligned.timeout_pose.to_i)
@@ -84,8 +99,8 @@ def generate_default_cmd
         world_to_aligned.configure
         world_to_aligned.start
 
-        pose_sample = generate_default_pose
-        cmd = generate_default_cmd
+        pose_sample = generate_rbs
+        cmd = generate_cmd
         pose_samples.write pose_sample
         cmd_in.write cmd
 
@@ -109,7 +124,7 @@ def generate_default_cmd
         world_to_aligned.configure
         world_to_aligned.start
 
-        pose_sample = generate_default_pose
+        pose_sample = generate_rbs
         cmd = world_to_aligned.cmd_in.new_sample
         cmd.angular = Eigen::Vector3.Unset
         cmd.time = Time.now
@@ -139,4 +154,62 @@ def generate_default_cmd
         end
     end
 
+    it "should consider the angular velocity representation to calculate the command" do
+        world_to_aligned.apply_conf_file("auv_control::WorldToAligned.yml")
+
+        world_to_aligned.configure
+        world_to_aligned.start
+
+        roll = -Math::PI / 6
+        pitch = Math::PI / 6
+        yaw = Math::PI / 2
+
+        ang_vel_cmd = [1, 1, 1]
+
+        # By default, the axis-angle representation is assumed. No modification should
+        # be done into the angular velocity command, regardless the current pose.
+        pose_sample = generate_rbs([1, 2, 3], [roll, pitch, yaw], [0, 0, 0], [0, 0, 0])
+        cmd = generate_cmd([1, 2, 3], ang_vel_cmd)
+
+        pose_sample.time = Time.now
+        cmd.time = Time.now
+
+        pose_samples.write pose_sample
+        cmd_in.write cmd
+        sample = assert_has_one_new_sample cmd_out, 1
+
+        expected_ang_vel = cmd.angular
+        assert_equal expected_ang_vel, sample.angular
+
+        # Assures that nothing is done when position_control = true even when
+        # ang_vel_euler_rate is set to true
+        world_to_aligned.position_control = true
+        world_to_aligned.ang_vel_euler_rate = true
+
+        pose_samples.write pose_sample
+        cmd_in.write cmd
+        sample = assert_has_one_new_sample cmd_out, 1
+
+        # Should have the same angular[0] and angular[1] values, angular[2] is altered
+        # as by removing the yaw value
+        expected_ang_vel = cmd.angular
+
+        assert_equal expected_ang_vel[0], sample.angular[0]
+        assert_equal expected_ang_vel[1], sample.angular[1]
+        assert_in_delta expected_ang_vel[2] - yaw, sample.angular[2], 1e-3
+
+        # This should change the output angular velocity command to consider an axis-angle
+        # representation as expected and not an Euler rate as provided.
+        world_to_aligned.position_control = false
+
+        pose_samples.write pose_sample
+        cmd_in.write cmd
+        sample = assert_has_one_new_sample cmd_out, 1
+
+        # Expected result should be given by w = J_inv(roll, pitch) * euler_rate
+        expected_ang_vel = vector3d_from_array([0.5, 0.433, 1.25])
+        (0..3).each do |i|
+            assert_in_delta expected_ang_vel[i], sample.angular[i], 1e-3
+        end
+    end
 end