classdef capsuleApproximation < robotics.manip.internal.InternalAccess
%CAPSULEAPPROXIMATION Capsule approximation of robot collision geometries
%   CAPSULEAPPROXIMATION object approximates the collision geometries
%   associated with every body in the rigid body tree robot model by
%   fitting a collision capsule. The object allows querying the collision
%   capsules associated with a rigid body in the rigid body tree, and
%   allows visualization. The capsule-approximated rigid body tree can be
%   queried by the property RigidBodyTree.
%
%   CAPAPPROX = capsuleApproximation(ROBOT) creates a capsule approximation
%   object associated with the input rigid body tree ROBOT.
%
%   CAPSULEAPPROXIMATION properties:
%       RigidBodyTree  - Capsule-approximated rigid body tree
%
%   CAPSULEAPPROXIMATION methods:
%       getCapsules    - Get collision capsules of rigid body
%       updatePose     - Update pose of rigid body capsule
%       updateGeometry - Update geometry of rigid body capsule
%       addCapsule     - Add a capsule to rigid body in rigid body tree
%       removeCapsule  - Remove capsule of rigid body in rigid body tree
%       show           - Visualize capsule-approximated rigid body tree
%
%   Example:
%       %Load a robot and create its capsule approximation
%       robot = loadrobot("kinovaGen3");
%       capskinova = capsuleApproximation(robot);
%
%       %Visualize the capsule-approximated robot.
%       show(capskinova.RigidBodyTree,Visuals="off",Collisions="on");
%
%   See also collisionCapsule

%   Copyright 2022 The MathWorks, Inc.

