## 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, `vfhCodeGen`, 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` function or the 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 `vfhCodeGen_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` with the `codegen` 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.