Trajectory.m

Go to the documentation of this file.

% ==================================================================
%> @brief Trajectory Class for generating and manipulating trajectories
%> from waypoints.
%>
%> The Trajectory class is used to generate and manipulate trajectories
%> from waypoints. The waypoints are defined by their start time, stay
%> time, position, and orientation. The trajectory is generated by
%> interpolating the position and orientation between the waypoints.
%>
%> @code
%> % Example usage:
%> % Create a Trajectory object
%> traj = Trajectory();
%>
%> % Add waypoints to the trajectory
%> t_start = seconds([0, 5, 10, 15]);
%> t_stay  = seconds([5, 5, 5, 5]);
%> pos     = [0,0,0; 1,1,1; 2,2,2; 3,3,3];
%> ori     = quaternion([1,0,0,0; 0.7071,0,0.7071,0; 0.7071,0,0,0.7071; 0,0,0,1]);
%> traj.addWaypoints(t_start, t_stay, pos, ori);
%>
%> % Plot the waypoints
%> traj.plotWaypoints();
%>
%> % Generate the trajectory
%> traj.generateTrajectory();
%>
%> % Get the trajectory
%> traj.getTrajectory();
%> @endcode
%>
%> see also the more detailed example in @ref trajectoryExample.m
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository 
% ==================================================================
classdef Trajectory < handle
    
    properties (Constant, Access = private)
        %> names of the variables in the waypoints table
        tabVarNames = ["t_start", "t_stay","pos","ori"];
        
        %> types of the variables in the waypoints table
        tabVarTypes = ["duration", "duration","cell","quaternion"];
        
        %> available interpolation methods for position
        interpMethodsPos = ["minJerk","minSnap","linear","spline","pchip","makima"];
        
        %> available interpolation methods for orientation
        interpMethodsOri = ["linear","spline","pchip","makima"];
        
        %> names of the variables in the trajectory table
        trajVarNames = ["pos","vel","acc","jerk","ori","gyr"];
        
        %> types of the variables in the trajectory table
        trajVarTypes = ["cell","cell","cell","cell","quaternion", "cell"];
        
        %> units of the variables in the trajectory table
        trajVarUnits = ["m","m/s","m/s^2","m/s^3","quaternion", "deg/s"];
    end
    
    properties (Access = private)
        %> sampling frequency in Hz
        fs = 200;
        
        %> waypoints table
        tab
        
        %> trajectory table
        traj
        
        %> interpolation method of the position
        interpMethodPos
        
        %> interpolation method of the orientation
        interpMethodOri
        
        %> flag if the trajectory is generated and up to date
        isGenerated = false;
    end
    
    methods (Access = public)
        % ==================================================================
        %> @brief Trajectory Class constructor
        %>
        %> @param options (optional) - Structure specifying custom settings for the Trajectory object.
        %>   - options.fs (optional) - Sampling frequency in Hz (default: 200)
        %>
        %> @retval obj - The created Trajectory object.
        % ==================================================================
        function obj = Trajectory(options)
            
            arguments
                options.fs (1,1) double = 200
            end
            
            obj.fs  = options.fs;
            obj.tab = table('Size',[0,numel(obj.tabVarNames)],'VariableNames', obj.tabVarNames,'VariableTypes', obj.tabVarTypes); % waypoints table
            
            obj.interpMethodPos = obj.interpMethodsPos(1);
            obj.interpMethodOri = obj.interpMethodsOri(1);
        end
 
        % ==================================================================
        %> @brief get a copy of the Trajectory object
        %>
        %> @param None
        %>
        %> @retval obj - Copy of the Trajectory object
        %> ==================================================================
        function obj = copy(obj)
            obj = Trajectory();
            obj.fs = obj.fs;
            obj.tab = obj.tab;
            obj.traj = obj.traj;
            obj.interpMethodPos = obj.interpMethodPos;
            obj.interpMethodOri = obj.interpMethodOri;
            obj.isGenerated = obj.isGenerated;
        end
        
        % ==================================================================
        %> @brief Add waypoints to the trajectory
        %>
        %> addWaypoints(obj,t_start,t_stay,pos,ori) adds waypoints to the
        %> trajectory from the inputs. The waypoints are defined by their
        %> start time, stay time, position, and orientation.
        %>
        %> @param t_start - Start time of each waypoint (duration array)
        %> @param t_stay  - Duration of each waypoint (duration array)
        %> @param pos     - Position of each waypoint (Nx3 double array)
        %> @param ori     - Orientation of each waypoint (quaternion array)
        %>
        %> @retval None
        %>
        %> @note The inputs must have the same height.
        % ==================================================================
        function addWaypoints(obj,t_start,t_stay,pos,ori)
            
            arguments
                obj
                t_start (:,1) duration
                t_stay (:,1) duration
                pos (:,3) double
                ori (:,1) quaternion
            end
            
            % make sure the height of the inputs is the same
            n = height(t_start);
            assert(all([height(t_stay),height(pos),height(ori)] == n), "The inputs must have the same height");
            
            % try to add the waypoints to the table
            try
                if isempty(obj.tab)
                    obj.tab = table(t_start, t_stay, pos, ori, 'VariableNames', obj.tabVarNames);
                else
                    obj.tab = [obj.tab; table(t_start, t_stay, pos, ori, 'VariableNames', obj.tabVarNames)];
                end
            catch
                error("The inputs are not compatible with the waypoints table");
            end
            
            % flag the trajectory as not generated
            obj.isGenerated = false;
        end
        
        % ==================================================================
        %> @brief set the sampling frequency
        %>
        %> @param fs - Sampling frequency in Hz (positive integer)
        %>
        %> @retval None
        %>
        %> @note The Sampling frequency is used to generate the trajectory
        % ==================================================================
        function setSamplingFrequency(obj,fs)
            
            arguments
                obj
                fs (1,1) double {mustBePositive,mustBeInteger}
            end
            
            % update the sampling frequency
            obj.fs = fs;
            
            % flag the trajectory as not generated
            obj.isGenerated = false;
        end
        
        % ==================================================================
        %> @brief get the sampling frequency
        %>
        %> @param None
        %>
        %> @retval fs - Sampling frequency in Hz
        % ==================================================================
        function fs = getSamplingFrequency(obj)
            fs = obj.fs;
        end
        
        % ==================================================================
        %> @brief plot the waypoints
        %>
        %> plotWaypoints(obj,ax,options) plots the waypoints on the
        %> specified axes using the custom settings specified by the options
        %> structure. The resulting plot is a 3D plot with the waypoints
        %> represented by a model of the JUMP sensor and a line connecting the
        %> waypoints.
        %>
        %> @param ax      (optional) - Axes to plot the waypoints on
        %> @param options (optional) - Structure specifying custom settings for the plot
        %>   - options.scale   (optional) - Scale factor for the waypoints (default: 10)
        %>   - options.numbers (optional) - Flag to display the waypoint numbers (default: true)
        %>   - options.line    (optional) - Flag to display a line between the waypoints (default: true)
        %>
        %> @retval fig - Figure handle of the plot
        % ==================================================================
        function fig = plotWaypoints(obj, ax, options)
            
            arguments
                obj
                ax            = gca
                options.scale   (1,1) double  = 10
                options.numbers (1,1) logical = true
                options.line    (1,1) logical = true
            end
            
            % Plot each waypoint
            for i = 1:height(obj.tab)
                poseplot(ax, obj.tab.ori(i), obj.tab.pos(i,:), "NED",...
                    MeshFileName   = "jumpSimplified.stl", PatchFaceAlpha = 0.8, ...
                    PatchFaceColor = [0,0.57,0.82],        ScaleFactor    = options.scale);
                hold on
                
                % plot the waypoint numbers above the waypoints
                if options.numbers
                    offset = 0.02*options.scale; % make sure the numbers are not hidden by the mesh
                    text(obj.tab.pos(i,1)-offset,obj.tab.pos(i,2)+offset,obj.tab.pos(i,3)-offset,...
                        num2str(i),'FontSize',12, 'Color', 'white','FontWeight','bold','BackgroundColor','black');
                end
            end
            xlabel('X [m]');
            ylabel('Y [m]');
            zlabel('Z [m]');
            
            % display a line between the waypoints with changing color
            if options.line
                colors = hsv(height(obj.tab)-1); % generate a colormap based on the number of waypoints
                for i = 1:height(obj.tab)-1
                    plot3(ax, obj.tab.pos(i:i+1,1), obj.tab.pos(i:i+1,2), obj.tab.pos(i:i+1,3), 'Color', colors(i,:), 'LineWidth', 2);
                    hold on;
                end
                
                % add a colorbar to the figure
                colormap(colors);
                clim([1,height(obj.tab)]);
                c                = colorbar("Direction","reverse");
                c.Label.String   = 'Waypoint number';
                c.Label.FontSize = 14;
                c.Ticks          = 1:height(obj.tab);
                c.Limits         = [1,height(obj.tab)];
            end
            
            hold off;
            
            fig = ax.Parent;
        end
        
        % ==================================================================
        %> @brief return the waypoints table in a reduced format
        %>
        %> @param None
        %>
        %> @retval tab - Waypoints table in a reduced format
        %>
        %> @note This format is used in the trajectoryGenerationApp.
        % ==================================================================
        function tab = reduce(obj)
            
            if height(obj.tab) == 0
                tab = table('Size',[0,9],'VariableNames',["t_start","t_stay","x","y","z","q","i","j","k"],'VariableTypes',["string","string","double","double","double","double","double","double","double"]);
                return
            end
            
            t_start = string(obj.tab.t_start,"mm:ss.SSS");
            dur     = string(obj.tab.t_stay,"mm:ss.SSS");
            
            x = obj.tab.pos(:,1);
            y = obj.tab.pos(:,2);
            z = obj.tab.pos(:,3);
            quat = obj.tab.ori.compact;
            q= quat(:,1);
            i= quat(:,2);
            j= quat(:,3);
            k= quat(:,4);
            
            tab = table(t_start,dur,x,y,z,q,i,j,k,'VariableNames',["t_start","t_stay","x","y","z","q","i","j","k"]);
        end
        
        % ==================================================================
        %> @brief import a waypoints table
        %>
        %> @param tab - Waypoints table
        %>
        %> @retval None
        %>
        %> @note this is used in the trajectoryGenerationApp. The table must
        %> have the same format as the one returned by the reduce method.
        % ==================================================================
        function importTab(obj,tab)
            
            t_start = duration(tab.t_start,"InputFormat","mm:ss.SSS");
            t_stay  = duration(tab.t_stay,"InputFormat","mm:ss.SSS");
            
            pos = [tab.x,tab.y,tab.z];
            ori = quaternion(tab.q,tab.i,tab.j,tab.k);
            
            obj.tab = table(t_start, t_stay, pos, ori, 'VariableNames', obj.tabVarNames);
            
            % flag the trajectory as not generated
            obj.isGenerated = false;
        end
        
        % ==================================================================
        %> @brief export the waypoints table
        %>
        %> @param None
        %>
        %> @retval tab - Waypoints table
        % ==================================================================
        function tab = getTab(obj)            
            tab = obj.tab;
        end
 
        % ==================================================================
        %> @brief set the waypoints table
        %>
        %> @param tab - Waypoints table
        %>
        %> @retval None
        % ==================================================================
        function setTab(obj,tab)
            obj.tab = tab;
            
            % flag the trajectory as not generated
            obj.isGenerated = false;
        end
        
        % ==================================================================
        %> @brief sort the waypoints table in ascending order of start time
        %>
        %> @param None
        %>
        %> @retval None
        % ==================================================================
        function sortTab(obj)
            % SORTTAB Sort the waypoints table by start time
            
            obj.tab = sortrows(obj.tab,1);
        end
        
        % ==================================================================
        %> @brief clean the waypoints table
        %>
        %> cleanTab(obj) removes duplicate waypoints, rounds the start and
        %> stay times to the nearest multiple of the sampling time, and checks
        %> for time overlapping. The resulting table is sorted by start time.
        %>
        %> @param None
        %>
        %> @retval None
        % ==================================================================
        function cleanTab(obj)
            
            % backup the table
            tab_backup = obj.tab;
            
            % make sure the table is sorted
            obj.sortTab();
            
            % if the smallest t_start is not 0, add a waypoint at the origin
            if obj.tab.t_start(1) > seconds(0)
                obj.addWaypoints(seconds(0),seconds(0),[0,0,0],quaternion(1,0,0,0));
                obj.sortTab(); % make sure the new waypoint is at the top
            end
            
            % remove duplicate waypoints (same t_start)
            [~,idx] = unique(obj.tab.t_start);
            if isempty(idx) && height(obj.tab) > 0
                idx = 1;
            end
            obj.tab = obj.tab(idx,:);
 
            % round the start time to the nearest multiple of the sampling time
            obj.tab.t_start = round(obj.tab.t_start/seconds(1/obj.fs))*seconds(1/obj.fs);
            
            % round the stay time to the nearest multiple of the sampling time
            obj.tab.t_stay = round(obj.tab.t_stay/seconds(1/obj.fs))*seconds(1/obj.fs);
            
            % check for time overlapping (t_start + t_stay must be < t_start of the next waypoint)
            for i = 1:height(obj.tab)-1
                if obj.tab.t_start(i) + obj.tab.t_stay(i) > obj.tab.t_start(i+1) - seconds(2/obj.fs)
                    % reduce the t_stay of the current waypoint
                    obj.tab.t_stay(i) = obj.tab.t_start(i+1) - obj.tab.t_start(i) - seconds(2/obj.fs);
                end
            end
            
            % flag the trajectory as not generated if the table has changed
            if ~isequal(tab_backup,obj.tab)
                obj.isGenerated = false;
            end
        end
        
        % ==================================================================
        %> @brief get the interpolation methods
        %>
        %> @param None
        %>
        %> @retval methods - Structure containing the available interpolation
        %> methods for position and orientation, as well as the currently
        %> selected methods.
        % ==================================================================
        function methods = getInterpMethods(obj)
            
            methods.pos        = obj.interpMethodsPos;
            methods.ori        = obj.interpMethodsOri;
            methods.posCurrent = obj.interpMethodPos;
            methods.oriCurrent = obj.interpMethodOri;
        end
 
        % ==================================================================
        %> @brief set the interpolation methods
        %>
        %> setInterpMethods(obj,methods) sets the interpolation methods for
        %> position and orientation.
        %>
        %> @param methods.pos (optional) - Interpolation method for position
        %> @param methods.ori (optional) - Interpolation method for orientation
        %>
        %> @retval None
        %>
        %> @note The interpolation methods must be part of the available
        %> interpolation methods. The available interpolation methods can be
        %> obtained by calling the getInterpMethods() method.
        % ==================================================================
        function setInterpMethods(obj,methods)
            
            if isfield(methods,"pos")
                if ismember(methods.pos,obj.interpMethodsPos)
                    obj.interpMethodPos = methods.pos;
                end
            end
            if isfield(methods,"ori")
                if ismember(methods.ori,obj.interpMethodsOri)
                    obj.interpMethodOri = methods.ori;
                end
            end
            
            % flag the trajectory as not generated
            obj.isGenerated = false;
        end
        
        % ==================================================================
        %> @brief Generate the trajectory from the waypoints table
        %>
        %> generateTrajectory(obj) generates the trajectory from the waypoints
        %> table using the default settings. For the generation of the
        %> trajectory, the position is interpolated to get a trajectory with
        %> the selected sampling frequency matching the waypoints.
        %>
        %> generateTrajectory(obj, options) generates the trajectory from the
        %> waypoints table using custom settings specified by the options
        %> structure.
        %>
        %> @param options (optional) - Custom settings for the trajectory
        %> generation
        %> @param options.pos (optional) - Interpolation method for
        %> position
        %> @param options.ori (optional) - Interpolation method for
        %> orientation
        %> @param options.fs (optional) - Sampling frequency in Hz
        % ==================================================================
        function generateTrajectory(obj, options)
            
            arguments
                obj
                options.pos (1,1) string = obj.interpMethodPos
                options.ori (1,1) string = obj.interpMethodOri
                options.fs  (1,1) double = obj.fs
            end
            
            % update the settings of the obj from the options
            obj.setSamplingFrequency(options.fs);
            obj.setInterpMethods(options);
            
            % make sure the table is clean
            obj.cleanTab();
            
            % get the time limits of the trajectory
            traj_t_start = obj.tab.t_start(1);
            traj_t_end   = obj.tab.t_start(end) + obj.tab.t_stay(end);
            
            % generate the time vector
            t = (traj_t_start:seconds(1/obj.fs):traj_t_end)';
            
            % generate the trajectory table
            obj.traj = timetable(...
                t,... % time vector
                nan(height(t),3),... % position
                nan(height(t),3),... % velocity
                nan(height(t),3),... % acceleration
                nan(height(t),3),... % jerk
                quaternion.zeros(height(t),1),... % orientation
                nan(height(t),3),... % angular velocity
                'VariableNames',obj.trajVarNames);
            
            obj.traj.Properties.VariableUnits = obj.trajVarUnits;
            
            % fill the known values and save the start and end indices of each waypoint
            idx_start = zeros(height(obj.tab),1);
            idx_end   = zeros(height(obj.tab),1);
            
            for i = 1:height(obj.tab)
                % find the indices of the current waypoint in the trajectory table
                idx_start(i) = find(t >= obj.tab.t_start(i),1);
                idx_end(i)   = find(t >= obj.tab.t_start(i) + obj.tab.t_stay(i),1);
                
                % fill the position
                obj.traj.pos(idx_start(i):idx_end(i),:) = repmat(obj.tab.pos(i,:),idx_end(i)-idx_start(i)+1,1);
                
                % fill the orientation
                obj.traj.ori(idx_start(i):idx_end(i)) = repmat(obj.tab.ori(i),idx_end(i)-idx_start(i)+1,1);
            end
            
            % interpolate the position, velocity, acceleration and jerk
            switch obj.interpMethodPos
                case {"minJerk","minSnap"}
                    assert(numel(idx_start) == numel(idx_end), "The number of start and end indices must be the same");
                    
                    
                    % explanation of the usage of a while loop and the iExtra variable:
                    % if the waypoint has no stay time, the position should not be
                    % slowed down to 0 at the end of the waypoint. Therefore, we
                    % interpolate trough the waypoints with no stay time and
                    % increment the index by the number of waypoints with no stay time
                    i = 1;
                    while (i < numel(idx_start))
                        
                        % handle waypoints with no stay time
                        iExtra = 0;
                        while i+iExtra < numel(idx_start)-1
                            if idx_start(i+iExtra+1) == idx_end(i+iExtra+1)
                                iExtra = iExtra + 1;
                            else
                                break;
                            end
                        end
                        idx = zeros(1,iExtra+2);
                        idx(1) = idx_end(i);
                        for j=1:iExtra+1
                            idx(j+1) = idx_start(j+i);
                        end
                        
                        switch obj.interpMethodPos
                            case "minJerk"
                                [pos, vel, acc, jerk, ~, ~, t] = minjerkpolytraj(obj.traj.pos(idx,:)', seconds(obj.traj.t(idx))', idx(end)-idx(1));
                                
                            case "minSnap"
                                [pos, vel, acc, jerk, ~, ~, ~, t] = minsnappolytraj(obj.traj.pos(idx,:)', seconds(obj.traj.t(idx))', idx(end)-idx(1));
                                
                        end
                        
                        % since the functions do not sample at the exact wanted time, we need to interpolate the results
                        tt = timetable(seconds(t'),pos',vel',acc',jerk','VariableNames',["pos","vel","acc","jerk"]);
                        tt = retime(tt, obj.traj.t(idx(1):idx(end)), 'linear');
                        
                        % fill the trajectory table
                        assert(all(tt.Time == obj.traj.t(idx(1):idx(end))), "The time vectors do not match");
                        obj.traj.pos(idx(1):idx(end),:)  = tt.pos;
                        obj.traj.vel(idx(1):idx(end),:)  = tt.vel;
                        obj.traj.acc(idx(1):idx(end),:)  = tt.acc;
                        obj.traj.jerk(idx(1):idx(end),:) = tt.jerk;
                        
                        % increment the index
                        i = i + iExtra + 1;
                        
                    end
                    
                case {"linear","spline","pchip","makima"}
                    % interpolate the actual position
                    obj.traj.pos = fillmissing(obj.traj.pos,obj.interpMethodPos,1);
                    
                    % find the velocity, acceleration, and jerk from the position
                    obj.traj.vel  = gradient(obj.traj.pos',1/obj.fs)';
                    obj.traj.acc  = gradient(obj.traj.vel',1/obj.fs)';
                    obj.traj.jerk = gradient(obj.traj.acc',1/obj.fs)';
                    
                    % Suppress extremely small values
                    threshold = 1e-10;
                    obj.traj.vel(abs(obj.traj.vel) < threshold) = 0;
                    obj.traj.acc(abs(obj.traj.acc) < threshold) = 0;
                    obj.traj.jerk(abs(obj.traj.jerk) < threshold) = 0;
                    
                    
            end
            
            % interpolate the orientation
            f = nan(height(obj.traj),1);
            f(idx_end)   = 0:numel(idx_end)-1; %end of the waypoint = start of the orientation change
            f(idx_start) = 0:numel(idx_end)-1; %start of the waypoint = end of the orientation change
            
            % fill the missing values during a stay time
            for i=1:numel(idx_start)
                f(idx_start(i):idx_end(i)) = fillmissing(f(idx_start(i):idx_end(i)),"nearest");
            end
            
            % add values to start and end to smooth beginning and stop
            f = [zeros(10,1); f; ones(10,1)*max(f)];
            
            % fill the missing values with the interpolation method
            f = fillmissing(f,obj.interpMethodOri,1);
            
            % remove the added ones and zeros
            f = f(11:end-10);
            
            % restrict to numbers greater than one
            f(f > 1) = f(f > 1) - floor(f(f > 1));
            
            for i = 1: numel(idx_start)-1
                idx = [idx_end(i), idx_start(i+1)];
                
                % define the normalized amount of the transformation
                this_f = f(idx(1):idx(2));
                
                % make sure, beginning and end ar correct (problem occurs
                % with zero stay waypoints)
                this_f(1) = 0;
                this_f(end) = 1;
                
                % interpolate
                ori  = nan(numel(this_f),4);
                % NOTE : can also be a parfor loop
                for j    = 1:numel(this_f)
                    ori(j,:) = quatinterp(obj.traj.ori(idx(1)).normalize.compact,obj.traj.ori(idx(2)).normalize.compact,this_f(j),"slerp");
                end
                
                % fill the trajectory table
                obj.traj.ori(idx(1):idx(2)) = quaternion(ori);
                
            end
            
            % fill the missing values
            obj.traj.ori = obj.quaternionFillMissing(obj.traj.ori);
            obj.traj.gyr = angvel(obj.traj.ori,1/obj.fs,"frame") * 180/pi;
            obj.traj(:,1:end-2) = fillmissing(obj.traj(:,1:end-2),"nearest");
            
            % flag the trajectory as generated
            obj.isGenerated = true;
            
        end
        
        % ==================================================================
        %> @brief get the trajectory timetable
        %>
        %> @param None
        %>
        %> @retval traj - Trajectory timetable
        % ==================================================================
        function traj = getTrajectory(obj)
            if ~obj.isGenerated
                obj.generateTrajectory();
            end
            traj = obj.traj;
        end
        
        function isGenerated = getIsGenerated(obj)
            isGenerated = obj.isGenerated;
        end
    end
    
    methods (Access = private, Static)
 
        % ==================================================================
        %> @brief fill missing values in a quaternion array
        %>
        %> @param q - Quaternion array
        %>
        %> @retval qFilled - Quaternion array with filled missing values
        % ==================================================================
        function qFilled = quaternionFillMissing(q)
            arguments
                q (:,1) quaternion
            end
        
            n = length(q);
            qFilled = q; % Initialize filled quaternion vector with original values
            
            % Identify zero quaternions
            isZero = arrayfun(@(x) isequal(x, quaternion.zeros(1)), q);
            isZeroIdx = find(isZero);
            
            for i = 1:numel(isZeroIdx) % Iterate over indices of zero quaternions                
                for j = 1:n/2
 
                    % check previous values
                    prevIdx = isZeroIdx(i) - j;
                    if prevIdx > 0
                        if ~isZero(prevIdx)
                            qFilled(isZeroIdx(i)) = q(prevIdx);
                            break;
                        end
                    end
 
                    % check next values
                    nextIdx = isZeroIdx(i) + j;
                    if nextIdx <= n
                        if ~isZero(nextIdx)
                            qFilled(isZeroIdx(i)) = q(nextIdx);
                            break;
                        end
                    end
                end
            end
        end
 
    end
end