Accelerate Robotics Algorithms with Code Generation

You can generate code for select Robotics System Toolbox™ algorithms to speed up their execution. Set up the algorithm that supports code generation as a separate function that you can insert into your workflow. To use code generation, you must have a MATLAB^® Coder™ license. For a list of code generation support in Robotics System Toolbox, see Functions Supporting Code Generation.

For this example, use a inverseKinematics object with a rigidBodyTree robot model to solve for robot configurations that achieve a desired end-effector position.

Create Separate Function for Algorithm

Create a separate function, ikCodegen, that runs the inverse kinematics algorithm. Create inverseKinematics object and build the rigidBodyTree model inside the function. Specify %#codegen inside the function to identify it as a function for code generation.

function qConfig = ikCodegen(endEffectorName,tform,weights,initialGuess)
	%#codegen
    
    robot = rigidBodyTree('MaxNumBodies',3,'DataFormat','row');
    body1 = rigidBody('body1');
    body1.Joint = rigidBodyJoint('jnt1','revolute');

    body2 = rigidBody('body2');
    jnt2 = rigidBodyJoint('jnt2','revolute');
    setFixedTransform(jnt2,trvec2tform([1 0 0]))
    body2.Joint = jnt2;

    body3 = rigidBody('tool');
    jnt3 = rigidBodyJoint('jnt3','revolute');
    setFixedTransform(jnt3,trvec2tform([1 0 0]))
    body3.Joint = jnt3;
    
    addBody(robot,body1,'base')
    addBody(robot,body2,'body1')
    addBody(robot,body3,'body2')


	ik = inverseKinematics('RigidBodyTree',robot);
	
	[qConfig,~] = ik(endEffectorName,tform,weights,initialGuess); 
end 

Save the function in your current folder.

Perform Code Generation for Algorithm

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

Specify sample values for the input arguments.

endEffectorName = 'tool';       
tform = trvec2tform([0.7 -0.7 0]);
weights = [0.25 0.25 0.25 1 1 1];
initialGuess = [0 0 0];

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

codegen ikCodegen -args {endEffectorName,tform,weights,initialGuess}

If your input can come from variable-size lengths, specify the canonical type of the inputs by using coder.typeof (Fixed-Point Designer) with the codegen (MATLAB Coder) function.

Check Performance of Generated Code

Compare the timing of the generated MEX function to the timing of your original function by using timeit.

time = timeit(@() ikCodegen(endEffectorName,tform,weights,initialGuess))
mexTime = timeit(@() ikCodegen_mex(endEffectorName,tform,weights,initialGuess))

time =

    0.0425


mexTime =

    0.0011

The MEX function runs over 30 times faster in this example. Results might vary in your system.

Replace Algorithm Function with MEX Function

Open the main function for running your robotics workflow. Replace the ik object call with the MEX function that you created using code generation. For this example, use the simple 2-D path tracing example.

Open the 2-D Path Tracing with Inverse Kinematics example.

openExample('robotics/TwoDInverseKinematicsExampleExample')

Modify the example code to use the new ikCodegen_mex function. The code that follows is a copy of the example with modifications to use of the new MEX function. Defining the robot model is done inside the function, so skip the Construct the Robot section.

Define the Trajectory

t = (0:0.2:10)'; % Time
count = length(t);
center = [0.3 0.1 0];
radius = 0.15;
theta = t*(2*pi/t(end));
points = center + radius*[cos(theta) sin(theta) zeros(size(theta))];

Inverse Kinematics Solution

Pre-allocate configuration solutions as a matrix, qs. Specify the weights for the end-effector transformation and the end-effector name.

q0 = [0 0 0];
ndof = length(q0);
qs = zeros(count, ndof);
weights = [0, 0, 0, 1, 1, 0];
endEffector = 'tool';

Loop through the trajectory of points to trace the circle. Replace the ik object call with the ikCodegen_mex function. Calculate the solution for each point to generate the joint configuration that achieves the end-effector position. Store the configurations to use later.

qInitial = q0; % Use home configuration as the initial guess
for i = 1:count
    % Solve for the configuration satisfying the desired end effector
    % position
    point = points(i,:);
    qSol = ikCodegen_mex(endEffector,trvec2tform(point),weights,qInitial);
    % Store the configuration
    qs(i,:) = qSol;
    % Start from prior solution
    qInitial = qSol;
end

Animate Solution

Now that all the solutions have been generated. Animate the results. You must recreate the robot because it was originally defined inside the function. Iterate through all the solutions.

robot = rigidBodyTree('MaxNumBodies',15,'DataFormat','row');
body1 = rigidBody('body1');
body1.Joint = rigidBodyJoint('jnt1','revolute');

body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
setFixedTransform(jnt2,trvec2tform([0.3 0 0]))
body2.Joint = jnt2;

body3 = rigidBody('tool');
jnt3 = rigidBodyJoint('jnt3','revolute');
setFixedTransform(jnt3,trvec2tform([0.3 0 0]))
body3.Joint = jnt3;

addBody(robot,body1,'base')
addBody(robot,body2,'body1')
addBody(robot,body3,'body2')

% Show first solution and set view.
figure
show(robot,qs(1,:));
view(2)
ax = gca;
ax.Projection = 'orthographic';
hold on
plot(points(:,1),points(:,2),'k')
axis([-0.1 0.7 -0.3 0.5])

% Iterate through the solutions
framesPerSecond = 15;
r = rateControl(framesPerSecond);
for i = 1:count
    show(robot,qs(i,:),'PreservePlot',false);
    drawnow
    waitfor(r);
end

This example showed you how can you generate code for specific algorithms or functions to improve their speed and simply replace them with the generated MEX function in your workflow.