Add visualizers (Robot Visualizer) and sensors libraries (IMUsensor)

app/robots/iCubGenova04/configRobot.m

@@ -63,6 +63,7 @@
 
 % Robot frames list
 Frames.BASE = 'root_link';
+Frames.IMU = 'imu_frame';
 Frames.COM = 'com';
 Frames.LEFT_FOOT = 'l_sole';
 Frames.RIGHT_FOOT = 'r_sole';

lib/+mwbs/+RobotSensors/+IMUsensor/IMUsensorProc.m

@@ -0,0 +1,76 @@
+classdef IMUsensorProc < matlab.System
+    % IMUsensorProc This block computes the typical outputs of an IMU from the base pose and joints kinematics.
+    % 
+    % Assuming DoFs is the number of internal degrees of freedom of the robot:
+    % 
+    % Input:
+    %   - w_H_b: Base pose, 4x4 matrix representing the homogenous transformation between the the
+    %   base frame and the world frame.
+    %   - nu: Base and joints velocities: Vector of size 6 representing the linear and angular
+    %   velocity of the base frame, concatenated with a vector of size DoFs, representing the
+    %   velocity of the joints.
+    %   - nuDot: Base and joints accelerations: Vector of size 6 representing the linear and angular
+    %   acceleration of the base frame, concatenated with a vector of size DoFs, representing the
+    %   acceleration of the joints.
+    %   - Jimu: Jacobian of the IMU frame.
+    %   - JimuDotNu: Bias linear acceleration of the IMU frame with respect to the inertial (world)
+    %   frame.
+    % 
+    % Output:
+    %   The sensor frame orientation, linear acceleration and angular velocity w.r.t. the inertial
+    %   (world) frame. Linear acceleration and angular velocity are expressed in the world frame.
+    %   - w_rollPitchYaw: roll, pitch, yaw Euler angles in rad.
+    %   - w_linAcc: Proper linear acceleration in m/s^2.
+    %   - w_omega: Angular velocity in rad/s.
+    %   - imuOut: A [12x1] 1-D vector concatenating the previous three outputs and a placeholder for
+    %   other measurements (e.g. magnetometer).
+    %   [roll,pitch,yaw,accx,accy,accz,omegax,omegay,omegaz,0,0,0]'.
+    %   
+    % Parameters:
+    %   - GRAVITY_VECTOR: [3x1] [m/s^2] gravity vector retrieved from the closest ConfigIm block.
+
+    % Public, tunable properties
+    properties (Nontunable)
+        GRAVITY_VECTOR;
+    end
+
+    properties(DiscreteState)
+
+    end
+
+    % Pre-computed constants
+    properties(Access = private)
+
+    end
+
+    methods(Access = protected)
+        function setupImpl(obj)
+            % Perform one-time calculations, such as computing constants
+        end
+
+        function [imuOut,w_omega,w_linAcc,w_rollPitchYaw] = stepImpl(obj,Jimu,JimuDotNu,w_H_imu,nu,nudot)
+            % Implement algorithm. Calculate y as a function of inputs and discrete states.
+
+            % Sensor frame orientation
+            R = mwbs.State.H2Rp(w_H_imu);
+
+            % Gyroscope measurements
+            w_imuVel = Jimu*nu;
+            w_omega = R'*w_imuVel(4:6);
+
+            % Accelerometer measurements
+            w_imuAcc = Jimu*nudot + JimuDotNu;
+            w_linAcc = R'*(w_imuAcc(1:3)-obj.GRAVITY_VECTOR);
+
+            % Euler angles estimation
+            w_rollPitchYaw = wbc.rollPitchYawFromRotation(R);
+
+            % composite sensor output
+            imuOut = [w_rollPitchYaw; w_linAcc; w_omega; zeros(3,1)];
+        end
+
+        function resetImpl(~)
+            % Initialize / reset discrete-state properties
+        end
+    end
+end

src/robotVisualizer/Visualizer.m → lib/+mwbs/+Visualizers/+robotVisualizer/RobotVisualizer.m

@@ -1,6 +1,6 @@
-classdef Visualizer < matlab.System
-    % Visualizer matlab.System handling the robot visualization.
-    % go in app/robots/iCub*/initVisualizer.m to change the setup config
+classdef RobotVisualizer < matlab.System
+    % RobotVisualizer matlab.System handling the robot visualization.
+    % go in app/robots/iCub*/initRobotVisualizer.m to change the setup config
 
     properties (Nontunable)
         config

lib/+mwbs/+Visualizers/+robotVisualizer/initRobotVisualizer.m

@@ -0,0 +1,3 @@
+%% configuration for the matlab iDyntree visualizer
+
+confVisualizer.modelPath = mwbs.getModelPathFromFileNameAndYarpFinder(confVisualizer.fileName);

lib/+mwbs/+RobotDynamicsWithContacts/slblocks.m → lib/slblocks.m

@@ -3,9 +3,9 @@
 		% in the Library Browser
 		% and be cached in the browser repository
 
-		Browser.Library = 'robotDynamicsWithContacts_lib'; % name of the library
+		Browser.Library = 'mwbs_lib'; % name of the library
 
-		Browser.Name = 'RobotDynamicsWithContacts'; % library name that appears in the Library Browser
+		Browser.Name = 'Matlab Whole-body Simulator'; % library name that appears in the Library Browser
 
         Browser.IsFlat  =  0;
 

src/robotVisualizer/initVisualizer.m → src/initVisualizer.m

@@ -1,11 +1,11 @@
 %% configuration for the matlab iDyntree visualizer
 
 confVisualizer.fileName = robot_config.fileName;
-confVisualizer.modelPath = mwbs.getModelPathFromFileNameAndYarpFinder(confVisualizer.fileName);
 confVisualizer.meshFilePrefix = robot_config.meshFilePrefix;
 confVisualizer.jointOrder = robot_config.jointOrder;
 confVisualizer.robotFrames = robot_config.robotFrames;
-% initial infos specified in configRobot
+
+% initial joints configuration specified in configRobot
 confVisualizer.joints_positions = robot_config.initialConditions.s;
 confVisualizer.world_H_base = robot_config.initialConditions.w_H_b;