WIP for adding old model sl_ibvs.slx to the Toolbox

Refactored the Simulink library roblocks into 3 sublibraries

Author: petercorke

toolbox/generallib.slx

@@ -0,0 +1,51 @@
+function p = slcamera(cam, u)
+    if all(u == 0)
+        % Simulink is probing for the dimensions
+        np = (length(u)-16)/3;
+        p = zeros(np, 2);
+    else
+        P = reshape(u(17:end), [], 3);
+        Tcam = reshape(u(1:16), 4, 4);
+        p = cam.plot(P, pose=Tcam);
+        drawnow;
+    end
+
+% function slcamera(block)
+%     setup(block);
+% end
+% 
+% function setup(block)
+%     % Set number of input ports
+%     block.NumInputPorts = 1;
+%     block.InputPort(1).DimensionsMode = 'Variable'; % Allow variable dimensions
+%     block.InputPort(1).DirectFeedthrough = true;
+% 
+%     % Set the number of output ports
+%     block.NumOutputPorts = 1; % Assuming one output port
+%     block.OutputPort(1).Dimensions = 2; % Example output dimensions, adjust as needed
+% 
+%     % Set block sample time
+%     block.SampleTimes = [0 0]; % Continuous
+% 
+%     % Set the block methods
+%     block.RegBlockMethod('InitializeConditions', @mdlInitializeConditions);
+%     block.RegBlockMethod('Outputs', @mdlOutputs);
+% end
+% 
+% function mdlInitializeConditions(block)
+%     % Initialize conditions if needed
+% end
+% 
+% function mdlOutputs(block)
+%     u = block.InputPort(1).Data; % Get input data
+%     if all(u == 0)
+%         % Simulink is probing for the dimensions
+%         np = (length(u) - 16) / 3;
+%         p = zeros(np, 2);
+%     else
+%         P = reshape(u(17:end), [], 3);
+%         Tcam = reshape(u(1:16), 4, 4);
+%         p = cam.plot(P, 'pose', Tcam, 'drawnow');
+%     end
+%     block.OutputPort(1).Data = p; % Set output data
+% end

toolbox/mobilelib.slx

@@ -0,0 +1,50 @@
+%STRPRINT	S-function for transformation display in a block
+%
+% Input is the SE2 or SE3 transform to display.
+
+
+function sltrprint(block)
+    setup(block);
+end
+
+function setup(block)
+    % Set number of dialog parameters
+    block.NumDialogPrms = 1; % Example for options, adjust as needed
+
+    % Set the number of input ports
+    block.NumInputPorts = 1;
+    block.InputPort(1).DimensionsMode = 'Fixed'; % Allow variable dimensions
+    % block.InputPort(1).DirectFeedthrough = true;
+
+    % Set the number of output ports
+    block.NumOutputPorts = 0; % No outputs
+
+    % Set block sample time
+    block.SampleTimes = [0 0]; % Continuous
+
+    % Set the block methods
+    block.RegBlockMethod('InitializeConditions', @mdlInitializeConditions);
+    block.RegBlockMethod('Outputs', @mdlOutputs);
+end
+
+function mdlInitializeConditions(block)
+    % Initialize conditions if needed
+end
+
+function mdlOutputs(block)
+    u = block.InputPort(1).Data; % Get input data
+    options = block.DialogPrm(1).Data; % Get options from dialog
+
+    if ~isempty(u)
+        if length(u) == 3
+            s = printtform2d(trvec2tform(u(1:2)) * rotm2d(u(3)), options{:});
+        elseif istform2d(u)
+            s = printtform2d(u, options{:});
+        elseif istform(u)
+            s = printtform(u);
+        else
+            error('RVC:sltrprint', 'unknown type passed, expecting 3x1, 3x3, 4x4');
+        end
+        set_param(block.BlockHandle, MaskDisplay=sprintf("disp('%s');", s));
+    end
+end