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

2 views (last 30 days)
William Snider on 26 Sep 2022
Answered: Karsh Tharyani on 4 Oct 2022
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;
% Change collision body to prevent constant self-collision, see code for
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")
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();
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?

Karsh Tharyani on 4 Oct 2022
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

### Categories

Find more on Optimization Toolbox in Help Center and File Exchange

R2022a

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!