OrigamiRobot.m

classdef OrigamiRobot < handle
    properties
        OD (1, 1) double = 1 % Robot outer diameter
        s0 (1, 1) double = 1 % Robot initial length for rrt
        smin (1, 1) double = 1 % Robot minimal length
        smax (1, 1) double = 1 % Robot maximal length
    end

    methods
        function obj = OrigamiRobot(OD, s0, smin, smax)
            if nargin == 0
                return
            end
            obj.OD = OD;
            obj.s0 = s0;
            obj.smin = smin;
            obj.smax = smax;
        end

        %% Forward Kinematics
        function [T, kappa] = fkin(obj, s, theta, phi, base_transformation)
            % Calculate the forward kinematics of the origami robot: 
            %
            % s -- arc length (1x1 double)
            % theta -- bending angle (1x1 double in radians)
            % phi -- bending direction (1x1 double in radians)
            % base_transformation -- transformation matrix at base 
            % (4x4 SO(3) matrix)
            arguments
                obj
                s (1,1) double
                theta (1,1) double
                phi (1,1) double
                base_transformation (4,4) double
            end
            kappa = theta/s;
            if theta == 0
                T_temp = [1, 0, 0, 0; 
                          0, 1, 0, 0; 
                          0, 0, 1, s; 
                          0, 0, 0, 1];
                T = base_transformation * T_temp;
            else
                T11 = cos(phi)*cos(phi)*cos(theta) + sin(phi)*sin(phi);
                T12 = cos(phi)*sin(phi)*(cos(theta) - 1);
                T21 = T12;
                T13 = cos(phi)*sin(theta);
                T31 = -T13;
                T14 = cos(phi)*(1 - cos(theta))/kappa;
                T22 = sin(phi)*sin(phi)*cos(theta) + cos(phi)*cos(phi);
                T23 = sin(phi)*sin(theta);
                T32 = -T23;
                T24 = sin(phi) * (1-cos(theta))/kappa;
                T33 = cos(theta);
                T34 = sin(theta)/kappa;
                T41 = 0;
                T42 = 0;
                T43 = 0;
                T44 = 1;
                T = base_transformation * [T11, T12, T13, T14; 
                                           T21, T22, T23, T24; 
                                           T31, T32, T33, T34; 
                                           T41, T42, T43, T44];
            end
        end
        
        %% Forward kinematic 3 modules
        % function T = fkin_3modules(obj, parameters, T_base)
        %     arguments
        %         obj 
        %         parameters (3, 3) double
        %         T_base (4, 4) double
        %     end
        %     p11 = parameters(1,1); 
        %     p12 = parameters(2,1);
        %     p13 = parameters(3,1);
        %     p21 = parameters(1,2);
        %     p22 = parameters(2,2);
        %     p23 = parameters(3,2);
        %     p31 = parameters(1,3);
        %     p32 = parameters(2,3);
        %     p33 = parameters(3,3);
        % 
        %     % Calculate transformation matrices for each module
        %     T1 = obj.fkin(p11, p12, p13, T_base);
        %     T2 = obj.fkin(p21, p22, p23, T1);
        %     T3 = obj.fkin(p31, p32, p33, T2);
        % 
        %     % disp("T3 in 3module Fkin code:")
        %     % disp(T3)
        % 
        %     % Combine transformations to get the final transformation matrix
        %     T = T3;
        % 
        % end
        function T = fkin_nmodules(obj, parameters, T_base)
        % General forward kinematics for N serial continuum modules
        %
        % parameters : (3 x N) double
        %   parameters(:,i) = [S_i; Theta_i; Phi_i]
        %
        % T_base     : (4 x 4) double
        % T          : (4 x 4) double (end-effector transform)
        
            arguments
                obj
                parameters double
                T_base (4,4) double
            end
        
            % Validate parameter size
            assert(size(parameters,1) == 3, ...
                'parameters must be a 3xN matrix [S;Theta;Phi]');
        
            nModules = size(parameters,2);
        
            % Initialize transform
            T = T_base;
        
            % Chain forward kinematics
            for i = 1:nModules
                S     = parameters(1,i);
                Theta = parameters(2,i);
                Phi   = parameters(3,i);
        
                T = obj.fkin(S, Theta, Phi, T);
            end
        end
        
        %% Backbone Generation as an instance method
        function positions = backbone(obj, s, theta, phi, base_transformation, sampling)
            % Calculates the backbone location of the robot:
            %
            % s -- arc length
            % theta -- bending angle
            % phi -- bending direction
            % base_transformation -- transformation at base
            % sampling -- number of sampling points along backbone
            arguments
                obj
                s (1,1) double
                theta (1,1) double
                phi (1,1) double
                base_transformation (4,4) double
                sampling (1,1) double
            end
            nPoints = sampling;
            s_values = linspace(0, s, nPoints);
            positions = zeros(3, nPoints);  % Pre-allocate
            
            for i = 1:nPoints
                % Interpolate bending angle for current segment:
                theta_i = (s_values(i) / s) * theta;
                % Use forward kinematics for current arc length and angle:
                [T, ~] = obj.fkin(s_values(i), theta_i, phi, base_transformation);
                % Extract position from transformation matrix:
                positions(:, i) = T(1:3, 4);
            end
        end
        
        %% Frame Plotting as an instance method
        function frame(obj, T, scale)
            % draw the XYZ-frame at location:
            %
            % T -- current transformation
            % scale -- scaling factor for frame size
            arguments
                obj
                T (4,4) double = eye(4)
                scale (1,1) double = 0.5
            end
            origin_base = T(1:3,4);
            R_base = T(1:3,1:3);
            
            quiver3(origin_base(1), origin_base(2), origin_base(3), ...
                scale*R_base(1,1), scale*R_base(2,1), scale*R_base(3,1), ...
                'r', 'LineWidth',2, 'MaxHeadSize',0.5, 'HandleVisibility','off');
            quiver3(origin_base(1), origin_base(2), origin_base(3), ...
                scale*R_base(1,2), scale*R_base(2,2), scale*R_base(3,2), ...
                'g', 'LineWidth',2, 'MaxHeadSize',0.5, 'HandleVisibility','off');
            quiver3(origin_base(1), origin_base(2), origin_base(3), ...
                scale*R_base(1,3), scale*R_base(2,3), scale*R_base(3,3), ...
                'b', 'LineWidth',2, 'MaxHeadSize',0.5, 'HandleVisibility','off');
        end
        
        %% Robot Body
        function body = robot_body(obj, bb, NBetween, NArround)
            % Draw robot body based on OD and backbones
            % Requires add-on function: gencyl
            %   
            % bb -- backbone
            % NBetween -- see documentation of gencyl
            % NArround -- see documentation of gencyl

            arguments
                obj
                bb
                NBetween (1,1) double = 2
                NArround (1,1) double = 20
            end
            % Constructs a robot body by sweeping a circular cross-section along a backbone.
            % "radii" is defined as OD/2 at every backbone point.
            radii = (obj.OD/2) * ones(1, size(bb, 2));
            [X, Y, Z] = gencyl(bb, radii, NBetween, NArround);
            body.X = X;
            body.Y = Y;
            body.Z = Z;
        end

        %% Create mesh for Collision Detection
        function mesh_ans = mesh(obj, body)
            % Create the robot body mesh in definition of OcTree Collision
            % Detection:
            %
            % body -- output of robot_body
            arguments
                obj 
                body 
            end
            [w, l] = size(body.X');
            mesh_ans = [reshape(body.X', [w*l, 1]), reshape(body.Y', [w*l, 1]), reshape(body.Z', [w*l, 1])];
        end

        
        %% Generate Parameters based on Cable Length
        function [S, theta, phi] = CableParameters(obj, l1, l2, l3)
            % Create the robot parameters for Fkin based on cable length
            arguments
                obj 
                l1 
                l2 
                l3 
            end
            S = (3 * obj.smin + l1 + l2 + l3)/3;
            theta = 2 * sqrt(l1^2 + l1^2 + l1^2 - l1*l2 -l1*l3 - l2*l3)/ (3 * 0.12);
            % phi = atan(sqrt(3) * (l3 - l2) / (l2 + l3 - 2*l1));
            phi = 2 * pi * rand(1);

        end
        
        


    end
end