classdef dlCHOMP < robotics.manip.internal.InternalAccess
%DLCHOMP Create a Deep Learning initial guesser powered CHOMP
%   Deep Learning guess powered Covariant Hamiltonian Optimization for Motion Planning (DLCHOMP) for
%   rigid body tree optimizes neural network predicted trajectories for smoothness, and collision avoidance.
%
%   DLCHOMP = dlCHOMP(RIGIDBODYTREE,BPSENCODER,NUMWAYPOINTS) Creates a
%   Neural Network guesser with a CHOMP-based optimizer for a RIGIDBODYTREE robot.
%   Uses the specified BPSENCODER to encode an obstacle environment and predict a
%   trajectory having NUMWAYPOINTS waypoints.
%
%   DLCHOMP = dlCHOMP(RIGIDBODYTREE,BPSENCODER,NUMWAYPOINTS,PropertyName=PropertyValue)
%   sets properties on the object using name-value arguments.
%
%   dlCHOMP properties:
%       RigidBodyTree            - Rigid body tree used for motion planning
%       BPSEncoder               - Basis point set encoder used for encoding obstacle environment
%       NumWaypoints             - Total Number of waypoints needed in output trajectory
%       SmoothnessOptions        - Smoothness options associated with CHOMP optimizer
%       CollisionOptions         - Collision options associated with CHOMP optimizer
%       SolverOptions            - Solver options associated with CHOMP optimizer
%       Network                  - dlnetwork object used to obtain initial guess for CHOMP optimizer
%       NumInputs                - Number of inputs to network
%       NumOutputs               - Number of outputs of network
%       RigidBodyTreeSpheres     - Spheres of bodies in rigid body tree
%       SphericalObstacles       - Spherical obstacles in environment
%
%   dlCHOMP methods:
%       optimize          - Optimize a neural network guessed trajectory
%       generateSamples   - Generate training data samples for neural network
%       trainDLCHOMP      - Train neural network
%       resetCHOMPOptions - Resets optimization options to that of last trained state
%       show              - Visualize trajectory of rigid body tree
%
%   Example:
%       %Load a robot model into the workspace.
%       robot = loadrobot("kukaIiwa7",DataFormat="row");
%
%       %Create basis point encoder for encoding target obstacle environments
%       %with radius 1 for unit sphere and 1e4 basis points.
%       %Set random number generator seed for repeatability.
%       rng(200,"twister");
%       encoder = bpsEncoder('uniform-ball-3d',1e4);
%
%       %Create dlCHOMP object using basis points encoder and robot objects.
%       %Specify desired number of waypoints in optimized trajectory.
%       dlchomp = dlCHOMP(robot,encoder,40);
%
%       %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 dlCHOMP solver.
%       dlchomp.SmoothnessOptions = ...
%           chompSmoothnessOptions(SmoothnessCostWeight=1e-3);
%       dlchomp.CollisionOptions = ...
%           chompCollisionOptions(CollisionCostWeight=120);
%       dlchomp.SolverOptions = ...
%           chompSolverOptions( ...
%               Verbosity="none", ...
%               LearningRate=2.0, ...
%               FunctionTolerance=1e-4, ...
%               MaxIterations=200);
%
%       %Create dlCHOMPDataOptions object for generating data
%       dataOptions = dlCHOMPDataOptions( ...
%           RadiusRangeOfObstacles=[0.05 0.2], ...
%           CountRangeOfObstacles=[20 25], ...
%           MinDistanceFromBase=0.21, ...
%           NumSamples=1e3, ...
%           ValidationSplit=0.2);
%
%       %Obtain data for training and validation
%       dataFolder = "samplesFolder";
%       [trainDatastore,validDatastore] = generateSamples( ...
%                                           dlchomp, ...
%                                           dataOptions, ...
%                                           dataFolder);
%
%       %Create training options object
%       options = trainingOptions("adam", ...
%           "MaxEpochs",50, ...
%           "MiniBatchSize",64, ...
%           "InitialLearnRate",0.001, ...
%           "LearnRateDropFactor",0.9, ...
%           "LearnRateDropPeriod",10, ...
%           "LearnRateSchedule","piecewise", ...
%           "ValidationData",validDatastore, ...
%           "ExecutionEnvironment","auto", ...
%           "Plots","training-progress", ...
%           'Shuffle','every-epoch');
%
%       %Train neural network using options and the samples generated earlier
%       info = trainDLCHOMP(dlchomp,trainDatastore,"mean-squared-error",options);
%
%       %Create spheres to represent obstacles, and
%       %add them to the dlCHOMP 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)
%       dlchomp.SphericalObstacles = env;
%
%       %Initialize a trajectory, optimize it using the dlCHOMP 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];
%       [wptsamples,tsamples,solinfo] = optimize(dlchomp, ...
%                                       startconfig, ...
%                                       goalconfig);
%       disp(solinfo)
%       show(dlchomp,wptsamples);
%       zlim([-0.5 1.3])
%
%   References:
%       [1] J. Tenhumberg, D. Burschka and B. BÃ¤uml, "Speeding Up Optimization-based
%       Motion Planning through Deep Learning,"
%       2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Kyoto, Japan,
%       2022, pp. 7182-7189, doi: 10.1109/IROS47612.2022.9981717.
%
%       [2] 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, dlCHOMPDataOptions

% Copyright 2023-2024 The MathWorks, Inc.