%#codegen
    properties(Dependent)
        %RigidBodyTree - Capsule-approximated rigid body tree
        %   The property is used to access a rigid body tree with every bodies'
        %   collision geometry approximated as a capsule. This property is
        %   read-only.
        RigidBodyTree
    end

    properties(GetAccess=?robotics.manip.internal.capapprox.Utils,SetAccess=immutable)
        %RigidBodyTreeInternal - Internal handle passed by user at construction
        RigidBodyTreeInternal
    end

    properties(Access=private)
        %BodiesCapsules
        BodiesCapsules

        %BaseCapsules
        BaseCapsules
    end

    properties(Access=private,Constant)
        MAX_NUM_CAPSULES=10;
        OVERLAY_CAPSULE_PATCH_FACE_COLOR='#4DBEEE'; %graphics6 in parula
        OVERLAY_CAPSULE_PATCH_FACE_ALPHA=0.5;
        OVERLAY_CAPSULE_PATCH_EDGE_ALPHA=1;
        OVERLAY_CAPSULE_PATCH_LINE_STYLE='none';
        OVERLAY_CAPSULE_PATCH_DISP_NAME_SUFFIX='_coll_mesh';
        RIGIDBODYTREE_SHOW_OPTIONS={'Collisions','on','Visuals','off'};
        UPDATE_POSE_FCN_NAME='updatePose'
        UPDATE_GEOMETRY_FCN_NAME='updateGeometry'
        ADD_CAPSULE_FCN_NAME='addCapsule'
        REMOVE_CAPSULE_FCN_NAME='removeCapsule'
    end

    methods
        function obj=capsuleApproximation(tree)
        %capsuleApproximation Constructor
            validateattributes(tree,{'rigidBodyTree'},...
                               {'nonempty'},...
                               'capsuleApproximation','tree');
            obj.RigidBodyTreeInternal=copy(tree);
            obj.initializeStorageForCapsules(tree);
            obj.populateBodiesCapsules(tree);
            obj.populateBaseCapsules(tree);
        end

        function [capshandles,fitinfo]=getCapsules(obj,bodyname,maxCollisionCapsules)
        %getCapsules Get collision capsules of rigid body
        %   [CAPSULES,FITINFO] = getCapsules(CAPAPPROX,BODYNAME) returns a
        %   cell-array of collision capsules CAPSULES of the rigid body
        %   BODYNAME of the rigid body tree in property RigidBodyTree.
        %   FITINFO is an array of struct with the field "Residuals" which
        %   indicates the error in the fit of a capsule in CAPSULES. The
        %   residual of a capsule at an index corresponds to the collision
        %   geometry of the rigid body BODYNAME at that index.
        %
        %   [_] = getCapsules(CAPAPPROX,BODYNAME,MAXCOLLISIONCAPSULES)
        %   returns the first MAXCOLLISIONCAPSULES number of capsules of the
        %   rigid body BODYNAME during code-generation. If
        %   MAXCOLLISIONCAPSULES is unspecified during code-generation, the
        %   default is 10. If MAXCOLLISIONCAPSULES is specified in MATLAB
        %   execution, it is ignored.
        %
        %   Example:
        %
        %       kinova = loadrobot("kinovaGen3");
        %       capskinova = capsuleApproximation(kinova);
        %       [capsules,fitinfo]=getCapsules(capskinova,'Shoulder_Link');
        %
        %       show(capsules{1})
            [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname);
            if(~isBaseBody)
                caps=obj.BodiesCapsules(bodyIndex);
            else
                caps=obj.BaseCapsules;
            end
            capshandles=repmat({collisionCapsule(eps,eps)},1,size(caps.Radius,2));
            if(~coder.target('MATLAB'))
                if(nargin==2)
                    maxCollisionCapsules=obj.MAX_NUM_CAPSULES;
                end
            else
                maxCollisionCapsules=size(caps.Radius,2);
            end
            for i = coder.unroll(1:maxCollisionCapsules)
                if i <= size(caps.Radius,2)
                    capshandles{i}=collisionCapsule(caps.Radius(i),caps.Length(i));
                    capshandles{i}.Pose=caps.Pose(:,:,i);
                end
            end
            fitinfo=caps.Residuals;
        end

        function addCapsule(obj,bodyname,parameters,pose)
        %addCapsule Add a capsule to a rigid body
        %   addCapsule(CAPAPPROX,BODYNAME,PARAMETERS,POSE) adds a
        %   capsule to the rigid body BODYNAME, of dimensions specified
        %   by PARAMETERS. PARAMETERS is a 1-by-2 row vector with the
        %   elements representing the radius and the length of the
        %   capsule, respectively. POSE specifies the relative
        %   homogeneous transform, as a 4-by-4 matrix, of the capsule
        %   with respect to the rigid body BODYNAME frame.
        %
        %   Example:
        %
        %     kinova = loadrobot("kinovaGen3");
        %     capskinova = capsuleApproximation(kinova);
        %
        %     % Add a capsule to the 'Shoulder_Link' body of radius
        %     % 0.02, and length 0.20, at a relative offset of 0.15 along
        %     % the X-axis of the body frame.
        %     addCapsule(capskinova,'Shoulder_Link',[0.02,0.20],trvec2tform([0.15,0,0]));
        %
        %     show(capskinova,homeConfiguration(kinova));
            [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname);
            validateattributes(parameters,{'double'},{'row','ncols',2},...
                               obj.ADD_CAPSULE_FCN_NAME,'parameters')
            robotics.internal.validation.validatePositiveNumericScalar(parameters(1),...
                                                                       obj.ADD_CAPSULE_FCN_NAME,'radius');
            robotics.internal.validation.validateHomogeneousTransform(pose,...
                                                                      obj.ADD_CAPSULE_FCN_NAME,'pose');
            if(~isBaseBody)
                obj.BodiesCapsules(bodyIndex).Radius=[obj.BodiesCapsules(bodyIndex).Radius,parameters(1)];
                obj.BodiesCapsules(bodyIndex).Length=[obj.BodiesCapsules(bodyIndex).Length,parameters(2)];
                obj.BodiesCapsules(bodyIndex).Pose=cat(3,obj.BodiesCapsules(bodyIndex).Pose,pose);
                obj.BodiesCapsules(bodyIndex).Residuals=...
                    [obj.BodiesCapsules(bodyIndex).Residuals,struct("Residuals",parameters(1))];
                obj.updateBodiesCapsulesResiduals(bodyIndex,length(obj.BodiesCapsules(bodyIndex).Residuals));
            else
                obj.BaseCapsules.Radius=[obj.BaseCapsules.Radius,parameters(1)];
                obj.BaseCapsules.Length=[obj.BaseCapsules.Length,parameters(2)];
                obj.BaseCapsules.Pose=cat(3,obj.BaseCapsules.Pose,pose);
                obj.BaseCapsules.Residuals=[obj.BaseCapsules.Residuals,struct("Residuals",parameters(1))];
                obj.updateBaseCapsulesResiduals(length(obj.BaseCapsules.Residuals));
            end
        end

        function removeCapsule(obj,bodyname,varargin)
        %removeCapsule Remove a capsule from a rigid body
        %   removeCapsule(CAPAPPROX,BODYNAME) removes the last capsule
        %   from the rigid body BODYNAME.
        %
        %   removeCapsule(_,IDX) removes the capsule at index IDX from
        %   the rigid body BODYNAME.
        %
        %   Example:
        %
        %     kinova = loadrobot("kinovaGen3");
        %     capskinova = capsuleApproximation(kinova);
        %
        %     % Remove the last capsule on the 'Shoulder_Link'
        %     removeCapsule(capskinova,'Shoulder_Link');
        %
        %     show(capskinova,homeConfiguration(kinova));
            narginchk(2,3);
            [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname);
            if(~isBaseBody)
                if(isempty(obj.BodiesCapsules(bodyIndex).Radius))
                    robotics.manip.internal.error('capsuleapproximation:EmptyButRemove',bodyname);
                end
                obj.removeCapsuleFromBodiesCapsules(bodyIndex,varargin{:});
            else
                if(isempty(obj.BaseCapsules.Radius))
                    robotics.manip.internal.error('capsuleapproximation:EmptyButRemove',bodyname);
                end
                obj.removeCapsuleFromBaseCapsules(varargin{:});
            end
        end

        function updateGeometry(obj,bodyname,parameters,idx)
        %updateGeometry Update geometry of rigid body capsule
        %    updateGeometry(CAPAPPROX,BODYNAME,PARAMETERS) updates the
        %    geometry of the first collision capsule of rigid body.
        %    PARAMETERS is a 1-by-2 row vector with the elements
        %    representing the radius and the length of the capsule,
        %    respectively.
        %
        %    updateGeometry(_,IDX) updates the geometry of the collision
        %    capsule at the index IDX of the rigid body BODYNAME.
            [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname);
            validateattributes(parameters,{'double'},{'row','ncols',2},'updateGeometry','parameters')
            robotics.internal.validation.validatePositiveNumericScalar(parameters(1),...
                                                                       obj.UPDATE_GEOMETRY_FCN_NAME,'radius');
            if(nargin<4)
                idx=1;
            end
            robotics.internal.validation.validatePositiveIntegerScalar(idx,...
                                                                       obj.UPDATE_GEOMETRY_FCN_NAME,'idx');
            if(~isBaseBody)
                obj.validateIndex(idx,length(obj.BodiesCapsules(bodyIndex).Radius),obj.UPDATE_GEOMETRY_FCN_NAME);
                obj.BodiesCapsules(bodyIndex).Radius(idx)=parameters(1);
                obj.BodiesCapsules(bodyIndex).Length(idx)=parameters(2);
            else
                obj.validateIndex(idx,length(obj.BaseCapsules.Radius),obj.UPDATE_GEOMETRY_FCN_NAME);
                obj.BaseCapsules.Radius(idx)=parameters(1);
                obj.BaseCapsules.Length(idx)=parameters(2);
            end
        end

        function updatePose(obj,bodyname,pose,idx)
        %updatePose Update pose of rigid body capsule
        %    updatePose(CAPAPPROX,BODYNAME,POSE) updates the pose of the
        %    first collision capsule of rigid body BODYNAME. The POSE is
        %    defined with respect to the frame of BODYNAME and is a
        %    homogeneous transformation matrix of size 4-by-4.
        %
        %    updatePose(_,IDX) updates the pose of the collision capsule
        %    at the index IDX of the rigid body BODYNAME.
            [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname);
            robotics.internal.validation.validateHomogeneousTransform(pose,obj.UPDATE_POSE_FCN_NAME,'pose');
            if(nargin<4)
                idx=1;
            end
            robotics.internal.validation.validatePositiveIntegerScalar(idx,obj.UPDATE_POSE_FCN_NAME,'idx');
            if(~isBaseBody)
                obj.validateIndex(idx,length(obj.BodiesCapsules(bodyIndex).Radius),obj.UPDATE_POSE_FCN_NAME);
                obj.BodiesCapsules(bodyIndex).Pose(:,:,idx)=pose;
                obj.updateBodiesCapsulesResiduals(bodyIndex,idx);
            else
                obj.validateIndex(idx,length(obj.BaseCapsules.Radius),obj.UPDATE_POSE_FCN_NAME);
                obj.BaseCapsules.Pose(:,:,idx)=pose;
                obj.updateBaseCapsulesResiduals(idx);
            end
        end

        function rbt=get.RigidBodyTree(obj)
        %get.RigidBodyTree Getter for dependent RigidBodyTree property
        %   The dependent property will be populated with capsule geometries
        %   associated with the base and the bodies of the internal rigid
        %   body tree.
            rbt=copy(obj.RigidBodyTreeInternal);
            obj.addBodiesCapsulesToTree(rbt);
            obj.addBaseCapsulesToTree(rbt);
        end

        function newObj=copy(obj)
        %copy Create a deep copy
            newObj=capsuleApproximation(obj.RigidBodyTree);
        end

        function ax=show(obj,varargin)
        %show Visualize the capsule-approximated rigid body tree
        %   show(CAPAPPROX) shows the collision capsules superimposed on
        %   the rigid body tree's collision geometries, without the
        %   approximation, in the rigid body tree's home configuration.
        %
        %   show(CAPAPPROX,CONFIG) shows the collision capsules
        %   superimposed on the rigid body tree's collision geometries,
        %   without the approximation, given a joint configuration CONFIG.
        %
        %   AX = SHOW(CAPAPPROX, ___) returns the axes handle AX under
        %   which the capsule-approximated rigid body tree is plot.
        %
        %   show(CAPAPPROX,"Parent",AX) specifies the parent axes AX
        %   to plot the capsule-approximated rigid body tree.
        %
        %   Example:
        %        % Load a robot
        %        abb = loadrobot('abbirb120');
        %
        %        % Create the capsule approximation
        %        capsabb = capsuleApproximation(abb);
        %
        %        % Visualize the superimposed approximation
        %        show(capsabb,homeConfiguration(abb));

        % Show the rigid body tree as is.
            [ax,config]=obj.parseShowInputs(varargin{:});
            if isempty(ax)
                ax=show(obj.RigidBodyTreeInternal,...
                    config,...
                    obj.RIGIDBODYTREE_SHOW_OPTIONS{:});
            else
                show(obj.RigidBodyTreeInternal,...
                    config,...
                    'Parent',ax,...
                    obj.RIGIDBODYTREE_SHOW_OPTIONS{:});
            end
            bodynames=obj.RigidBodyTreeInternal.BodyNames;
            hold(ax,'on');
            for i = 1:length(bodynames)+1
                if(i>length(bodynames))
                    capsules=obj.BaseCapsules;
                    bname=obj.RigidBodyTreeInternal.BaseName;

                    % Typically, bodyIndex=0 corresponds to the base,
                    % however, when the collision patch will be configured
                    % downstream it will need to draw the correct
                    % correspondence with the "base" body patch output by
                    % simple show in the RigidBodyTreeVisualizationHelper.
                    % Thus, to comply with that, it must not be zero, but
                    % 1+NumBodies.
                    %
                    % See RigidBodyTreeVisualizationHelper/infoDisplay for
                    % more details.
                    bodyIndex=length(bodynames)+1;
                else
                    bodyIndex=obj.RigidBodyTreeInternal.TreeInternal.findBodyIndexByName(bodynames{i});
                    capsules=obj.BodiesCapsules(bodyIndex);
                    bname=bodynames{i};
                end

                % Compute the transform of the body "bname" with respect to the base
                % body in the input configuration.
                TBase_Body=obj.RigidBodyTreeInternal.getTransform(config,bname);

                % Superimpose the body with the capsule patches
                % corresponding to it. The shown patch must also be
                % configured to make sure it shows the banner of body
                % details when clicked upon- similar to regular rigid body
                % tree "show".
                for j=1:size(capsules.Radius,2)
                    cap=collisionCapsule(capsules.Radius(j),...
                                         capsules.Length(j));
                    cap.Pose=capsules.Pose(:,:,j);
                    cap.Pose=obj.RigidBodyTreeInternal.getTransform(config,bname)*cap.Pose;
                    [~,p]=show(cap,'Parent',ax);
                    obj.configurePatch(p,bname,bodyIndex,TBase_Body);
                end
            end
            hold(ax,'off');
        end
    end
    methods(Access=private,Static)
        function capsule=createVarSizedCapsuleStruct()
        %createVarSizedCapsuleStruct Creates a capsule storage struct.
        %   The struct corresponds to capsules on a rigid body inside the tree.
        %   Each rigid body can have multiple capsules.
            coder.varsize('scalarrowvectorfield',[1,inf],[0,1]);
            coder.varsize('structrowvectorfield',[1,inf],[0,1]);
            coder.varsize('tformfield',[4,4,inf],[0,0,1]);
            coder.varsize('columnvectorfield',[inf,1],[1,0]);
            scalarrowvectorfield=0;
            columnvectorfield=0;
            structrowvectorfield=struct("Residuals",columnvectorfield);
            tformfield=eye(4);
            capsule=struct("Radius",scalarrowvectorfield,...
                           "Length",scalarrowvectorfield,...
                           "Pose",tformfield,...
                           "Residuals",structrowvectorfield);
        end
    end
    methods(Access=private)
        function [ax,config]=parseShowInputs(obj,varargin)
        %parseShowInputs Parse the arguments to show function. 
            parser=inputParser;
            parser.StructExpand=false;
            addOptional(parser,'config',...
                        homeConfiguration(obj.RigidBodyTreeInternal));
            addParameter(parser,'Parent', [], ...
                         @(x)robotics.internal.validation.validateAxesHandle(x));
            parser.parse(varargin{:});
            ax=parser.Results.Parent;
            config=parser.Results.config;
        end
        function initializeStorageForCapsules(obj,tree)
        %initializeStorageForCapsules Initialize storage for rigid body capsules
        %   Assigns the storage for the tree's bodies' and base's collision
        %   capsules.
            capsule=obj.createVarSizedCapsuleStruct();
            obj.BodiesCapsules=repmat(capsule,[1,tree.NumBodies]);
            obj.BaseCapsules=capsule;
        end

        function populateBodiesCapsules(obj,tree)
        %populateBodiesCapsules Populate the bodies' capsule approximation from the robot
        %   Populate the collision capsule approximations from a robot by
        %   fitting collision capsules of the bodies' internal collisions.
            bodynames=tree.BodyNames;
            for i = 1:length(bodynames)
                body=tree.getBody(bodynames{i});
                [radii,lengths,poses,residualfit]=...
                    body.BodyInternal.CollisionsInternal.fitCollisionCapsules();
                bodyIndex=tree.TreeInternal.findBodyIndexByName(bodynames{i});
                obj.BodiesCapsules(bodyIndex).Radius=radii;
                obj.BodiesCapsules(bodyIndex).Length=lengths;
                obj.BodiesCapsules(bodyIndex).Pose=poses;
                obj.BodiesCapsules(bodyIndex).Residuals=residualfit;
            end
        end

        function populateBaseCapsules(obj,tree)
        %populateBaseCapsules Populate the base's capsule approximation from the robot
            [radii,lengths,poses,residualfit]=...
                tree.Base.BodyInternal.CollisionsInternal.fitCollisionCapsules();
            obj.BaseCapsules.Radius=radii;
            obj.BaseCapsules.Length=lengths;
            obj.BaseCapsules.Pose=poses;
            obj.BaseCapsules.Residuals=residualfit;
        end

        function addBodiesCapsulesToTree(obj,tree)
        %addBodiesCapsulesToTree Add the bodies' collision capsules to the robot
            bodynames=tree.BodyNames;
            for i = coder.unroll(1:tree.TreeInternal.MaxNumBodies)
                if(i<=tree.NumBodies)
                    bodyIndex=tree.TreeInternal.findBodyIndexByName(bodynames{i});
                    body=tree.getBody(bodynames{i});
                    setsize=size(obj.BodiesCapsules(bodyIndex).Radius,2);
                    body.BodyInternal.CollisionsInternal=...
                        robotics.manip.internal.CollisionSet(setsize);
                    for j=1:setsize
                        radius=obj.BodiesCapsules(bodyIndex).Radius(j);
                        length=obj.BodiesCapsules(bodyIndex).Length(j);
                        pose=obj.BodiesCapsules(bodyIndex).Pose(:,:,j);
                        body.addCollision("capsule",[radius,length],pose);
                    end
                end
            end
        end

        function addBaseCapsulesToTree(obj,tree)
        %addBodiesCapsulesToTree Add the base's collision capsules to robot
            setsize=size(obj.BaseCapsules.Radius,2);
            tree.TreeInternal.Base.CollisionsInternal=...
                robotics.manip.internal.CollisionSet(setsize);
            for j=1:setsize
                radius=obj.BaseCapsules.Radius(j);
                length=obj.BaseCapsules.Length(j);
                pose=obj.BaseCapsules.Pose(:,:,j);
                tree.Base.addCollision("capsule",[radius,length],pose);
            end
        end

        function removeCapsuleFromBodiesCapsules(obj,bodyIndex,idx)
        %removeCapsuleFromBodiesCapsules Remove capsules from the BodiesCapsules
            if(nargin<3)
                idx=length(obj.BodiesCapsules(bodyIndex).Radius);
            end
            robotics.internal.validation.validatePositiveIntegerScalar(idx,...
                                                                       obj.REMOVE_CAPSULE_FCN_NAME,'idx');
            obj.validateIndex(idx,length(obj.BodiesCapsules(bodyIndex).Radius),obj.REMOVE_CAPSULE_FCN_NAME);
            obj.BodiesCapsules(bodyIndex).Radius(idx)=[];
            obj.BodiesCapsules(bodyIndex).Length(idx)=[];
            obj.BodiesCapsules(bodyIndex).Pose(:,:,idx)=[];
            obj.BodiesCapsules(bodyIndex).Residuals(idx)=[];
            for i=idx:length(obj.BodiesCapsules(bodyIndex).Radius)
                updateBodiesCapsulesResiduals(obj,bodyIndex,i);
            end
        end

        function removeCapsuleFromBaseCapsules(obj,idx)
        %removeCapsuleFromBaseCapsules Remove capsules from the BaseCapsules
            if(nargin<2)
                idx=length(obj.BaseCapsules.Radius);
            end
            robotics.internal.validation.validatePositiveIntegerScalar(idx,...
                                                                       obj.REMOVE_CAPSULE_FCN_NAME,'idx');
            obj.validateIndex(idx,length(obj.BaseCapsules.Radius),obj.REMOVE_CAPSULE_FCN_NAME);
            obj.BaseCapsules.Radius(idx)=[];
            obj.BaseCapsules.Length(idx)=[];
            obj.BaseCapsules.Pose(:,:,idx)=[];
            obj.BaseCapsules.Residuals(idx)=[];
            for i=idx:length(obj.BaseCapsules.Radius)
                updateBaseCapsulesResiduals(obj,i);
            end
        end

        function configurePatch(obj,p,bname,bodyIndex,T)
        %configurePatch Configures the patch associated with a capsule
        %   The visualized patch must show the banner indicating the body
        %   name when clicked upon, and should have some translucency.
            robot=obj.RigidBodyTreeInternal.TreeInternal;
            p.ButtonDownFcn=...
                {@robotics.manip.internal.RigidBodyTreeVisualizationHelper.infoDisplay, ...
                 robot, bodyIndex, T};
            p.FaceAlpha=obj.OVERLAY_CAPSULE_PATCH_FACE_ALPHA;
            p.EdgeAlpha=obj.OVERLAY_CAPSULE_PATCH_EDGE_ALPHA;
            p.FaceColor=obj.OVERLAY_CAPSULE_PATCH_FACE_COLOR;
            p.Tag=robot.ShowTag;
            p.DisplayName=[bname,obj.OVERLAY_CAPSULE_PATCH_DISP_NAME_SUFFIX];
            p.LineStyle=obj.OVERLAY_CAPSULE_PATCH_LINE_STYLE;
        end


        function [bodyIndex,isBaseBody]=validateInputBodyName(obj,bodyname)
        %validateInputBodyName Validate input body name and find if it is Base body
        %   As per the internal RigidBodyTree convention, a body index of 0
        %   corresponds to the base body, and a non-zero index corresponds to a
        %   non-base body.
            bodyIndex=validateInputBodyName(obj.RigidBodyTreeInternal.TreeInternal,bodyname);
            if(bodyIndex>0)
                isBaseBody=false;
            else
                isBaseBody=true;
            end
        end

        function updateBodiesCapsulesResiduals(obj,bodyIndex,idx)
        %updateBodiesCapsulesResiduals Update non-base bodies' capsules' residuals
            body=obj.RigidBodyTreeInternal.TreeInternal.Bodies{bodyIndex};
            obj.BodiesCapsules(bodyIndex).Residuals(idx).Residuals=body.CollisionsInternal.computeResidualsGivenCapsule(...
                obj.BodiesCapsules(bodyIndex).Radius(idx),...
                obj.BodiesCapsules(bodyIndex).Pose(:,:,idx),idx);
        end

        function updateBaseCapsulesResiduals(obj,idx)
        %updateBaseCapsulesResiduals Update base body's capsules' residuals
            body=obj.RigidBodyTreeInternal.TreeInternal.Base;
            obj.BaseCapsules.Residuals(idx).Residuals=body.CollisionsInternal.computeResidualsGivenCapsule(...
                obj.BaseCapsules.Radius(idx),...
                obj.BaseCapsules.Pose(:,:,idx),idx);
        end

    end
    methods(Access=private,Static)
        function validateIndex(idx,maxvalue,fcnname)
            validateattributes(idx,{'numeric'},{'scalar','<=',maxvalue},fcnname,'idx');
        end
    end
end
