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

2 views (last 30 days)
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
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)
hold on
% 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?

Answers (1)

Karsh Tharyani
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: . 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.
Hope this helps!


Find more on Optimization Toolbox in Help Center and File Exchange

Community Treasure Hunt

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

Start Hunting!