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.
robot = loadrobot('frankaEmikaPanda', "DataFormat", 'row');
clearCollision(robot.Bodies{9});
addCollision(robot.Bodies{9}, "cylinder", [0.07, 0.05], trvec2tform([0.0, 0, 0.025]));
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];
rrt = manipulatorRRT(robot,env);
path = plan(rrt, q_start, q_goal);
interpPath = interpolate(rrt,path);
for i = 1:50:size(interpPath,1)
show(robot,interpPath(i,:));
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?
