classdef manipulatorCHOMP < robotics.manip.internal.InternalAccess
%MANIPULATORCHOMP CHOMP for rigid body tree
%   Covariant Hamiltonian Optimization for Motion Planning (CHOMP) for
%   rigid body tree optimizes trajectories for smoothness, and collision
%   avoidance.
%
%   MANIPCHOMP = manipulatorCHOMP(RIGIDBODYTREE) Creates a CHOMP based
%   optimizer for a rigid body tree object of type rigidBodyTree.
%
%   MANIPCHOMP = manipulatorCHOMP(RIGIDBODYTREE,PropertyName=PropertyValue)
%   sets properties on the object using name-value arguments.
%
%   manipulatorCHOMP properties:
%      SmoothnessOptions      - Options associated with smoothness cost
%      CollisionOptions       - Options associated with collision cost
%      SolverOptions          - Options associated with the solver
%      RigidBodyTree          - Rigid body tree used for motion planning
%      RigidBodyTreeSpheres   - Spheres of bodies in rigid body tree
%      SphericalObstacles     - Spherical obstacles in environment
%      MeshTSDF               - MeshTSDF representation of an environment
%
%   manipulatorCHOMP methods:
%       optimize - Optimize a trajectory
%       show     - Visualize trajectory of rigid body tree
%
%   Example:
%       %Load a robot model into the workspace, and create a CHOMP solver.
%       robot = loadrobot("kinovaGen3",DataFormat="row");
%       chomp = manipulatorCHOMP(robot);
%
%       %Create spheres to represent obstacles, and
%       %add them to the CHOMP solver.
%       env = [0.20 0.2 -0.1 -0.1;% sphere, radius 0.20 at (0.2,-0.1,-0.1)
%           0.15 0.2  0.0  0.5]'; % sphere, radius 0.15 at (0.2,0.0,0.5)
%       chomp.SphericalObstacles = env;
%
%       %To prioritize a collision-free trajectory, set the smoothness cost
%       %weight to a lower value than the collision cost weight. Then add
%       %the options to the CHOMP solver.
%       chomp.SmoothnessOptions = ...
%           chompSmoothnessOptions(SmoothnessCostWeight=1e-3);
%       chomp.CollisionOptions = ...
%           chompCollisionOptions(CollisionCostWeight=10);
%       chomp.SolverOptions = ...
%           chompSolverOptions(Verbosity="none",LearningRate=7.0);
%
%       %Initialize a trajectory, optimize it using the CHOMP solver, and
%       %show the waypoints in a figure.
%       startconfig = homeConfiguration(robot);
%       goalconfig = [0.5 1.75 -2.25 2.0 0.3 -1.65 -0.4];
%       timepoints = [0 5];
%       timestep = 0.05;
%       [wptsamples,tsamples,solinfo] = optimize(chomp, ...
%                                       [startconfig; goalconfig], ...
%                                       timepoints, ...
%                                       timestep);
%       disp(solinfo)
%       show(chomp,wptsamples,NumSamples=10);
%       zlim([-0.5 1.3])
%
%   References:
%       [1] N. Ratliff, M. Zucker, J. A. Bagnell and S. Srinivasa, "CHOMP:
%       Gradient optimization techniques for efficient motion planning,"
%       2009 IEEE International Conference on Robotics and Automation,
%       2009, pp. 489-494, doi: 10.1109/ROBOT.2009.5152817.
%
%   See also chompCollisionOptions, chompSmoothnessOptions,
%   chompSolverOptions


% Copyright 2022-2024 The MathWorks, Inc.

%#codegen


    properties(Access=private)
        %RigidBodyTreeInternal Internal handle holding the copy of the input rigid body tree
        RigidBodyTreeInternal

        %BodySpheresEncoding A CHOMPBodySpheresEncoding
        BodySpheresEncoding

        %RigidBodyTreeKinematics An instance of rigid body tree for kinematics
        RigidBodyTreeKinematics
    end

    properties(Access=?matlab.unittest.TestCase,Constant)
        MESHTSDF_OBSTACLES_ALPHA=0.5
        OVERLAY_MESHTSDF_COLOR=[1, 0.5, 0] %Standard collision obstacle color
        SPHERICAL_OBSTACLES_FACEALPHA=0.5
        OVERLAY_SPHERICAL_PATCH_FACE_APHA=0.5
        OVERLAY_SPHERICAL_PATCH_FACE_COLOR='#4DBEEE' %graphics6 in parula
        DEFAULT_SHOW_NUM_SAMPLES=20
        MESHTSDF_OBSTACLES_DISPLAY_TAG='DO_NOT_EDIT_meshtsdf_obs'
        SPHERICAL_OBSTACLES_DISPLAY_TAG='DO_NOT_EDIT_spherical_obs'
        COLLISION_OBJECTS_DISPLAY_TAG='DO_NOT_EDIT_collision_objs'

        %RBT_VIS_OPTIONS Visualization options for showing rigid body tree
        %   Show the collision geometries, hide the visual meshes and the
        %   rigid body frames.
        RBT_VIS_OPTIONS={'coll','on','vis','off','frames','off'};
    end

    properties(SetAccess=private,Dependent)

        %RigidBodyTree Rigid body tree used for motion planning
        %   The rigid body tree consists of collision spheres which are
        %   used to compute the collision cost during optimization.
        RigidBodyTree
    end

    properties(Access=?matlab.unittest.TestCase)
        %SkippedSelfCollisionsInternal
        SkippedSelfCollisionsInternal
    end

    properties(Access=?dlCHOMP)
        %CollisionCostThreshold Termination threshold on the collision cost
        %   Minimum value threshold for the collision cost during
        %   during manipulatorCHOMP/optimize, specified as a nonnegative
        %   numeric scalar which can also be NaN. If the collision cost is
        %   below the specified threshold, the optimize function terminates.
        %   Default value of NaN ensures manipulatorCHOMP/optimize is
        %   unaffected if this is unset.
        %
        %   Default: NaN
        CollisionCostThreshold (1,1) {...
            mustBeNumeric}=NaN
    end

    methods
        function set.CollisionOptions(obj,opts)
            obj.CollisionOptions=opts;
            obj.updateSkippedSelfCollisionsInternal();
        end
    end

    methods(Access=private)
        function updateSkippedSelfCollisionsInternal(obj)
            if(strcmp(obj.CollisionOptions.SkippedSelfCollisions,'parent'))
                obj.SkippedSelfCollisionsInternal=robotics.manip.internal.SkippedSelfCollisionType.PARENT;
            else
                obj.SkippedSelfCollisionsInternal=robotics.manip.internal.SkippedSelfCollisionType.ADJACENT;
            end
        end
    end

    properties

        %RigidBodyTreeSpheres Spheres corresponding to a body in the rigid body tree
        %   Spheres of bodies in the rigid body tree, represented as a
        %   table where each row contains a body name on the rigid body
        %   tree and a cell-array value containing a 4-by-N matrix that
        %   describes the spheres of the corresponding rigid body. N is the
        %   total number of spheres for the corresponding rigid body. Each
        %   column of the matrix represents a sphere defined with respect
        %   to the rigid body. The first row of the matrix represents the
        %   radii of the spheres, and the last three rows correspond to the
        %   coordinates of the center in the rigid body's frame.
        %
        %   Default: table containing rigid body names and their
        %   corresponding sphere approximations
        RigidBodyTreeSpheres

        %SmoothnessOptions Options associated with smoothness cost
        %   Options associated with smoothness cost, specified as a
        %   chompSmoothnessOptions object.
        %
        %   Default: chompSmoothnessOptions
        SmoothnessOptions (1,1) {...
            mustBeA(SmoothnessOptions,'chompSmoothnessOptions')...
                                }=chompSmoothnessOptions

        %SolverOptions Options associated with the solver
        %   Options associated with the solver, specified as a
        %   chompSolverOptions object.
        %
        %   Default: chompSolverOptions
        SolverOptions (1,1) {...
            mustBeA(SolverOptions,'chompSolverOptions')...
                            }=chompSolverOptions

        %CollisionOptions Options associated with collision cost
        %   Options associated with the collision cost, specified as a
        %   chompCollisionOptions object.
        %
        %   Default: chompCollisionOptions
        CollisionOptions (1,1) {...
            mustBeA(CollisionOptions,'chompCollisionOptions')...
                               }=chompCollisionOptions

        %SphericalObstacles Spherical obstacles in environment
        %   The spherical obstacles in the environment represented as a
        %   4-by-N matrix where the first row corresponds to the radii of
        %   the spheres and the last three rows specify the center of the
        %   spheres.
        %
        %   Default: []
        SphericalObstacles{...
            manipulatorCHOMP.mustBeSpherical(SphericalObstacles,'SphericalObstacles')}=zeros(4,0)
    end

    properties (SetAccess=immutable)
        %MeshTSDF Distance field from a collection of meshes
        %   Meshes in the environment, represented as a mesh truncated signed
        %   distance field manager (meshtsdf object). When left empty, this
        %   field is unused. This property is read-only. You can only set
        %   this property at construction.
        %
        %   Default: []
        MeshTSDF {...
            mustBeA(MeshTSDF, {'double','meshtsdf'}), ...
            manipulatorCHOMP.mustBeScalarTSDFOrZeroDimEmpty(MeshTSDF,'meshtsdf')}=[]
    end


    methods
        function obj=manipulatorCHOMP(rbt,nvargs)
        %manipulatorCHOMP Constructor
            arguments
                rbt (1,1) rigidBodyTree
                nvargs.RigidBodyTreeSpheres
                nvargs.SolverOptions=chompSolverOptions
                nvargs.SmoothnessOptions=chompSmoothnessOptions
                nvargs.CollisionOptions=chompCollisionOptions
                nvargs.SphericalObstacles
                nvargs.MeshTSDF
            end
            robotics.manip.internal.RigidBodyTreeUtils.validateTreeWithMovingJoints(...
                rbt,'rigidbodytree:PlanningAtleastOneNonFixedJoint');
            robotics.manip.internal.errorIfUnsupportedForFloatingJoint(rbt,'manipulatorCHOMP');
            obj.RigidBodyTreeInternal=copy(rbt);
            obj.RigidBodyTreeKinematics=...
                robotics.manip.internal.fastrbt.RigidBodyTreeKinematics(...
                obj.RigidBodyTreeInternal);
            obs=zeros(4,0);
            coder.varsize('obs',[4,inf],[0,1]);
            obj.SphericalObstacles=obs;
            obj.RigidBodyTreeSpheres=...
                manipulatorCHOMP.makeDefaultTableOfSpheres(rbt);
            userProvidedProperties=fieldnames(nvargs);
            for i=coder.unroll(1:length(userProvidedProperties))
                obj.(userProvidedProperties{i})=nvargs.(userProvidedProperties{i});
            end
        end

        function newObj=copy(obj)
        %copy Create a deep copy
        %   NEWMANIPCHOMP=copy(MANIPCHOMP) returns a deep copy of
        %   manipulatorCHOMP MANIPCHOMP. MANIPCHOMP and NEWMANIPCHOMP are
        %   different objects with same properties.
            newObj=manipulatorCHOMP(obj.RigidBodyTreeInternal, MeshTSDF=obj.MeshTSDF);
            newObj.SmoothnessOptions=obj.SmoothnessOptions;
            newObj.CollisionOptions=obj.CollisionOptions;
            newObj.SolverOptions=obj.SolverOptions;
            newObj.SphericalObstacles=obj.SphericalObstacles;
            newObj.RigidBodyTreeSpheres=obj.RigidBodyTreeSpheres;
            newObj.CollisionCostThreshold=obj.CollisionCostThreshold;
        end

        function [optimizedtraj,outtsamples,solinfo]=...
                optimize(obj,wpts,tpts,timestep,varargin)
        %optimize  Optimize a trajectory
        %   [OPTIMIZEDTRAJ,OUTTSAMPLES] = optimize(MANIPCHOMP,WPTS,TPTS,TIMESTEP)
        %   optimizes a trajectory specified by waypoints WPTS at time
        %   points TPTS. WPTS is a N-by-M matrix comprising N number of
        %   waypoints where each waypoint represents the joint
        %   configuration of the rigid body tree, and M, thus is the size
        %   of the joint configuration. TPTS is a 1-by-N vector specifying
        %   the time point of the corresponding waypoint. TIMESTEP is the
        %   time step at which the input trajectory is discretized. The
        %   OPTIMIZEDTRAJ matrix is the trajectory with each row
        %   representing a joint configuration in a trajectory. OUTTSAMPLES
        %   are the time samples at which each configuration in
        %   OPTIMIZEDTRAJ is specified. OUTTSAMPLES is a 1-by-SZT row
        %   vector, and OPTIMIZEDTRAJ is a SZT-by-M matrix, where SZT are
        %   the number of configurations in OPTIMIZEDTRAJ.
        %
        %   [_] = optimize(_,InitialTrajectoryFitType=FITTYPE) specifies
        %   the initial trajectory type string FITTYPE which should be fit
        %   for the input WPTS and TPTS.
        %   FITTYPE can be:
        %       - 'minjerkpolytraj' - Fit a minimum jerk polynomial
        %                             trajectory
        %       - 'quinticpolytraj' - Fit a quintic polynomial trajectory
        %       - 'interp1'         - Fit a linear interpolated trajectory
        %
        %   [_,SOLINFO] = optimize(_) returns a struct SOLINFO containing
        %   the following fields:
        %      - Iterations                 : Number of iterations taken to
        %                                     optimize
        %      - SmoothnessCost             : Smoothness cost at each
        %                                     iteration
        %      - CollisionCost              : Collision cost at each
        %                                     iteration
        %      - ObjectiveFunction          : Objective function value at
        %                                     each iteration
        %      - InitialSmoothnessCost      : Initial smoothness cost of
        %                                     trajectory
        %      - InitialCollisionCost       : Initial collision cost of
        %                                     trajectory
        %      - InitialObjectiveFunction   : Initial objective function
        %                                     value of trajectory
        %      - OptimizationTime           : Time taken for optimizing
        %                                     trajectory
        %      - IsCollisionFree            : Trajectory is collision free

            tic; % Start the timer

            validateattributes(wpts,{'double'},{'2d','nonempty','real','finite'},...
                               'optimize','wpts');
            validateattributes(tpts,{'double'},{'nonempty','vector','real','finite','increasing','nonnegative'},...
                               'optimize','tpts');
            robotics.internal.validation.validatePositiveNumericScalar(timestep,'optimize','timestep');
            if(size(wpts,1)~=length(tpts))
                robotics.manip.internal.error('manipulatorchomp:WptsTptsMismatch',size(wpts,1),length(tpts));
            end
            configsize=...
                robotics.manip.internal.RigidBodyTreeUtils.getConfigurationInfo(obj.RigidBodyTreeInternal);
            if(size(wpts,2)~=configsize)
                robotics.manip.internal.error('manipulatorchomp:ConfigSizeMismatch',configsize);
            end

            if ~isempty(obj.MeshTSDF)
                maxsphrad = obj.BodySpheresEncoding.computeMaxRBTSpheresRadius();
                if obj.MeshTSDF.TruncationDistance < maxsphrad 
                    robotics.manip.internal.error('manipulatorchomp:InvalidTSDFTruncationDistance', string(obj.MeshTSDF.TruncationDistance), string(maxsphrad));
                end
            end

            % Initialize diagnostic logger
            logger=robotics.manip.internal.CHOMPDiagnosticLogger(obj.SolverOptions.Verbosity);

            % Initialize trajectory
            [optimizedtraj_,outtsamples]=...
                obj.initializeTrajectory(wpts,tpts,timestep,varargin{:});

            % Initialize the utility class
            config0=optimizedtraj_(1,:)';
            configNplus1=optimizedtraj_(end,:)';
            utils=robotics.manip.internal.CHOMPUtils(size(outtsamples,2)-2,config0,configNplus1,timestep);

            % Flatten trajectory
            flattraj=reshape(optimizedtraj_(2:end-1,:)',[],1);

            % Abbreviated variables for options. See equation (1) in [1].
            wsmooth=obj.SmoothnessOptions.SmoothnessCostWeight;
            wcoll=obj.CollisionOptions.CollisionCostWeight;
            lr=obj.SolverOptions.LearningRate;
            epsilon=obj.CollisionOptions.CollisionClearance;

            % Initialize the cost/objective function records with NaNs for every
            % iteration. These are used later to populate the solinfo.
            recordOfCollisionCost=nan(1,obj.SolverOptions.MaxIterations);
            recordOfSmoothCost=nan(1,obj.SolverOptions.MaxIterations);
            recordOfObjectiveFunction=nan(1,obj.SolverOptions.MaxIterations);

            % Compute the initial cost of collision avoidance and
            % smoothness.
            [initialcollcost,collgrad]=obj.costOfCollisionAvoidance(flattraj,config0,timestep,epsilon,utils);
            [initialsmoothcost,smoothgrad]=obj.costOfSmoothness(flattraj,utils);
            initialobjectivefunction=wsmooth*initialsmoothcost+wcoll*initialcollcost;

            % See equation (1) in [1] for notation.
            A=obj.getHessian(utils);

            % Iteratively update the trajectory.
            iteration=0;
            logger.logDiagnostic(iteration,initialobjectivefunction,initialsmoothcost,initialcollcost);
            prevobjfcn=initialobjectivefunction;
            for iter = 1:obj.SolverOptions.MaxIterations

                % The covariant gradient descent update to the trajectory.
                flattraj=flattraj-lr*(A\(wsmooth*smoothgrad+wcoll*collgrad));
                if(toc>obj.SolverOptions.MaxTime)
                    break;
                end
                [collcost,collgrad]=obj.costOfCollisionAvoidance(flattraj,config0,timestep,epsilon,utils);
                [smoothcost,smoothgrad]=obj.costOfSmoothness(flattraj,utils);
                objfcn=wsmooth*smoothcost+wcoll*collcost;
                recordOfCollisionCost(iter)=collcost;
                recordOfSmoothCost(iter)=smoothcost;
                recordOfObjectiveFunction(iter)=objfcn;
                iteration=iter;
                logger.logDiagnostic(iteration,objfcn,smoothcost,collcost);
                if (prevobjfcn==objfcn) || (abs((prevobjfcn-objfcn)/objfcn)<obj.SolverOptions.FunctionTolerance ...
                        || (~isnan(obj.CollisionCostThreshold) && ...
                            obj.CollisionCostThreshold>obj.costOfCollisionAvoidance(flattraj,config0,timestep,0,utils)))
                    break;
                end
                prevobjfcn=objfcn;
            end
            [optimizedtraj,solinfo]=obj.populateOutputsOnReturn(...
                flattraj,...
                config0,...
                timestep,...
                utils,...
                recordOfCollisionCost,...
                recordOfSmoothCost,...
                recordOfObjectiveFunction,...
                initialcollcost,...
                initialsmoothcost,...
                initialobjectivefunction,...
                iteration,...
                toc,...
                optimizedtraj_);
        end

        function ax=show(obj,varargin)
        %show Visualize the trajectory of rigid body tree
        %   show(MANIPCHOMP,TRAJ) visualizes the rigid body tree's
        %   trajectory TRAJ, which is a N-by-M matrix with N number of
        %   joint configurations of the rigid body tree, and where M is the
        %   size of a joint configuration. The spherical obstacles
        %   specified by property SphericalObstacles and mesh obstacles
        %   represented by truncated signed distance fields specified by
        %   the MeshTSDF property are also overlaid on the
        %   visualization.
        %
        %   show(MANIPCHOMP) visualizes the rigid body tree in its home
        %   configuration.
        %
        %   AX = show(MANIPCHOMP,_) returns the axes object AX of the visualization.
        %
        %   [_] = show(_,Name=Value) provides additional options specified by
        %   one or more name-value pair arguments.
        %
        %      NumSamples       - Uniformly sample NumSamples number of
        %                         joint configurations in trajectory TRAJ
        %                         and visualize them.
        %
        %                         Default: 20
        %
        %      Parent           - The axes object on which the
        %                         visualization should be shown
        %
        %                         Default: newplot
        %
        %      CollisionObjects - Cell-array of collision objects to be
        %                         visualized.
        %
        %                         Default: {}
        %
        %      ShowMeshTSDF     - Logical indicating whether or not to
        %                         display any included MeshTSDF visuals.
        %
        %                         Default: true
        %
        %   Example:
        %       %Load a robot and create a CHOMP solver
        %       franka = loadrobot("franka",DataFormat="row");
        %       chomp = ...
        %          manipulatorCHOMP(franka,SphericalObstacles=[0.1,0.4,0.4,0.2]');
        %
        %       %Visualize the robot's default spherical approximation in
        %       %its home configuration and the spherical obstacles in the
        %       %environment for the CHOMP problem.
        %       show(chomp);


            rbt=obj.RigidBodyTreeInternal;
            rbt.DataFormat='row';
            rbtspheres=obj.RigidBodyTree;
            rbtspheres.DataFormat='row';
            showmeshtsdf=true;

            % There are three syntaxes which "show" supports. In all the
            % syntaxes spherical obstacles are shown and an axes object is
            % output.
            %    1) Visualize the rigid body tree in its home configuration
            %    with its spherical approximation overlaid on top of its
            %    vanilla collision meshes
            %
            %    2) Visualize the user specified trajectory of the rigid
            %    body tree (with spherical approximation). All
            %    configurations in the trajectory are super imposed by
            %    "hold"ing the axes.
            %
            %    3) Including 1) and 2) additional options are specified
            %    for visualizing a trajectory via NV pairs, and, the user
            %    might also specify a cell-array of collision objects which
            %    they want to overlay on the plot.
            if(nargin==1)
                config=homeConfiguration(rbt);
                ax=obj.visualizeTrajectory(...
                    rbt,rbtspheres,config,[],obj.DEFAULT_SHOW_NUM_SAMPLES);
            elseif(nargin==2)
                traj=varargin{1};
                ax=obj.visualizeTrajectory(...
                    rbt,rbtspheres,traj,[],obj.DEFAULT_SHOW_NUM_SAMPLES);
            else
                if(isnumeric(varargin{1}))
                    traj=varargin{1};
                    tobeparsed=varargin(2:end);
                else
                    traj=homeConfiguration(rbt);
                    tobeparsed=varargin(1:end);
                end
                [numsamples,ax,collobjects,showmeshtsdf]=obj.parseShowInputs(tobeparsed{:});
                ax=obj.visualizeTrajectory(...
                    rbt,rbtspheres,traj,ax,numsamples);
                obj.visualizeCollObjects(ax,collobjects);
            end
            obj.visualizeSphericalObstacles(ax);
            if showmeshtsdf
                obj.visualizeMeshTSDF(ax);
            end
            drawnow;
        end
    end

    methods(Static,Access=private)
        function visualizeCollObjects(ax,collobjects)
        %visualizeCollObjects Visualize collision objects
            hold(ax,'on');
            for i=1:length(collobjects)
                [~,p]=show(collobjects{i},'parent',ax);
                p.Tag=manipulatorCHOMP.COLLISION_OBJECTS_DISPLAY_TAG;
            end
            hold(ax,'off');
        end

        function [numsamples,ax,collobjects,showmeshtsdf]=parseShowInputs(varargin)
        %parseShowInputs
            p=...
                robotics.core.internal.NameValueParser(...
                {'CollisionObjects','NumSamples','Parent','ShowMeshTSDF'},{{},20,[],true});
            p.parse(varargin{:});
            numsamples=p.parameterValue('NumSamples');
            robotics.internal.validation.validatePositiveIntegerScalar(...
                numsamples,'show','NumSamples');
            ax=p.parameterValue('Parent');
            if(~isempty(ax))
                robotics.internal.validation.validateAxesHandle(ax);
            end
            collobjects=p.parameterValue('CollisionObjects');
            showmeshtsdf=p.parameterValue('ShowMeshTSDF');
            if ~isempty(collobjects)
                robotics.internal.validation.validateCollisionGeometryArray(collobjects,'show','CollisionObjects');
            end
            validateattributes(showmeshtsdf,"logical",["nonempty" "scalar"],"show","ShowMeshTSDF");
        end

        function modifyCollisionMeshPatchesWithOverlayValues(ax)
        %modifyCollisionMeshPatchesWithOverlayValues
            sphpatches=findobj(ax.Children,'-regexp','DisplayName','coll_mesh');
            set(sphpatches,'FaceColor',manipulatorCHOMP.OVERLAY_SPHERICAL_PATCH_FACE_COLOR);
            set(sphpatches,'FaceAlpha',manipulatorCHOMP.OVERLAY_SPHERICAL_PATCH_FACE_APHA);
        end

        function ax=visualizeTrajectory(rbt,rbtspheres,traj,ax,numsamples)
        %visualizeTrajectory Visualize trajectory with overlaid robot spheres
            numconfig=size(traj,1);
            freq=ceil(numconfig/numsamples);
            sequence=1:freq:numconfig;
            if(isempty(ax))
                ax=show(rbtspheres,traj(1,:),manipulatorCHOMP.RBT_VIS_OPTIONS{:});
                sequence(1)=[];
            end
            hold(ax,'on');
            for i=sequence
                show(rbtspheres,traj(i,:),'parent',ax,manipulatorCHOMP.RBT_VIS_OPTIONS{:});
            end
            manipulatorCHOMP.modifyCollisionMeshPatchesWithOverlayValues(ax);
            for i=1:freq:numconfig
                show(rbt,traj(i,:),'parent',ax,manipulatorCHOMP.RBT_VIS_OPTIONS{:});
            end
            hold(ax,'off');
        end

    end

    methods(Access=private)
        function visualizeSphericalObstacles(obj,ax)
        %visualizeSphericalObstacles Show spherical obstacles as an overlay
            hold(ax,'on');
            for i=1:size(obj.SphericalObstacles,2)
                csph=collisionSphere(obj.SphericalObstacles(1,i));
                csph.Pose=trvec2tform(obj.SphericalObstacles(2:end,i)');
                [~,p]=show(csph,'parent',ax);
                p.FaceAlpha=obj.SPHERICAL_OBSTACLES_FACEALPHA;
                p.Tag=obj.SPHERICAL_OBSTACLES_DISPLAY_TAG;
            end
            hold(ax,'off');
        end

        function visualizeMeshTSDF(obj,ax)
        %visualizeMeshTSDF Show voxelized meshes as an overlay
            if ~isempty(obj.MeshTSDF)
                hold(ax,'on');
                scatterHandles = obj.MeshTSDF.show("Parent",ax);
                for i = 1:numel(scatterHandles)
                    scatterHandles(i).MarkerFaceAlpha = obj.MESHTSDF_OBSTACLES_ALPHA;
                    scatterHandles(i).MarkerEdgeAlpha = obj.MESHTSDF_OBSTACLES_ALPHA;
                    scatterHandles(i).MarkerFaceColor = obj.OVERLAY_MESHTSDF_COLOR;
                    scatterHandles(i).MarkerEdgeColor = obj.OVERLAY_MESHTSDF_COLOR;
                    scatterHandles(i).Tag=obj.MESHTSDF_OBSTACLES_DISPLAY_TAG;
                end
                hold(ax,'off');
            end
        end
    end

    methods
        function rbt=get.RigidBodyTree(obj)
        %get.RigidBodyTree Getter for dependent property RigidBodyTree
            rbt=copy(obj.RigidBodyTreeInternal);
            bodynames=rbt.BodyNames;
            for i = coder.unroll(1:rbt.MaxNumBodies+1)
                if(i<=length(bodynames)+1)
                    if(i>length(bodynames))
                        rb=rbt.Base;
                        bodyname=rbt.BaseName;
                    else
                        rb=rbt.getBody(bodynames{i});
                        bodyname=bodynames{i};
                    end
                    spheresofbody=...
                        obj.RigidBodyTreeSpheres(bodyname,'spheres').spheres{1};
                    numspheres=size(spheresofbody,2);
                    rb.BodyInternal.CollisionsInternal=...
                        robotics.manip.internal.CollisionSet(numspheres);
                    for ii = 1:numspheres
                        rb.addCollision(...
                            "sphere",...
                            spheresofbody(1,ii),...
                            trvec2tform([spheresofbody(2:end,ii)]'));
                    end
                end
            end
        end

        function set.RigidBodyTreeSpheres(obj,tableofspheres)
        %set.RigidBodyTreeSpheres
            obj.validateRigidBodyTreeSpheres(tableofspheres)
            obj.RigidBodyTreeSpheres=tableofspheres;
            obj.makeBodySpheresEncodingEncodingFromRBTSpheres();
        end
    end

    methods(Access={?dlCHOMP})
        function [cost,grad]=costOfCollisionAvoidance(obj,flattraj,config0,dt,epsilon,utils)
        %costOfCollisionAvoidance Cost of collision avoidance.
        %   [COST,GRAD]=costOfCollisionAvoidance(obj,FLATTRAJ,CONFIG0,DT,EPSILON,UTILS)
        %
        %   Inputs:
        %       FLATTRAJ - Flattened trajectory of size
        %       NUMCONFIG*CONFIGSIZE-by-1 where NUMCONFIG are the number of
        %       joint configurations in the trajectory, and CONFIGSIZE is
        %       the size of each configuration.
        %
        %       CONFIG0 - Initial joint configuration.
        %
        %       DT - Time step between two consecutive configurations.
        %
        %       EPSILON - Collision clearance.
        %
        %       UTILS - A utilities instance.
        %
        %   Outputs:
        %       COST - Scalar cost of collision avoidance of a trajectory
        %
        %       GRAD - Rate of change of COST with respect to the change in
        %       trajectory.
        %
        %   See equation (4) in [1] for notational reference.

            if(obj.SkippedSelfCollisionsInternal==robotics.manip.internal.SkippedSelfCollisionType.PARENT)
                [cost,grad]=...
                    costOfCollisionAvoidance_(obj,flattraj,config0,dt,epsilon,utils,...
                                              @(bodyindices,rbt,bidx)(...
                    robotics.manip.internal.CHOMPUtils.maskSelfAndParentCollisionIndices(...
                    bodyindices,rbt,bidx)));
            else
                [cost,grad]=...
                    costOfCollisionAvoidance_(obj,flattraj,config0,dt,epsilon,utils,...
                                              @(bodyindices,~,bidx)(...
                    robotics.manip.internal.CHOMPUtils.maskSelfAndAdjacentCollisionIndices(...
                    bodyindices,bidx)));
            end
        end

        function [cost,grad]=costOfCollisionAvoidance_(obj,flattraj,config0,dt,epsilon,utils,selfcollidxfcn)
        %costOfCollisionAvoidance_ Helper to costOfCollisionAvoidance
            rbt=obj.RigidBodyTreeInternal;
            m=size(config0,1);
            tform0=obj.RigidBodyTreeKinematics.forwardKinematics(config0(:)');
            unflattraj=reshape(flattraj,m,[])';
            tforms=obj.RigidBodyTreeKinematics.forwardKinematics(unflattraj);
            jacs=obj.RigidBodyTreeKinematics.geometricJacobians(unflattraj);
            cost=0;
            grad=zeros(size(flattraj));
            numconfig=size(unflattraj,1);
            % For every body sphere on the rigid body tree.
            for u = 1:numpoints(obj.BodySpheresEncoding)
                if(isempty(obj.SphericalObstacles))
                    sphericalObstaclesInWorld_=[0;repmat(realmax,3,1)];
                else
                    sphericalObstaclesInWorld_=obj.SphericalObstacles;
                end

                % The spherical obstacles are static, however, we need them for
                % every configuration during the cost computation.
                sphericalObstaclesInWorld=...
                    repmat(sphericalObstaclesInWorld_,1,1,numconfig);


                % Transform body sphere's center in rigid body frame to
                % world frame at the initial joint configuration CONFIG0.
                bidx=rigidBodyIndex(obj.BodySpheresEncoding,u);
                tform0bidx=tform0(:,:,bidx+1);
                x0=utils.transformPoints(tform0bidx,sphereCenter(obj.BodySpheresEncoding,u));

                % Compute the cost, and gradient of this cost, of the
                % sphere as it transitions over the trajectory in presence
                % of spherical obstacles.
                bodyindicesofencoding=rigidBodyIndices(obj.BodySpheresEncoding);
                bodySpheresEncodingOfInterest=...
                    encodingAtIndices(...
                    obj.BodySpheresEncoding,selfcollidxfcn(bodyindicesofencoding,rbt,bidx));
                if(~obj.CollisionOptions.IgnoreSelfCollision)
                    for v = 1:numpoints(bodySpheresEncodingOfInterest)
                        selfcollsph=sphere(bodySpheresEncodingOfInterest,v);
                        selfcollsphcenter=sphereCenter(bodySpheresEncodingOfInterest,v);
                        selfcollsphbidx=rigidBodyIndex(bodySpheresEncodingOfInterest,v);
                        selfcollsphcenterinworld=...
                            utils.transformPoints(tforms(:,:,selfcollsphbidx+1,:),selfcollsphcenter);
                        radiusselfcollsph=selfcollsph(1);
                        selfcollsphinworld_=[repmat(radiusselfcollsph,1,numconfig);
                                             selfcollsphcenterinworld];
                        selfcollsphinworld=...
                            reshape(selfcollsphinworld_,[],1,numconfig);
                        sphericalObstaclesInWorld=cat(2,sphericalObstaclesInWorld,selfcollsphinworld);
                    end
                end
                [costu1,gradu1]=utils.costOfTreeSphericalCollisionsInWorld(...
                    sphere(obj.BodySpheresEncoding,u),...
                    tforms(:,:,bidx+1,:),...
                    jacs(:,:,bidx+1,:),...
                    x0,...
                    dt,...
                    epsilon,...
                    @(S1)utils.distanceToNearestSphereFrom(S1,sphericalObstaclesInWorld));
                [costu2,gradu2]=obj.costOfTreeSphericalObstaclesInMeshWorld(...
                    sphere(obj.BodySpheresEncoding,u),...
                    tforms(:,:,bidx+1,:),...
                    jacs(:,:,bidx+1,:),...
                    x0,...
                    dt,...
                    epsilon,...
                    utils);
                cost=cost+costu1+costu2;
                grad=grad+gradu1+gradu2;
            end
        end

        function [cost,grad]=costOfSmoothness(obj,flattraj,utils)
        %costOfSmoothness Cost of smoothness of trajectory
        %   [COST,GRAD]=costOfSmoothness(obj,FLATTRAJ,UTILS)
        %   Inputs:
        %       FLATTRAJ - Flattened trajectory of size
        %       NUMCONFIG*CONFIGSIZE-by-1 where NUMCONFIG are the number of
        %       joint configurations in the trajectory, and CONFIGSIZE is
        %       the size of each configuration.
        %
        %       UTILS - A utilities instance.
        %
        %   Outputs:
        %       COST - Scalar cost of smoothness of a trajectory
        %
        %       GRAD - Rate of change of COST with respect to the change in
        %       trajectory.
        %
        %   See equation (1) in [1] for notational reference.

            wvel=obj.SmoothnessOptions.VelocitySmoothnessWeight;
            wacc=obj.SmoothnessOptions.AccelerationSmoothnessWeight;
            wjerk=obj.SmoothnessOptions.JerkSmoothnessWeight;
            [costvel,gradvel]=utils.sumSquaredVelocity(flattraj);
            [costacc,gradacc]=utils.sumSquaredAcceleration(flattraj);
            [costjerk,gradjerk]=utils.sumSquaredJerk(flattraj);
            cost=wvel*costvel+wacc*costacc+wjerk*costjerk;
            grad=wvel*gradvel+wacc*gradacc+wjerk*gradjerk;
        end

        function A=getHessian(obj,utils)
        %getHessian The Hessian matrix of the smoothness cost
        %   The Hessian matrix is defined in terms of the smoothness cost
        %   of CHOMP.
        %
        %   See equation (1) in [1] for notational reference.
            k1=utils.K1;
            k2=utils.K2;
            k3=utils.K3;
            wvel=obj.SmoothnessOptions.VelocitySmoothnessWeight;
            wacc=obj.SmoothnessOptions.AccelerationSmoothnessWeight;
            wjerk=obj.SmoothnessOptions.JerkSmoothnessWeight;
            A=(wvel*(k1'*k1))+(wacc*(k2'*k2))+(wjerk*(k3'*k3));
        end


        function makeBodySpheresEncodingEncodingFromRBTSpheres(obj)
        %makeBodySpheresEncodingEncodingFromRBTSpheres Make body points encoding
            obj.BodySpheresEncoding=...
                robotics.manip.internal.CHOMPBodySpheresEncoding(...
                robotics.manip.internal.CHOMPUtils.makeBodyPointsEncodingFromRBTSpheres(...
                obj.RigidBodyTreeInternal.TreeInternal,...
                obj.RigidBodyTreeSpheres));
        end

        function [optimizedtraj,solinfo]=populateOutputsOnReturn(...
            obj,...
            flattraj,...
            config0,...
            timestep,...
            utils,...
            recordOfCollisionCost,...
            recordOfSmoothCost,...
            recordOfObjectiveFunction,...
            initialcollcost,...
            initialsmoothcost,...
            initialobjectivefunction,...
            iteration,...
            optimtime,...
            optimizedtraj_)
        %populateOutputsOnReturn Populate outputs on return from optimize
        %   Just when we are going to return from optimize, this function
        %   populates all the necessary outputs requested by the user i.e.,
        %   the solution struct, and the unflattened optimized trajectory.
            isCollisionFree=...
                obj.costOfCollisionAvoidance(...
                flattraj,config0,timestep,0,utils)==0;
            solinfo=obj.makeSolutionInfoStruct(...
                recordOfCollisionCost,...
                recordOfSmoothCost,...
                recordOfObjectiveFunction,...
                initialcollcost,...
                initialsmoothcost,...
                initialobjectivefunction,...
                iteration,...
                optimtime,...
                isCollisionFree);
            optimizedtraj_(2:end-1,:)=reshape(flattraj,size(config0,1),[])';
            optimizedtraj=optimizedtraj_;
        end

        function [costu,gradu]=costOfTreeSphericalObstaclesInMeshWorld(obj, spheres, tforms, jacs, x0, dt, epsilon, utils)
        %costOfTreeSphericalObstaclesInMeshWorld Compute the cost of rigidbodytree spherical obstacles in a meshtsdf-defined world

            if ~isempty(obj.MeshTSDF) && (obj.MeshTSDF.NumMesh>0)
                [costu,gradu]=utils.costOfTreeSphericalCollisionsInWorld(...
                    spheres,...
                    tforms,...
                    jacs,...
                    x0,...
                    dt,...
                    epsilon,...
                    @(S)utils.distanceFromMeshTSDF(S,obj.MeshTSDF));
            else
                costu=0;
                gradu=0;
            end
        end
    end

    methods(Access={?dlCHOMP},Static)
        function solinfo=makeSolutionInfoStruct(...
            recordOfCollisionCost,...
            recordOfSmoothCost,...
            recordOfObjectiveFunction,...
            initialcollcost,...
            initialsmoothcost,...
            initialobjectivefunction,...
            iter,...
            optimtime,...
            isCollisionFree)
        %makeSolutionInfoStruct Make solution info struct output by optimize
        %   SOLINFO=makeSolutionInfoStruct(
        %   RECORDOFCOLLISIONCOST,
        %   RECORDOFSMOOTHCOST,
        %   RECORDOFOBJECTIVEFUNCTION,
        %   INITIALCOLLCOST,
        %   INITIALSMOOTHCOST,
        %   INITIALOBJECTIVEFUNCTION,
        %   ITER,
        %   OPTIMTIME,
        %   ISCOLLISIONFREE) returns a struct solution info with the following fields:
        %      - Iterations: Number of iterations taken to optimize
        %      - SmoothnessCost: Smoothness cost at each iteration
        %      - CollisionCost: Collision cost at each iteration
        %      - ObjectiveFunction: Objective function value at each iteration
        %      - InitialSmoothnessCost: Initial smoothness cost of trajectory
        %      - InitialCollisionCost: Initial collision cost of trajectory
        %      - InitialObjectiveFunction: Initial objective function value of trajectory
        %      - OptimizationTime: Time taken for optimizing trajectory
        %      - IsCollisionFree: Trajectory is collision free
        %
        %   where, RECORDCOLLCOST, RECORDSMOOTHCOST,
        %   RECORDOBJECTIVEFUNCTION are row vectors of size
        %   1-by-MAXITERATIONS where MAXITERATIONS is the maximum
        %   iterations specified for CHOMP, ITER is a scalar specifying the
        %   number of iterations CHOMP took during "optimize" and
        %   INITIALCOLLCOST, INITIALSMOOTHCOST, and
        %   INITIALOBJECTIVEFUNCTION are the initial collision cost,
        %   initial smoothness cost, and initial objective function values,
        %   respectively. ISCOLLISIONFREE is a logical indicating if the
        %   trajectory given the spherical obstacles and the body
        %   spheres encoding, is collision free.

            solinfo=struct("Iterations",iter,...
                           "SmoothnessCost",recordOfSmoothCost,...
                           "CollisionCost",recordOfCollisionCost,...
                           "ObjectiveFunction",recordOfObjectiveFunction,...
                           "InitialObjectiveFunction",initialobjectivefunction,...
                           "InitialSmoothnessCost",initialsmoothcost,...
                           "InitialCollisionCost",initialcollcost,...
                           "OptimizationTime",optimtime,...
                           "IsCollisionFree", isCollisionFree);
        end

        function [optimizedtraj,outtsamples]=...
                initializeTrajectory(wpts,tpts,timestep,varargin)
        %initializeTrajectory
            fittype=manipulatorCHOMP.parseInitialTrajectoryFitType(varargin{:});
            outtsamples=tpts(1):timestep:tpts(end);
            if(strcmpi(fittype,'quinticpolytraj'))
                optimizedtraj=(quinticpolytraj(wpts',tpts,outtsamples))';
            elseif(strcmpi(fittype,'minjerkpolytraj'))
                [~,~,~,~,pp]=...
                    minjerkpolytraj(wpts',tpts,2);
                optimizedtraj=ppval(pp,outtsamples)';
            else
                optimizedtraj=interp1(tpts,wpts,outtsamples);
            end
        end

        function fittype=...
                parseInitialTrajectoryFitType(varargin)
        %parseInitialTrajectoryFitType
            [s,u]=coder.internal.nvparse({'InitialTrajectoryFitType'},varargin{:});
            if(u.InitialTrajectoryFitType)
                fittype=...
                    validatestring(s.InitialTrajectoryFitType,...
                                   {'quinticpolytraj','minjerkpolytraj','interp1'},...
                                   'optimize','InitialTrajectoryFitType');
            else
                fittype='minjerkpolytraj';
            end
        end

        function spheres=genspheres(capsule,ratios)
        %genspheres Generate spheres with non-compile time ratios
        %    Note that this behaves exactly like
        %    collisionCapsule/genspheres, however, genspheres requires ratio
        %    to be a compile time constant which might not be the case. Note
        %    that this actually gives out an encoding of a 4-by-N matrix of
        %    spheres instead of a cell-array of collisionSpheres.
            spheres=zeros(4,size(ratios,2));
            zpoints=ratios*capsule.Length-capsule.Length/2;
            points=[zeros(2,length(zpoints));
                    zpoints;
                    ones(1,length(zpoints))];
            tformedpoints=capsule.Pose*points;
            spheres(1,:)=capsule.Radius;
            spheres(2:end,:)=tformedpoints(1:3,:);
        end

        function rbtsph=makeDefaultTableOfSpheres(rbt)
        %makeDefaultTableOfSpheres Makes default spherical model of rigid body tree
        %   The default table of spheres is constructed via a
        %   capsuleApproximation and with spheres generated at a ratio
        %   proportional to the length of the capsule.
            bodynames=rbt.BodyNames;
            bodynames{end+1}=rbt.BaseName;
            sph=zeros(4,0);
            coder.varsize('sph',[4,inf],[0,1]);
            spheres=repmat({sph},length(bodynames),1);
            cap=capsuleApproximation(rbt);
            for i = 1:length(bodynames)
                cap_=getCapsules(cap,bodynames{i});
                for ii = 1:length(cap_)
                    capsule=cap_{ii};
                    ratio=capsule.Radius/capsule.Length;
                    ratios=0:ratio:1;
                    spheres{i}=[spheres{i},...
                                manipulatorCHOMP.genspheres(capsule,ratios)];
                end
            end
            rbtsph=table(spheres,...
                         'RowNames',bodynames,...
                         'VariableNames',{'spheres'});
        end

        function mustBeSpherical(sphobst,argname)
        %mustBeSpherical Validates whether this is a valid spherical matrix
        %   A spherical matrix is of size 4-by-N with each column
        %   representing a sphere. The first row of the matrix are the radii
        %   of the spheres and the last three rows are the coordinates of
        %   the spheres.
            validateattributes(sphobst,...
                               {'numeric'},...
                               {'finite','size',[4,nan]},...
                               'manipulatorCHOMP',argname)
        end

        function mustBeScalarTSDFOrZeroDimEmpty(tsdf,argname)
        %mustBeScalarTSDFOrEmpty Input must be scalar if it's a TSDF and empty otherwise

            % Values can be either meshtsdf or empty doubles. Error if a
            % non-empty double is provided.
            if isa(tsdf, 'double') && ~isempty(tsdf)
                robotics.manip.internal.error('manipulatorchomp:ExpectedMeshTSDFOrEmpty');
            end
            
            % MeshTSDF entries must be scalar
            if isa(tsdf, 'meshtsdf') && ~isscalar(tsdf)
                mustBeScalarOrEmpty(tsdf);
            end

            % Expect empty values to be zero-dimensional
            if isempty(tsdf) || isa(tsdf, "double")
                validateattributes(tsdf, {'double'}, {'size',[0 0]},'manipulatorCHOMP',argname);
            end

        end
    end

    methods(Access=private)
        function validateRigidBodyTreeSpheres(obj,tableofspheres)
        %validateRigidBodyTreeSpheres Validate the table of spheres of a rigid body tree
        %   The function validates that:
        %       - RigidBodyTreeSpheres is a table class
        %       - Row names are the same as body and base names
        %       - Variable names contains spheres
        %       - Values in spheres are of size 4-by-N

        % Validate it is a table class
            validateattributes(tableofspheres,...
                               {'table'},...
                               {},...
                               'manipulatorCHOMP','RigidBodyTreeSpheres');

            rigidbodynames=obj.RigidBodyTreeInternal.BodyNames;
            rigidbodynames{end+1}=obj.RigidBodyTreeInternal.BaseName;
            if(~isequal(length(rigidbodynames),...
                        length(tableofspheres.Properties.RowNames)))
                robotics.manip.internal.error('manipulatorchomp:TableOfSpheresInsufficientRows',length(rigidbodynames));
            end
            if coder.target('MATLAB')
                % Validate the RowNames
                if(~isequal(sort(rigidbodynames)',sort(tableofspheres.Properties.RowNames)))
                    robotics.manip.internal.error('manipulatorchomp:TableOfSpheresRowNameMismatch');
                end

                % Validate the VariableNames
                if(~any(ismember(tableofspheres.Properties.VariableNames,'spheres')))
                    robotics.manip.internal.error('manipulatorchomp:TableVariableNameShouldBeSpheres');
                end

                % Validate the size of the values in "spheres"
                cellfun(@(x)manipulatorCHOMP.mustBeSpherical(x,'spheres'),tableofspheres.spheres);
            end
        end
    end
end

% LocalWords:  voxelized
