Usage of addCollision() in Matlab Coder fails with "CollisionSet>>add ... Cannot exceed MaxElements"

I try to export a function that contains calls to addCollision(). But whenever it is called, the Coder fails at the last step (after having generated the test .mex) with the following error.

Test Output:

Error using CollisionSet>>add (line 69)
Cannot exceed MaxElements

Error in RigidBody>>addCollisionInternal (line 344)
 obj.CollisionsInternal.add(collisionGeometry);

Error in rigidBody>>addCollision (line 266)
 obj.BodyInternal.addCollisionInternal(varargin{:})

Error in random_configs (line 35)
 addCollision(bodies{i}, "cylinder", [0.02 0.5]);

I tried several positions in the code and varied the amounts of collision objects (leaving out the first or last one) but with no success. I also tried simple calls with only a "cylinder", [0.02 0.5] argument. See the comments in the following code. I also tried to export the sample code for addCollision() (see below).

My code:

function collidedConfig = random_configs()
    dhparams = zeros(6, 4);
    mm_to_m = 1 / 1000;
    deg_to_rad = pi / 180;
    %                 a (in m)          alpha (in rad)     d (in m)                theta (in rad)
    dhparams(1, :) = [0,                90 * deg_to_rad,   74.5 * mm_to_m,         0];
    dhparams(2, :) = [520 * mm_to_m,    0  * deg_to_rad,   0,                      pi / 2];
    dhparams(3, :) = [58.5 * mm_to_m,   90 * deg_to_rad,   0,                      -pi / 4];
    dhparams(4, :) = [0,                90 * deg_to_rad,   400.72 * mm_to_m,       0];
    dhparams(5, :) = [0,                90 * deg_to_rad,   0,                      pi];
    dhparams(6, :) = [18.5 * mm_to_m,   0  * deg_to_rad,   277 * mm_to_m,          0];

    %% Create Collision boxes
    lengths = [0.1,0.52,0.08,0.4,0.15,0.15];
    boxes = {collisionCylinder(0.02,lengths(1));
             collisionCylinder(0.02,lengths(2));
             collisionCylinder(0.02,lengths(3));
             collisionCylinder(0.02,lengths(4));
             collisionCylinder(0.02,lengths(5));
             collisionCylinder(0.02,lengths(6));
        };
    boxes{1}.Pose = trvec2tform([0, -lengths(1)/2, 0]) * eul2tform([0, 0, pi/2]);
    boxes{2}.Pose = trvec2tform([-lengths(2)/2, 0, 0]) * eul2tform([0, pi/2, 0]);
    boxes{3}.Pose = trvec2tform([-lengths(3)/2, 0, 0]) * eul2tform([0, pi/2, 0]);
    boxes{4}.Pose = trvec2tform([0, -lengths(4)/2, 0]) * eul2tform([0, 0, pi/2]);
    boxes{5}.Pose = trvec2tform([0, 0, +lengths(5)/2]) * eul2tform([pi/2, 0, 0]);
    boxes{6}.Pose = trvec2tform([0, 0, -lengths(6)/2]) * eul2tform([pi/2, 0, 0]);

    %% Create Robot from Parameters
    robot = rigidBodyTree("DataFormat", "row", "MaxNumBodies",24);%6);
    bodies = cell(6,1);
    joints = cell(6,1);
    for i = 1:6
        bodies{i} = rigidBody(['body' num2str(i)]);
        % addCollision(bodies{i}, "cylinder", [0.02 0.5]);
        % addCollision(bodies{i},boxes{i});
        joints{i} = rigidBodyJoint(['jnt' num2str(i)],"revolute");
        joints{i}.HomePosition = dhparams(i, 4);

        setFixedTransform(joints{i},dhparams(i,:),"dh");
        bodies{i}.Joint = joints{i};

        if i == 1 % Add first body to base
            addBody(robot,bodies{i},"base")
        else % Add current body to previous body by name
            addBody(robot,bodies{i},bodies{i-1}.Name)
        end
        addCollision(robot.Bodies{i}, "cylinder", [0.02 lengths(i)]);
    end

%     addBody(robot,bodies{1},"base")
%     addCollision(robot.Bodies{1},boxes{1})
%     addBody(robot,bodies{2},bodies{1}.Name)
%     addCollision(robot.Bodies{2},boxes{2})
%     etc.

    %% Check random poses for collision
    rng(0)
    collidedConfig = zeros(1,6);
    rng('shuffle');
    count = int16(20);
    for i = 1:count
        config = -pi + (pi+pi)*rand(1,6);%randomConfiguration(robot);
        isColliding = checkCollision(robot,config, "SkippedSelfCollisions","parent");
        if isColliding
            collidedConfig = config;
            fprintf('collided at try: %i\n', i);
            break
        end
    end

end

Simplified sample code from the addCollision() help page. This also fails with the error above.

function collidedConfig = random_configs()
    robot = loadrobot('kukaIiwa7','DataFormat','column');
    collisionObj = collisionCylinder(0.05,0.25);
    for i = 1:robot.NumBodies
        if i > 6 && i < 10 % Skip these bodies.
        else
            addCollision(robot.Bodies{i},collisionObj)
        end
    end
    collidedConfig = zeros(1,7);
    rng(0) % Set random seed for repeatability.
    for i = 1:20
        config = randomConfiguration(robot);
        isColliding = checkCollision(robot,config,'SkippedSelfCollisions','parent');
        if isColliding
            collidedConfig = config;
            break
        end
    end
end

[Harald]

Hi Elias,

I can reproduce the problem and, unfortunately, do not have suggestions how to fix it.

My suggestion would be to contact the Technical Support team for detailed investigation, adding the following lines for reproducing the problem:

codegen random_configs -args {}
cf = random_configs;
cf2 = random_configs_mex;

Best wishes,

Harald

Answer 1

[Karsh Tharyani]

Hi Elias,

You need to specify the “MaxNumCollisions” Name-Value argument in the “rigidBody” function. By default, it is zero. Please refer to the help text >>help rigidBody

for details. This Name-Value argument is necessary in codegen and specifies the capacity of collision geometries a rigid body can hold.

Hope that helps, Karsh

[Elias Ortlieb]

Hi Karsh,

many thanks for your answer. It worked!

Unfortunately do I don´t find the argument in the documentation. Maybe this should be added in the Extended Capabilities section of the rigidBody help page. At least this is the case for "MaxNumBodies" for rigidBodyTree.

Best regards Elias