To implement the same you would need to modify "isStateValid" routine of a "manipulatorCollisionBodyValidator". You would need to allow the standard checks, and add a check to ensure that a minimum distance is maintained from the environment.
You can refer to the following example where a custom "interpolate" function has been used for "manipulatorStateSpace":
The following is a skeletal code you can use for implementing the same:
classdef exampleHelperCustomCollisionBodyValidator < manipulatorCollisionBodyValidator
properties
MinSeparationDistance % Minimum separation distance from obstacles
end
methods
function obj = exampleHelperCustomCollisionBodyValidator(robotModel, minSeparationDistance)
% Constructor
obj@manipulatorCollisionBodyValidator(robotModel);
obj.MinSeparationDistance = minSeparationDistance;
end
function isValid = isStateValid(obj, state)
% First, check for collisions using the parent method
isValid = isStateValid@manipulatorCollisionBodyValidator(obj, state);
% If there's no collision, check for minimum distance
if isValid
% Retrieve the current configuration's collision bodies
collisionBodies = obj.RigidBodyTree.CollisionMeshes;
% Loop through each collision body to check distance
for i = 1:length(collisionBodies)
[isInCollision, separationDist] = checkCollision(obj, collisionBodies{i}, state);
if isInCollision || separationDist < obj.MinSeparationDistance
% If any collision body is too close, the state is not valid
isValid = false;
return;
end
end
end
end
end
end
Note: This code may need modifications to be accurately implemented
You can refer to the following MATLAB Answer where the same problem was solved:
