How to plan manipulator path to avoid collision with a cage / stay inside a constrained region?

I have a 7dof robotic manipulator that is housed inside a cage. The cage is exactly spherical. When I plan a path, I want to ensure that the path does not result in a collision with the cage. What is the best way to do this?

Here is code that demonstrates my problem. In this example, the planned path from q_start to q_goal results in a collision with the sphere. Is there a way to add a spherical constraint to the planner?

This is potentially not the best example, because the q_goal always results in a collision with the sphere. However, when using RRT to plan a path between two valid positions, it is possible that an intermediate waypoint could be in collision with the sphere. This is what I want to check for / add as a constraint to the planner.

clear; close all;

% Load robot
robot = loadrobot('frankaEmikaPanda', "DataFormat", 'row');

% Change collision body to prevent constant self-collision, see code for
% exampleHelperLoadPickAndPlaceRRT function at https://www.mathworks.com/help/robotics/ug/pick-and-place-using-rrt-for-manipulators.html
clearCollision(robot.Bodies{9});
addCollision(robot.Bodies{9}, "cylinder", [0.07, 0.05], trvec2tform([0.0, 0, 0.025]));

% Define start and goal joint positions
q_start = [-0.0121707,-0.561084,0.00127942,-2.60702,-0.0211893,2.03285,0.802306, 0.01, 0.01];
q_goal = [1.11,1.56,1.00,0.60702,-0.0211893,2.03285,0.802306, 0.01,0.01];

% Define sphere ("cage")
radius = 0.70;
x0 = 0;
y0 = 0;
z0 = 0.39;

% Calculate path from start to goal - How to incorporate sphere in path
% planning?
env = {};
rrt = manipulatorRRT(robot,env);
path = plan(rrt, q_start, q_goal);

% Plot interpolated path
interpPath = interpolate(rrt,path);
for i = 1:50:size(interpPath,1)
    show(robot,interpPath(i,:));
    hold on
end

% Plot sphere/cage
[x, y, z] = sphere();
x=x*radius + x0;
y=y*radius + y0;
z=z*radius + z0;
h = surfl(x, y, z); 
set(h, 'FaceAlpha', 0.1)
        

The resulting plot shows that the calculated path results in a collision with the sphere. How can I ensure that at all points along a path, my manipulator is within a spherical region?

Answer 1

[Karsh Tharyani]

Hi William,

I suggest you create a customized state space from a manipulatorStateSpace and use constraintDistanceBounds to sample a joint configuration in sampleUniform that is within your cage's radius.

You can refer to an example which customizes a state space here: https://www.mathworks.com/help/robotics/ug/plan-paths-with-end-effector-constraints.html . It should be straightforward to override the sampleUniform by constraining the default uniformly sampled configuration to be within some distance from the base link via generalizedInverseKinematics and constraintDistanceBounds. https://www.mathworks.com/help/robotics/ref/constraintdistancebounds.html

Hope this helps!

Karsh