%#codegen

    properties(Access=private)
        %SmoothnessOptionsLastTrained Internal handle holding the copy of SmoothnessOptions
        %   SmoothnessOptionsInternal holds a copy of SmoothnessOptions
        %   public property value when last trainDLCHOMP() finished
        %   successfully. Matches the public property value by default.
        %   Matches the SmoothnessOptions property if that is passed as a
        %   name-value pair to constructor.
        SmoothnessOptionsLastTrained

        %CollisionOptionsLastTrained Internal handle holding the copy of CollisionOptions
        %   CollisionOptionsInternal holds a copy of SmoothnessOptions
        %   public property value when last trainDLCHOMP() finished
        %   successfully. Matches the public property value by default.
        %   Matches the CollisionOptions property if that is passed as a
        %   name-value pair to constructor.
        CollisionOptionsLastTrained

        %SolverOptionsLastTrained Internal handle holding the copy of SolverOptions
        %   SolverOptionsInternal holds a copy of SolverOptions
        %   public property value when last trainDLCHOMP() finished
        %   successfully. Matches the public property value by default.
        %   Matches the SolverOptions property if that is passed as a
        %   name-value pair to constructor.
        SolverOptionsLastTrained

        %CHOMPInternal Internal handle holding an instance of manipulatorCHOMP
        %   CHOMPInternal is an instance of manipulatorCHOMP used for
        %   optimization when calls are made to optimize() and generateSamples() functions
        CHOMPInternal

        %RandomStreamInternal Internal handle holding the random stream
        %used for data generation
        %   RandomStreamInternal is an instance of RandomStream used for
        %   data generation when calls are made to generateSamples()
        %   function
        RandomStreamInternal

        %VerboseOutput Determines if errors are thrown when DATAOPTIONS object is set incorrectly
        %   If the DATAOPTIONS object passed as input during a
        %   generateSamples() call was set incorrectly, VERBOSE_OUTPUT will
        %   throw errors suggesting next steps to correct the DATAOPTIONS
        %   object to the user. If verboseOutput is true, errors are
        %   thrown, otherwise the generateSamples() call will continue to
        %   execute until DATAOPTIONS.NumSamples samples are generated.
        %   If set true, VERBOSE_OUTPUT provides a preemptive stop when
        %   DATAOPTIONS are set incorrectly.
        VerboseOutput
    end

    properties(Constant,Access={ ...
                                 ?matlab.unittest.TestCase, ...
                                 ?dlCHOMPDatastore, ...
                                 ?robotics.datagen.internal.DLCHOMPUtils})
        %INPUT_LAYER_NAMES Names of the input layers of network
        %   NumInputs property holds the sizes of these layers in order, also
        %   used for network validation
        INPUT_LAYER_NAMES={'JointInputLayer' 'EncodingInputLayer'}

        %FINAL_LAYER_NAME Name of the last-but-one layer of network
        %   NumOutputs property holds the size of this layer. Note that this
        %   is not the output layer since dlnetwork objects do not support
        %   output layers
        FINAL_LAYER_NAME = 'FinalFCLayer';

        %NUMPOINTS_FOR_ALPHASHAPE Number of points sampled for creating alphaShape for data generation
        %   State space of robot is sampled NUMPOINTS_FOR_ALPHASHAPE number
        %   of times. The sampled points are used to represent the robot
        %   workspace as an alphaShape.
        NUMPOINTS_FOR_ALPHASHAPE = 1e4;

        %MAX_NUMPOINTS_IN_ALPHASHAPE Maximum number of attempts at sampling points in alphaShape
        %   Volume around robot is sampled MAX_NUMPOINTS_IN_ALPHASHAPE
        %   number of times to obtain points within alphaShape for obstacle placement.
        MAX_NUMPOINTS_IN_ALPHASHAPE = 1e5;

        %INITIAL_TRAJ_TYPE Initial trajectory type used with CHOMPInternal for data generation
        %   INITIAL_TRAJ_TYPE initial trajectory type is used during data generation
        %   with CHOMPInternal in the generateSamples() function
        INITIAL_TRAJ_TYPE = "minjerkpolytraj";

        %MAX_TRIES_PER_ENV Maximum number of attempts made at obtaining valid data samples in a given obstacle environment
        %   Maximum number of attempts made at obtaining valid data samples
        %   in a given obstacle environment before a new environment is generated.
        %   Valid data sample requirement is defined in the help text for
        %   obtainValidDataSamples() function.
        MAX_TRIES_PER_ENV = 1e4;

        %MAX_FAIL_PERC_PER_ENV Maximum fraction of failed attempts made at obtaining valid data samples in a given obstacle environment
        %   Maximum fraction of failed attempts made at obtaining valid data samples
        %   in a given obstacle environment before a new environment is generated.
        %   Valid data sample requirement is defined in the help text for
        %   obtainValidDataSamples() function.
        MAX_FAIL_FRAC_PER_ENV = 0.75;

        %TPOINTS Start and goal configuration time points fixed for network
        %   The dlnetwork NETWORK is unaware of time points and hence
        %   assumes a fixed set of start and goal time points for the start and
        %   goal configurations of the robot. The optimized output trajectory is
        %   generated accordingly. The user is expected to scale the
        %   optimized trajectory, if needed, to fit their desired time range.
        TPOINTS = [0 1];

        %COLLISION_COST_THRESHOLD Termination threshold on the collision cost
        %   Minimum value threshold for the collision cost during internal
        %   calls to manipulatorCHOMP/optimize, specified as a non-negative
        %   numeric scalar which can also be NaN. If the collision cost is
        %   below the specified threshold, the optimize function terminates.
        %   It is assumed that if the value of collision cost goes below this
        %   threshold, we have obtained a collision-free trajectory.
        %
        %   Default: 1e-5
        COLLISION_COST_THRESHOLD = 1e-5;

        %ALPHASHAPE_SAMPLING_RADIUS Radius of the sphere used to sample alphaShape workspace points for obstacle placement
        %   Radius of the sphere centered on the robot used to sample the
        %   alphaShape object representing the robot workspace. This
        %   sampling is done to obtain possible obstacle placement positions
        %   when generating data samples using generateSamples()
        ALPHASHAPE_SAMPLING_RADIUS = 3;

        %ENV_GEN_ERRORS Environment generation error codes
        %   Possible environment generation error codes that are used along
        %   with the throwEnvironmentGenerationError() function
        ENV_GEN_ERRORS=struct( ...
            "ClutteredEnv",1, ...
            "SimpleEnv",2, ...
            "MinObstaclesPlacement",3);

        %DATA_SAMPLE_SCHEMA Data sample JSON's field names
        %   Schema of the data sample JSONs generated and saved by
        %   generateSamples() function.
        %   dlCHOMPDatastore expects the JSON
        %   samples to match this format.
        DATA_SAMPLE_SCHEMA = struct( ...
            "ObstaclesFieldName", "SphericalObstacles", ...
            "CentersFieldName", "centers", ...
            "RadiiFieldName", "radius", ...
            "TrajectoryFieldName", "Trajectory", ...
            "EncodingFieldName", "Encoding", ...
            "DataSampleSchema", struct( ...
            "SphericalObstacles",struct( ...
            "centers",[], ...
            "radius",[]), ...
            "Trajectory",[], ...
            "Encoding",[]));
    end

    properties(Access=?matlab.unittest.TestCase)
        %LastGuess Guess generated by the network during the last optimize() call
        %   Guess that is generated by the last call to dlchomp/optimize.
        %   This is a numeric 1-by-P array where P is equal to (N-2) times M
        %   where N is the NumWaypoints and M is of size
        %   obj.RigidBodyTree.TreeInternal.PositionNumber.
        %   Notice that the network only predicts the intermediate
        %   waypoints between the start and goal.
        LastGuess
    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 (1,1) rigidBodyTree
    end

    properties(Dependent)
        %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 the smoothness cost of CHOMP optimizer
        %   Options associated with smoothness cost, specified as a
        %   chompSmoothnessOptions object.
        %
        %   Default: chompSmoothnessOptions
        SmoothnessOptions (1,1) {...
            mustBeA(SmoothnessOptions,'chompSmoothnessOptions')...
                                }

        %SolverOptions Options associated with the solver of CHOMP optimizer
        %   Options associated with the solver, specified as a
        %   chompSolverOptions object.
        SolverOptions (1,1) {...
            mustBeA(SolverOptions,'chompSolverOptions')...
                            }

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

    properties
        %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{...
            dlCHOMP.mustBeSpherical(SphericalObstacles,'SphericalObstacles')}=zeros(4,0)
    end

    properties(SetAccess=immutable)
        %BPSEncoder Basis point set encoder used for encoding obstacle environment
        %   This basis points encoder object is used to encode the target environment
        %   having spherical obstacles. Its output is then fed to input layer-2 of the
        %   underlying neural network.
        BPSEncoder (1,1) bpsEncoder

        %NumWaypoints Total number of waypoints desired in output trajectory
        %   NumWaypoints includes the start and goal hence must be greater than 2.
        %   The waypoints output from the neural network are associated with timepoints
        %   that are linearly interpolated across the [0 1] range and
        %   this trajectory is then fed to CHOMP for optimization.
        NumWaypoints (1,1) double {...
            mustBeGreaterThanOrEqual(NumWaypoints,2),...
            mustBeInteger}=40
    end

    properties(SetAccess=private)
        %Network Neural network used to provide initial guess to CHOMP
        %optimizer
        %   A neural network with two input layers and one output regression layer.
        %   Input layer-1 of size 2 x numel(RigidBodyTree.homeConfiguration())
        %   would correspond to the start and goal configurations.
        %   Input layer-2 would correspond to the basis point set encoding vector
        %   for the environment.
        %   Output layer of size (NumWaypoints-2) x numel(RigidBodyTree.homeConfiguration())
        %   would correspond to the predicted guess. Start and goal
        %   configurations prepended and postpended to this output, respectively, would
        %   form the final predicted guess fed to CHOMP for optimization
        Network (1,1) dlnetwork
    end

    properties (SetAccess=private,Dependent)
        %NumInputs Number of inputs to neural network
        %   Tuple of the input layer sizes of the Network property
        NumInputs (1,2) double {...
            mustBePositive,...
            mustBeInteger}

        %NumOutputs Number of outputs of neural network
        %   Output layer size of the Network property
        NumOutputs (1,1) double {...
            mustBePositive,...
            mustBeInteger}
    end

    % Public methods
    methods
        function obj = dlCHOMP(rbt,encoder,numWaypoints,nvargs)
        %dlCHOMP Constructor
            arguments
                rbt (1,1) rigidBodyTree
                encoder
                numWaypoints
                nvargs.RigidBodyTreeSpheres
                nvargs.Network
                nvargs.SolverOptions
                nvargs.SmoothnessOptions
                nvargs.CollisionOptions
                nvargs.SphericalObstacles
            end
            robotics.manip.internal.RigidBodyTreeUtils.validateTreeWithMovingJoints(...
                rbt,'rigidbodytree:PlanningAtleastOneNonFixedJoint');
            obj.BPSEncoder = encoder;
            obj.NumWaypoints = numWaypoints;
            obj.CHOMPInternal = manipulatorCHOMP(rbt);
            obj.CHOMPInternal.CollisionCostThreshold = obj.COLLISION_COST_THRESHOLD;
            obs=zeros(4,0);
            coder.varsize('obs',[4,inf],[0,1]);
            obj.SphericalObstacles=obs;
            lgraph = robotics.network.internal.defaultDLCHOMPLayerGraph();
            obj.Network = obj.modifyNetworkToPlanningSpec(lgraph);
            userProvidedProperties=fieldnames(nvargs);
            for i=coder.unroll(1:length(userProvidedProperties))
                if strcmp(userProvidedProperties{i},"Network")
                    validateNetwork(obj,nvargs.Network);
                end
                obj.(userProvidedProperties{i})=nvargs.(userProvidedProperties{i});
            end
            obj.SmoothnessOptionsLastTrained = obj.SmoothnessOptions;
            obj.CollisionOptionsLastTrained = obj.CollisionOptions;
            obj.SolverOptionsLastTrained = obj.SolverOptions;
        end

        function newObj=copy(obj)
        %copy Create a deep copy
        %   NEWDLCHOMP=copy(DLCHOMP) returns a deep copy of
        %   dlCHOMP DLCHOMP. DLCHOMP and NEWDLCHOMP are
        %   different objects with same properties.
            newObj=dlCHOMP( ...
                copy(obj.RigidBodyTree), ...
                obj.BPSEncoder, ...
                obj.NumWaypoints, ...
                "Network",obj.Network);
            newObj.SmoothnessOptions=obj.SmoothnessOptions;
            newObj.CollisionOptions=obj.CollisionOptions;
            newObj.SolverOptions=obj.SolverOptions;
            newObj.SphericalObstacles=obj.SphericalObstacles;
            newObj.RigidBodyTreeSpheres=obj.RigidBodyTreeSpheres;
            newObj.SmoothnessOptionsLastTrained = obj.SmoothnessOptionsLastTrained;
            newObj.CollisionOptionsLastTrained = obj.CollisionOptionsLastTrained;
            newObj.SolverOptionsLastTrained = obj.SolverOptionsLastTrained;
            newObj.CHOMPInternal = copy(obj.CHOMPInternal);
        end

        function [trainDS,validDS] = generateSamples(obj,dataOptionsObj,dataFolderPath,nvargs)
        %generateSamples Generate training datasets for training dlCHOMP
        %   [TRAINDS,VALIDDS] = generateSamples(OBJ,DATAOPTIONS,DATAFOLDERPATH,NVARGS) Generates data samples using
        %   parameters specified using dlCHOMPDataOptions DATAOPTIONS and writes the data samples as JSON files
        %   to folder location specified by DATAFOLDERPATH. It then returns TRAINDS training and VALIDDS
        %   validation datastore objects that read this data from DATAFOLDERPATH. NVARGS only accepts the name VERBOSE
        %   which is a logical scalar. If VERBOSE is true, execution is stopped during long runs and errors with recommendations
        %   to improve DATAOPTIONS object input are thrown, otherwise continues to run until DATAOPTIONS.NUMSAMPLES samples are generated.
        %   Property conditions that can cause these errors are:
        %       - RadiusRangeOfObstacles   : Is such that it forces generation
        %                                    of very large obstacles that always collide
        %                                    with the robot irrespective of its start
        %                                    and goal configurations, in which case a
        %                                    dlchomp:ClutteredEnvGenerated error is thrown.
        %                                    Similarly, if this range forces the obstacles
        %                                    to be too small, initial trajectory is
        %                                    always collision-free, in which case a
        %                                    dlCHOMP:SimpleEnvGenerated error is thrown.
        %       - CountRangeOfObstacles    : Is such that it forces generation of a
        %                                    large number of obstacles that always
        %                                    collide with the robot irrespective of its
        %                                    start and goal configurations, in which case
        %                                    a dlchomp:ClutteredEnvGenerated error
        %                                    is thrown. Similarly, if this range forces
        %                                    the number of obstacles to be too small,
        %                                    initial trajectory is always collision-free,
        %                                    in which case a dlCHOMP:SimpleEnvGenerated
        %                                    error is thrown.
        %       - MinDistFromRobotBase     : Is too small such that obstacles always collide
        %                                    with the robot base sphere, so minimum number
        %                                    of desired obstacles cannot be placed, in
        %                                    this case a dlchomp:MinObstaclesCannotBePlaced
        %                                    error is thrown. Similarly, if this value is too
        %                                    large, then again minimum number of desired
        %                                    obstacles cannot be placed within the robot's
        %                                    workspace (reachability space), in which case
        %                                    the same dlCHOMP:MinObstaclesCannotBePlaced
        %                                    error is thrown.
            arguments
                obj
                dataOptionsObj (1,1) dlCHOMPDataOptions
                dataFolderPath (1,1) string
                nvargs.VerboseOutput (1,1) logical=true
            end
            userProvidedProperties=fieldnames(nvargs);
            for i=coder.unroll(1:length(userProvidedProperties))
                obj.(userProvidedProperties{i})=nvargs.(userProvidedProperties{i});
            end

            % Set random stream
            if ~strcmp(dataOptionsObj.RandomStream,"global")
                obj.RandomStreamInternal = RandStream(dataOptionsObj.RandomStream,Seed=dataOptionsObj.Seed);
            else
                obj.RandomStreamInternal = RandStream.getGlobalStream;
            end

            % Obtain positions inside alpha shape workspace for obstacle placement
            stateSpace = robotics.manip.internal.ManipulatorStateSpace(obj.RigidBodyTree);
            collisionValidator = robotics.manip.internal.ManipulatorStateValidator( ...
                stateSpace, ...
                {}, ...
                0.1,...
                "parent");
            collisionValidator.IgnoreSelfCollision = obj.CollisionOptions.IgnoreSelfCollision;
            [workspace,origMinBound,origMaxBound] = obj.computeRobotWorkspace(stateSpace,obj.NUMPOINTS_FOR_ALPHASHAPE);
            positions = obj.samplePointsInWorkspace(workspace,origMinBound,origMaxBound,obj.MAX_NUMPOINTS_IN_ALPHASHAPE);

            % Create data generation progress tracker
            progressTrackerObj = robotics.datagen.internal.progressTracker( ...
                getString(message( ...
                'robotics:robotmanip:dlchomp:DataGenTaskStartMessage')), ...
                getString(message( ...
                'robotics:robotmanip:dlchomp:DataGenSubtaskMessage')), ...
                dataOptionsObj.NumSamples);

            % Retry until desired number of valid samples are obtained
            sampleCount = 0;
            startTime = datetime;
            while sampleCount < dataOptionsObj.NumSamples

                % Create obstacle environment
                numObstacles = randi(obj.RandomStreamInternal,dataOptionsObj.CountRangeOfObstacles);
                [spheres,numObstaclesPlaced] = obj.createEnvironment(positions,numObstacles, ...
                                                                     dataOptionsObj.MinDistanceFromBase,dataOptionsObj.RadiusRangeOfObstacles);
                if numObstaclesPlaced < dataOptionsObj.CountRangeOfObstacles(1)
                    obj.throwEnvironmentGenerationError(obj.ENV_GEN_ERRORS.MinObstaclesPlacement);
                end
                collisionValidator.Environment = spheres(1:numObstaclesPlaced);
                obstacleRadius = zeros(1,numObstaclesPlaced);
                obstacleCenters = zeros(numObstaclesPlaced,3);

                % Create obstacles array for CHOMP object
                obs=zeros(numObstaclesPlaced,4);
                for obsIdx = 1:numObstaclesPlaced
                    obstacleRadius(1,obsIdx) = spheres{obsIdx}.Radius;
                    obstacleCenters(obsIdx,:) = spheres{obsIdx}.Pose(1:3,end)';
                    obs(obsIdx,:)=[spheres{obsIdx}.Radius, spheres{obsIdx}.Pose(1:3,end)'];
                end
                obj.CHOMPInternal.SphericalObstacles = obs';

                % Obtain valid start configurations, goal configurations and
                % optimized trajectories in current obstacle environment
                [batchSamplesWritten,startTime] = obj.saveValidDataSamples(stateSpace,collisionValidator, ...
                                                                           dataOptionsObj.NumSamples-sampleCount, dataOptionsObj.NumSamples, ...
                                                                           obstacleRadius, obstacleCenters, dataFolderPath, ...
                                                                           progressTrackerObj,startTime);
                sampleCount = sampleCount + batchSamplesWritten;
            end

            % Create datastore objects for training and validation datasets
            validationIndices = 1:floor(dataOptionsObj.ValidationSplit * dataOptionsObj.NumSamples);
            trainingIndices = (numel(validationIndices)+1):dataOptionsObj.NumSamples;
            validDS = dlCHOMPDatastore(dataFolderPath,validationIndices);
            trainDS = dlCHOMPDatastore(dataFolderPath,trainingIndices);
        end

        function info = trainDLCHOMP(obj,trainDS,lossType,options)
        %trainDLCHOMP Trains neural network component of dlCHOMP
        %   INFO = trainDLCHOMP(OBJ, TRAINDS, LOSSTYPE, OPTIONS) Trains neural network of dlCHOMP using
        %   TRAINDS training datastore, LOSSTYPE loss and OPTIONS training options.
        %   INFO is a struct containing the same fields as that returned by a similar trainnet call on a dlnetwork object
        %
        %   See also: trainnet
            arguments
                obj
                trainDS (1,1) matlab.io.Datastore
                lossType
                options
            end
            [trainedNetwork, info] = trainnet(trainDS,obj.Network,lossType,options);
            obj.SmoothnessOptionsLastTrained = obj.SmoothnessOptions;
            obj.CollisionOptionsLastTrained = obj.CollisionOptions;
            obj.SolverOptionsLastTrained = obj.SolverOptions;
            obj.Network = trainedNetwork;
        end

        function [optimizedtraj,outtsamples,solinfo]=...
                optimize(obj,start,goal)
        %optimize Optimize neural network guess trajectory using CHOMP
        %   [OPTIMIZEDTRAJ,OUTTSAMPLES,SOLINFO] = optimize(OBJ, START, GOAL) optimizes a trajectory discretized
        %   by linearly interpolating NUMWAYPOINTS (property) waypoints between START and GOAL having timepoints
        %   0 and 1 respectively. Returns the optimized waypoints OPTIMIZEDTRAJ and timepoints OUTTSAMPLES. SOLINFO is a struct
        %   that contains the same fields as that returned by a similar optimize() call on a manipulatorCHOMP object
        %   with an additional field NetworkGuessTime representing the time taken for neural network guess prediction.
        %
        %   See also: manipulatorCHOMP.optimize

        % Validate input arguments
            funcName = "optimize";
            validateattributes( ...
                start, ...
                {'double'},{ ...
                             'nonempty','real','finite', ...
                             'size',[1,obj.RigidBodyTree.TreeInternal.PositionNumber]}, ...
                funcName,"start");
            validateattributes( ...
                goal, ...
                {'double'},{ ...
                             'nonempty','real','finite', ...
                             'size',[1,obj.RigidBodyTree.TreeInternal.PositionNumber]}, ...
                funcName,"goal");

            % Validate obstacles not being empty
            if isempty(obj.SphericalObstacles)
                robotics.manip.internal.error('dlchomp:ObstaclesEmptyForOptimize');
            end

            % Construct inputs for and obtain guess from network
            jointsInput = [start,goal];
            encodingInput = encode(obj.BPSEncoder,obj.SphericalObstacles)';
            tic;
            guessTraj = predict(obj.Network,jointsInput,encodingInput);
            guessTime = toc;

            % Convert guess trajectory format to match CHOMP input format.
            guessTrajForCHOMP = convertNetworkOutputToCHOMPInputFormat(...
                obj,guessTraj,start,goal);
            obj.LastGuess = guessTrajForCHOMP;

            % Make dlCHOMP obstacles known to CHOMPInternal
            obj.CHOMPInternal.SphericalObstacles = obj.SphericalObstacles;

            % Feed network guess to CHOMPInternal for optimization
            tpoints = linspace(0,1,obj.NumWaypoints);
            tstep = 1/(obj.NumWaypoints-1);
            [optimizedtraj,outtsamples,solinfo] = optimize( ...
                obj.CHOMPInternal, ...
                guessTrajForCHOMP, ...
                tpoints, ...
                tstep, ...
                "InitialTrajectoryFitType",obj.INITIAL_TRAJ_TYPE);

            % Add guess time to info struct
            solinfo.NetworkGuessTime = guessTime;
        end

        function ax=show(obj,varargin)
        %show Visualize the trajectory of rigid body tree
        %   show(DLCHOMP,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 are also overlaid on
        %   the visualization.
        %
        %   show(DLCHOMP) visualizes the rigid body tree in its home
        %   configuration.
        %
        %   AX = show(DLCHOMP,_) 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: {}
        %
        %   See also: manipulatorCHOMP.show()

            obj.CHOMPInternal.SphericalObstacles = obj.SphericalObstacles;
            ax=obj.CHOMPInternal.show(varargin{:});
        end

        function resetCHOMPOptions(obj)
        %resetCHOMPOptions Used to reset optimization properties to the
        %last trained state of the object
        %   resetCHOMPOptions(OBJ) Resets the SmoothnessOptions, CollisionOptions and
        %   SolverOptions of dlCHOMP to their values when dlCHOMP/trainDLCHOMP was last
        %   successfully called. If dlCHOMP/trainDLCHOMP was never called, it resets the values
        %   to those used during object construction.
            obj.SmoothnessOptions = obj.SmoothnessOptionsLastTrained;
            obj.CollisionOptions = obj.CollisionOptionsLastTrained;
            obj.SolverOptions = obj.SolverOptionsLastTrained;
        end
    end

    % Getter and Setter methods
    methods
        function rbt=get.RigidBodyTree(obj)
        %get.RigidBodyTree Getter for dependent property RigidBodyTree
            rbt = obj.CHOMPInternal.RigidBodyTree;
        end

        function rigidBodyTreeSpheres = get.RigidBodyTreeSpheres(obj)
        %get.RigidBodyTreeSpheres Getter for dependent property RigidBodyTreeSpheres
            rigidBodyTreeSpheres = obj.CHOMPInternal.RigidBodyTreeSpheres;
        end

        function set.RigidBodyTreeSpheres(obj,tableofspheres)
        %set.RigidBodyTreeSpheres Setter for dependent property RigidBodyTreeSpheres
            obj.CHOMPInternal.RigidBodyTreeSpheres = tableofspheres;
        end

        function smoothnessOptions = get.SmoothnessOptions(obj)
        %get.SmoothnessOptions Getter for dependent property SmoothnessOptions
            smoothnessOptions = obj.CHOMPInternal.SmoothnessOptions;
        end

        function set.SmoothnessOptions(obj,smoothnessOptions)
        %set.SmoothnessOptions Setter for dependent property SmoothnessOptions
            obj.CHOMPInternal.SmoothnessOptions = smoothnessOptions;
        end

        function collisionOptions = get.CollisionOptions(obj)
        %get.CollisionOptions Getter for dependent property CollisionOptions
            collisionOptions = obj.CHOMPInternal.CollisionOptions;
        end

        function set.CollisionOptions(obj,collisionOptions)
        %set.CollisionOptions Setter for dependent property CollisionOptions
            obj.CHOMPInternal.CollisionOptions = collisionOptions;
        end

        function solverOptions = get.SolverOptions(obj)
        %get.SolverOptions Getter for dependent property SolverOptions
            solverOptions = obj.CHOMPInternal.SolverOptions;
        end

        function set.SolverOptions(obj,solverOptions)
        %set.SolverOptions Setter for dependent property SolverOptions
            obj.CHOMPInternal.SolverOptions = solverOptions;
        end

        function numInputs = get.NumInputs(obj)
        %get.NumInputs Getter for dependent property NumInputs
            allLayerNames = {obj.Network.Layers.Name};
            inputLayers = obj.Network.Layers(ismember(allLayerNames,obj.INPUT_LAYER_NAMES));
            numInputs = [inputLayers.InputSize];
        end

        function numOutputs = get.NumOutputs(obj)
        %get.NumOutputs Getter for dependent property NumOutputs
            allLayerNames = {obj.Network.Layers.Name};
            finalLayer = obj.Network.Layers(strcmp(allLayerNames,obj.FINAL_LAYER_NAME));
            numOutputs = finalLayer.OutputSize;
        end
    end

    % Static SphericalObstacles utility
    methods(Static,Access=private)
        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]},...
                               'dlCHOMP',argname)
        end
    end

    % Network Utilities
    methods(Access={?matlab.unittest.TestCase})
        function network = modifyNetworkToPlanningSpec(obj,lgraph)
        %modifyNetworkToPlanningSpec Modifies passed network to meet
        %planning specifications
        %   Changes the input layer graph by modifying the input and final
        %   layers based on the RigidBodyTree, BPSEncoder and NumWaypoints properties.
        %   Input and output layers are assumed to have names as specified in the
        %   help text for validateNetwork().

        % Modify input layer sizes to match planning specification
            jointInputLayerName = obj.INPUT_LAYER_NAMES{1};
            numPositionDoF = obj.RigidBodyTree.TreeInternal.PositionNumber;
            newJointInputLayer = featureInputLayer( ...
                2*numPositionDoF,"Name",jointInputLayerName);
            lgraph = lgraph.replaceLayer( ...
                jointInputLayerName, newJointInputLayer);
            encodingInputLayerName = obj.INPUT_LAYER_NAMES{2};
            newEncodingInputLayer = featureInputLayer( ...
                obj.BPSEncoder.EncodingSize,"Name",encodingInputLayerName);
            lgraph = lgraph.replaceLayer( ...
                encodingInputLayerName, newEncodingInputLayer);

            % Modify output layer size to match planning specification
            finalLayerName = obj.FINAL_LAYER_NAME;
            newFinalLayer = fullyConnectedLayer( ...
                (obj.NumWaypoints-2)*numPositionDoF,"Name",finalLayerName);
            lgraph = lgraph.replaceLayer( ...
                finalLayerName, newFinalLayer);
            network = dlnetwork(lgraph,OutputNames=finalLayerName);
        end

        function validateNetwork(obj, network)
        %validateNetwork Validates the network type, input and output layers of the
        %network.
        %   Checks the network type. Checks the input and output layer names,
        %   types and sizes. There should be input layers with names
        %   INPUT_LAYER_NAMES and a final layer with name FINAL_LAYER_NAME.

        % Validate network input class type
            validateattributes(network, {'dlnetwork'}, {}, '', 'Network')
            if coder.target('MATLAB')

                % Validate layers in simulation mode. In the codegen
                % mode, accessing Layers property is not supported
                if length(network.Layers)<2
                    robotics.manip.internal.error('dlchomp:MinimumNumLayers')
                end
            end

            % Validate input and output layers' names
            inputLayerNameMatches = ismember(obj.INPUT_LAYER_NAMES,network.InputNames);
            if ~all(inputLayerNameMatches)
                robotics.manip.internal.error('dlchomp:InputLayerNotFound', ...
                                              obj.INPUT_LAYER_NAMES{find(~inputLayerNameMatches,1)});
            end
            if ~all(ismember(network.OutputNames,obj.FINAL_LAYER_NAME))
                robotics.manip.internal.error('dlchomp:FinalLayerNotFound', ...
                                              obj.FINAL_LAYER_NAME);
            end

            % Validate input and output layers' types
            allLayerNames = {network.Layers.Name};
            inputLayers = network.Layers(ismember(allLayerNames,obj.INPUT_LAYER_NAMES));
            expectedInputLayerType = 'nnet.cnn.layer.FeatureInputLayer';
            inputLayerTypeMatches = robotics.network.internal.ValidateNetworkUtil.getLayerTypeMatches( ...
                network, ...
                obj.INPUT_LAYER_NAMES, ...
                expectedInputLayerType);
            if ~all(inputLayerTypeMatches)
                robotics.manip.internal.error('dlchomp:InputLayerTypeMismatch', ...
                                              inputLayers(find(inputLayerTypeMatches,1).Name, ...
                                                          expectedInputLayerType));
            end
            expectedFinalLayerType = 'nnet.cnn.layer.FullyConnectedLayer';
            finalLayerMatches = robotics.network.internal.ValidateNetworkUtil.getLayerTypeMatches( ...
                network, ...
                obj.FINAL_LAYER_NAME, ...
                expectedFinalLayerType);
            if ~all(finalLayerMatches)
                robotics.manip.internal.error('dlchomp:FinalLayerTypeMismatch', ...
                                              obj.FINAL_LAYER_NAME,expectedFinalLayerType);
            end

            % Validate network's input and output sizes
            actualInputSizes = [inputLayers.InputSize];
            expectedInputSizes = [obj.RigidBodyTree.TreeInternal.PositionNumber*2, obj.BPSEncoder.EncodingSize];
            inputSizeMatches = actualInputSizes == expectedInputSizes;
            if nnz(~inputSizeMatches)
                robotics.manip.internal.error('dlchomp:InputSizeMismatch', ...
                                              actualInputSizes,expectedInputSizes)
            end
            finalLayer = network.Layers(strcmp(allLayerNames,obj.FINAL_LAYER_NAME));
            actualOutputSize = finalLayer.OutputSize;
            expectedOutputSize = (expectedInputSizes(1)/2) * (obj.NumWaypoints-2);
            outputSizeMatches = actualOutputSize == expectedOutputSize;
            if nnz(~outputSizeMatches)
                robotics.manip.internal.error('dlchomp:OutputSizeMismatch', ...
                                              actualOutputSize,expectedOutputSize);
            end
        end

        function reshapedTraj = convertNetworkOutputToCHOMPInputFormat(...
            obj,networkOutputTraj,start,goal)
        %convertNetworkOutputToCHOMPInputFormat Reshapes network guess
        %trajectory into CHOMP input format
        %   Reshapes a 1-by-P trajectory output by the network into a M-by-N
        %   trajectory suitable as input for CHOMP, where M is the number of
        %   waypoints including start and goal, N is the number of movable
        %   joints of the robot, and P is equal to (M-2) times N. M is equal
        %   to the NUMWAYPOINTS property of the DLCHOMP object

        % Network guess trajectory only includes intermediate waypoints and
        % excludes start and goal.
            networkOutputTrajNumWaypoints = obj.NumWaypoints-2;

            networkOutputTraj = double(networkOutputTraj);
            reshapedGuessTraj = reshape(networkOutputTraj, ...
                                        obj.RigidBodyTree.TreeInternal.PositionNumber, ...
                                        networkOutputTrajNumWaypoints)';
            reshapedTraj = [start;reshapedGuessTraj;goal];
        end
    end

    % Data generation utilities
    methods(Access=private)
        function [spheres,numObstaclesPlaced] = createEnvironment(obj,positions,numObstacles,minDistanceFromBase,radiusRangeOfObstacles)
        %createEnvironment Generates obstacle environment with given specifications
        %   Generates an environment with numObstacles spherical obstacles having
        %   radiusRange radius range placed at positions having a minimum distance
        %   of minDistanceFromBase from robot base
            validPositionIndices = vecnorm(positions,2,2) > minDistanceFromBase;
            validPositions = positions(validPositionIndices,:);
            takenPositions = zeros(size(validPositions,1),1);
            spheres = cell(1,numObstacles);
            numObstaclesPlaced = 0;
            for obsIdx = 1:numObstacles
                obstacle = collisionSphere( ...
                    (radiusRangeOfObstacles(2)-radiusRangeOfObstacles(1)).*rand(obj.RandomStreamInternal,1,1) ...
                    + radiusRangeOfObstacles(1));
                possiblePositionIdxs = vecnorm(validPositions,2,2) > minDistanceFromBase + obstacle.Radius;
                currentPositionIdx = find(possiblePositionIdxs & ~takenPositions,1);
                if ~isempty(currentPositionIdx)
                    currentPosition = validPositions(currentPositionIdx,:);
                    obstacle.Pose = robotics.internal.trvec2tform(currentPosition);
                    spheres{1,obsIdx} = obstacle;
                    takenPositions(currentPositionIdx) = 1;
                    numObstaclesPlaced = numObstaclesPlaced + 1;
                else
                    return;
                end
            end
        end

        function throwEnvironmentGenerationError(obj,errorCase)
        %throwEnvironmentGenerationError Throws an environment generation error
            if obj.VerboseOutput
                switch errorCase
                  case obj.ENV_GEN_ERRORS.ClutteredEnv
                    robotics.manip.internal.error('dlchomp:ClutteredEnvGenerated');
                  case obj.ENV_GEN_ERRORS.SimpleEnv
                    robotics.manip.internal.error('dlchomp:SimpleEnvGenerated');
                  case obj.ENV_GEN_ERRORS.MinObstaclesPlacement
                    robotics.manip.internal.error('dlchomp:MinObstaclesCannotBePlaced');
                end
            end
        end

        function [batchSamplesWritten, startTime] = saveValidDataSamples(obj,stateSpace,collisionValidator, ...
                                                                         samplesLeft, totalNumSamples, obstacleRadius, obstacleCenters, dataFolderPath, ...
                                                                         progressTrackerObj, startTime)
        %saveValidDataSamples Obtain and save valid data samples for training and validation
        %   Obtains and saves valid data samples such that start and goal
        %   configurations are collision-free, initial trajectory has
        %   collisions and final optimized trajectory using CHOMPInternal
        %   is collision-free
            maxFails = floor(obj.MAX_TRIES_PER_ENV * obj.MAX_FAIL_FRAC_PER_ENV);
            startAndGoalConfigs = sampleUniform(stateSpace,2*obj.MAX_TRIES_PER_ENV);
            areStartsAndGoalsValid = isStateValid(collisionValidator,startAndGoalConfigs);
            startAndGoalInvalidCount = 0;
            initTrajIsCollFreeCount = 0;
            batchSamplesWritten = 0;
            for k=1:obj.MAX_TRIES_PER_ENV
                if startAndGoalInvalidCount > maxFails && totalNumSamples == (samplesLeft-batchSamplesWritten)
                    % Cluttered environment generated
                    obj.throwEnvironmentGenerationError(obj.ENV_GEN_ERRORS.ClutteredEnv);
                    return;
                elseif initTrajIsCollFreeCount > maxFails && totalNumSamples == (samplesLeft-batchSamplesWritten)
                    % Simple environment generated
                    obj.throwEnvironmentGenerationError(obj.ENV_GEN_ERRORS.SimpleEnv);
                    return;
                end

                % Check if current start and goal configurations are invalid
                % If so, continue the loop
                isStartAndGoalValid = sum(areStartsAndGoalsValid(k*2-1:k*2))==2;
                if ~isStartAndGoalValid
                    startAndGoalInvalidCount = startAndGoalInvalidCount+1;
                    continue;
                end
                startConfig = startAndGoalConfigs(k*2-1,:);
                goalConfig = startAndGoalConfigs(k*2,:);

                % Compute timestep
                tstep = 1/(obj.NumWaypoints-1);

                % Check if initially fit trajectory between the start
                % and goal configurations has collisions
                % If not, continue the loop
                isInitTrajCollisionFree = checkIfInitialTrajectoryIsCollisionFree( ...
                    obj,startConfig,goalConfig,tstep);
                if isInitTrajCollisionFree
                    initTrajIsCollFreeCount = initTrajIsCollFreeCount+1;
                    continue;
                end

                % Check if final trajectory between the start and goal configurations
                % is collision free
                [isFinalTrajCollisionFree, currPlanFromCHOMP] = checkIfFinalTrajectoryIsCollisionFree( ...
                    obj,startConfig,goalConfig,tstep);
                if isFinalTrajCollisionFree
                    if batchSamplesWritten < samplesLeft
                        % Construct struct data sample from current plan
                        batchSamplesWritten = batchSamplesWritten+1;
                        trajectory = currPlanFromCHOMP;
                        encoding = encode(obj.BPSEncoder,obj.CHOMPInternal.SphericalObstacles);
                        dataSample = obj.constructDataSampleStruct(obstacleCenters,obstacleRadius,trajectory,encoding);

                        % Save struct data sample as JSON file
                        filename = "sample_" + num2str((totalNumSamples-samplesLeft)+batchSamplesWritten) + ".json";
                        fullPathToFile = fullfile(dataFolderPath,filename);
                        obj.writeJSON(dataSample,fullPathToFile);
                        endTime = datetime;
                        sampleGenerationTime = endTime-startTime;

                        % Update progress tracker
                        progressTrackerObj.updateProgress( ...
                            (totalNumSamples-samplesLeft)+batchSamplesWritten, ...
                            sampleGenerationTime);
                        startTime = datetime;
                    else
                        break;
                    end
                end
            end
        end

        function isInitTrajCollisionFree = checkIfInitialTrajectoryIsCollisionFree(obj,startConfig,goalConfig,tstep)
        %checkIfInitialTrajectoryIsCollisionFree Checks if initial trajectory is collision free
        %   Checks if an initial trajectory obtained using obj.INITIAL_TRAJ_TYPE interpolation
        %   between the startConfig and goalConfig using tpoints time points and tstep time step,
        %   is collision free.
        % Compute initial trajectory
            [optimizedtraj_,outtsamples]=...
                obj.CHOMPInternal.initializeTrajectory( ...
                [startConfig;goalConfig],obj.TPOINTS,tstep, ...
                "InitialTrajectoryFitType",obj.INITIAL_TRAJ_TYPE);

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

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

            % Check if initially fit trajectory between the start
            % and goal configurations has collisions
            % If not, continue the loop
            isInitTrajCollisionFree=...
                obj.CHOMPInternal.costOfCollisionAvoidance(...
                flattraj,config0,tstep,0,utils)==0;
        end

        function [isFinalTrajCollisionFree, planCHOMP] = checkIfFinalTrajectoryIsCollisionFree(obj,startConfig,goalConfig,tstep)
        %checkIfFinalTrajectoryIsCollisionFree Checks if final trajectory is collision free
        %   Checks if an optimized trajectory obtained using obj.INITIAL_TRAJ_TYPE interpolation
        %   between the startConfig and goalConfig using tpoints time points and tstep time step,
        %   as initial guess to CHOMPInternal, is collision free.
            [planCHOMP,~,solinfo] = obj.CHOMPInternal.optimize([startConfig;goalConfig], ...
                                                               obj.TPOINTS,tstep, ...
                                                               "InitialTrajectoryFitType",obj.INITIAL_TRAJ_TYPE);
            isFinalTrajCollisionFree = solinfo.IsCollisionFree;
        end

        function [workspace,origMinBound,origMaxBound] = computeRobotWorkspace(obj,stateSpace,numPoints)
        %computeRobotWorkspace Uniformly samples points in state space
        %   Uniformly samples numPoints points from state space stateSpace
        %   and creates an alphaShape object from these points representing
        %   robot workspace. Also returns the original bounds of this workspace
        %   before adjustment to dimensions having zero variance.
            jointConfigs = sampleUniform(stateSpace,numPoints);
            rbtKin = robotics.manip.internal.fastrbt.RigidBodyTreeKinematics(obj.RigidBodyTree);
            tform = rbtKin.forwardKinematics(jointConfigs);
            points3D = tform(1:3,4,end,:);
            sampledPoints = reshape(points3D,3,[])';
            [origMinBound,origMaxBound] = bounds(sampledPoints,1);
            adjustedPoints = obj.adjustDimsWithZeroVariance(sampledPoints);
            workspace = alphaShape(adjustedPoints);
        end

        function positions = samplePointsInWorkspace(obj,workspace,minBound,maxBound,maxNumPositions)
        %samplePointsInWorkspace Uniformly samples points in robot workspace
        %   Uniformly samples maxNumPositions points on a sphere of radius 3
        %   around the robot and returns the ones inside the workspace
        %   alphaShape. Forces the workspace dimensions having zero variance
        %   to take their corresponding mean value.

        % Obtain random elevation, azimuth and radii in spherical
        % coordinate space. Then convert spherical into cartesian
        % coordinates.
            elevation = asin(2*rand(maxNumPositions,1) - 1);
            az = 2*pi*rand(maxNumPositions,1);
            radii = obj.ALPHASHAPE_SAMPLING_RADIUS*...
                    (rand(maxNumPositions,1).^(1/3));
            [x,y,z] = sph2cart(az,elevation,radii);

            % Ensure every non-varying dimension retains its
            % original constant value
            sampledPositions = [x,y,z];
            sampledPositions(:,minBound==maxBound) = ...
                repmat(maxBound(minBound==maxBound), ...
                       [size(sampledPositions,1) 1]);

            % Get sampled positions inside workspace
            inShapeIndices = workspace.inShape(sampledPositions);
            positions = sampledPositions(inShapeIndices,:);
        end
    end

    % Data generation utilities
    methods(Access=private,Static)
        function adjustedPoints = adjustDimsWithZeroVariance(sampledPoints)
        %adjustDimsWithZeroVariance Adds minimal variation to non-varying dimensions of a set of 3D points
            [minBound,maxBound] = bounds(sampledPoints,1);
            adjustedPoints = sampledPoints;
            numPoints = size(adjustedPoints,1);
            uniformVariation = [
                ones(floor(numPoints/2),1)*eps;
                ones(numPoints-floor(numPoints/2),1)*(-eps)];
            adjustedPoints(:,minBound==maxBound) = ...
                adjustedPoints(:,minBound==maxBound) + uniformVariation;
        end

        function writeJSON(dataSample,fullPathToFile)
        %writeJSON Writes jsonString to file with location fullPathToFile
            jsonString = jsonencode(dataSample,PrettyPrint=true);
            [folderName,~,~] = fileparts(fullPathToFile);
            if ~isfolder(folderName)
                mkdir(folderName);
            end
            fjson = fopen(fullPathToFile, 'w');
            fprintf(fjson, '%s', jsonString);
            fclose(fjson);
        end

        function dataSample = constructDataSampleStruct(obstacleCenters,obstacleRadius,trajectory,encoding)
        %constructDataSampleStruct Construct a data sample struct to be saved as a JSON
        %   Constructs a struct with the following fields as per the DATA_SAMPLE_SCHEMA property:
        %       - Obstacles.centers     : N-by-3 numeric array of centers of N spherical obstacles
        %       - Obstacles.radius      : 1-by-N numeric array of radii of N spherical obstacles
        %       - Trajectory            : N-by-M waypoints trajectory returned by manipulatorCHOMP/optimize
        %                                 where N is the number of waypoints and M is the size
        %                                 of joint configuration of rigid body tree of robot.
        %                                 It must include start and goal as the first and last
        %                                 waypoints respectively.
        %       - Encoding              : N-by-1 numeric column vector of spherical obstacles
        %                                 returned by bpsEncoder/encode
            obstacles = struct( ...
                dlCHOMP.DATA_SAMPLE_SCHEMA.CentersFieldName,obstacleCenters, ...
                dlCHOMP.DATA_SAMPLE_SCHEMA.RadiiFieldName,obstacleRadius);
            dataSample = struct( ...
                dlCHOMP.DATA_SAMPLE_SCHEMA.ObstaclesFieldName,obstacles, ...
                dlCHOMP.DATA_SAMPLE_SCHEMA.TrajectoryFieldName,trajectory, ...
                dlCHOMP.DATA_SAMPLE_SCHEMA.EncodingFieldName,encoding);
        end
    end
end

% LocalWords:  RIGIDBODYTREE BPSENCODER NUMWAYPOINTS dlnetwork kinovaGen dlchomp adam startconfig
% LocalWords:  goalconfig DATAOPTIONS minjerkpolytraj JSONs timepoints postpended rigidbodytree
% LocalWords:  DATAFOLDERPATH NVARGS NUMSAMPLES robotmanip OPTIMIZEDTRAJ OUTTSAMPLES SOLINFO
% LocalWords:  timestep cartesian
