Generate Code for Motion Planning Using Robot Model Imported from URDF

Write Algorithm to Plan Path

Create a function, iiwaPathPlanner, that uses a manipulatorRRT object to plan a path between two configurations for the KUKA LBR iiwa 14 robot model in an obstacle filled environment.

function path = iiwaPathPlanner(startConfig, goalConfig, collisionDims, collisionPoses)
    %#codegen

    % Load the rigid body tree for which the planner will be defined 
    % and make it persistent. This improves the run-time performance
    % of the function since the robot need only be instantiated once.
    persistent robot 
    if isempty(robot)
        robot = iiwaForCodegen('row');
    end
    
    collisionObjects = cell(1,length(collisionDims));
    for i=1:length(collisionDims)
        switch length(collisionDims{i})
            case 1
                sphereRadius = collisionDims{i};
                collisionObjects{i} = collisionSphere(sphereRadius{1});
                collisionObjects{i}.Pose = collisionPoses{i};
            case 2
                cylinderDims = collisionDims{i};
                collisionObjects{i} = collisionCylinder(cylinderDims{1}, cylinderDims{2});
                collisionObjects{i}.Pose = collisionPoses{i};
            case 3
                boxDims = collisionDims{i};
                collisionObjects{i} = collisionBox(boxDims{1}, boxDims{2}, boxDims{3});
                collisionObjects{i}.Pose = collisionPoses{i};
        end
    end

    planner = manipulatorRRT(robot, collisionObjects);
    planner.SkippedSelfCollisions = "parent";
    
    path = plan(planner, startConfig, goalConfig);
end

The algorithm acts as a wrapper for a standard RRT motion planning call. It loads the robot inside the function, accepts standard inputs, and returns a set of robot configuration vectors as the path. Since you cannot use handle objects as the input or output for functions that are supported for code generation. Save the iiwaPathPlanner function in your current folder.

Verify Motion Planning Algorithm in MATLAB

Verify the motion planning algorithm in MATLAB before generating code.

Import a KUKA LBR iiwa 14 robot model and the Robotiq 2F85 gripper model as rigidBodyTree objects. Set the data format to "row".

robot = importrobot('iiwa14.urdf',DataFormat="row");
gripper = loadrobot('robotiq2F85',DataFormat="row");

Incorporate the rigidBodyTree object for the gripper into the rigidBodyTree object for the robot.

addSubtree(robot,"iiwa_link_ee_kuka",gripper,ReplaceBase=true)

Each finger of the Robotiq gripper contains a five-bar linkage. Since closed chains are not supported in rigidBodyTree objects, part of the gripper is always in self-collision. The exampleHelperClearCollisions function prevents the self-collision by clearing the collision mesh of one of the two colliding bodies in each finger.

robot = exampleHelperClearCollisions(robot);

Use the writeAsFunction function to load the rigidBodyTree of the modified robot for code generation.

writeAsFunction(robot,'iiwaForCodegen');

As of MATLAB R2021b, an unmodified rigidBodyTree object can be called directly with importrobot in codegen. It is also possible to directly modify the robot in the function used for code generation.

Create a simple environment with obstacles using collision primitives.

env = {collisionBox(0.5, 0.5, 0.05),collisionSphere(0.15)};
env{1}.Pose = trvec2tform([0.35 0.35 0.3]);
env{2}.Pose = trvec2tform([-0.25 0.1 0.6]);

Define a start and goal configuration. You must specify a start and goal that do not overlap with the obstacles. Otherwise, the planner throws an error.

startConfig = robot.homeConfiguration;
goalConfig = [-2.9236 1.8125 0.6484 1.6414 -0.4698 -0.4181 0.3295 0 0 0 0 0 0];

Visualize initial and final positions of the robot.

figure; 
show(robot,startConfig); 
hold on
show(robot,goalConfig);
show(env{1});
show(env{2});
axis equal

Extract the collision data from the environment.

collisionDims = {{env{1}.X env{1}.Y env{1}.Z},{env{2}.Radius}};
collisionPoses = cellfun(@(x)(x.Pose),env,'UniformOutput',false);

Plan the path.

path = iiwaPathPlanner(startConfig,goalConfig,collisionDims,collisionPoses);

Visualize the robot. Set the 'FastUpdate' option of the show method to true to get a smooth animation.

figure;
ax = show(robot,startConfig);
hold on
show(env{1},'Parent',ax);
show(env{2},'Parent',ax);
axis equal
rrt = manipulatorRRT(robot,env);
interpPath = interpolate(rrt,path);
for i = 1:size(interpPath,1)
    show(robot,interpPath(i,:),'PreservePlot',false,'FastUpdate',true);
    drawnow;
end

Generate Code for Motion Planning Algorithm

You can use either the codegen (MATLAB Coder) function or the MATLAB Coder (MATLAB Coder) app to generate code. For this example, generate a MEX file by calling codegen at the MATLAB command line. Specify sample input arguments for each input to the function using the -args input argument.

Call the codegen function and specify the input arguments in a cell array. This function creates a separate iiwaPathPlanner_mex function to use. You can also produce C code by using the options input argument. This step can take some time.

codegen iiwaPathPlanner -args {startConfig,goalConfig,collisionDims,collisionPoses}

Code generation successful.

Verify Results Using Generated MEX Function

Plan the path by calling the MEX version of the motion planning algorithm for the specified start and goal configurations, and collision data from the environment.

path = iiwaPathPlanner_mex(startConfig,goalConfig,collisionDims,collisionPoses);

Visualize the robot with the robot configurations computed using the MEX version of the motion planning algorithm. Set the 'FastUpdate' option of the show method to true to get a smooth animation.

figure;
ax = show(robot,startConfig);
hold on
show(env{1},'Parent',ax);
show(env{2},'Parent',ax);
axis equal 

interpPath = interpolate(rrt,path);
for i = 1:size(interpPath,1)
    show(robot,interpPath(i,:),'PreservePlot',false,'FastUpdate',true);
    drawnow;
end