function robot = exampleHelperDeltaRobot(includePayload)

% This function is for internal use only and may be removed in a future
% release.

% Copyright 2019 The MathWorks, Inc.

% If no arguments are provided, do not attach a payload body
if nargin < 1
    includePayload = false;
end

% Create the base robot and three subtrees
robot = rigidBodyTree('DataFormat', 'Column');
arm1 = createArm('arm1');
arm2 = createArm('arm2');
arm3 = createArm('arm3');

% The thre arms are attached to the base so that they are equidistant from
% each other. The first arm is attached directly to the base, while the
% other two are attached to the base at positions that are rotated 120
% degrees from the overall base origin. This is achieved by creating bodies
% at each of these locations with transforms that define the 120 degree
% orientation. This is emphasized by the names of these bodies.

% Add first subtree
addSubtree(robot, 'base', arm1);

% Add second subtree at 120 degree rotation
T120 = eul2tform([2*pi/3, 0, 0]);
body = createBodyWithJoint('base_120', 'base_jnt_120', 'fixed', T120);
addBody(robot, body, 'base');
addSubtree(robot, 'base_120', arm2);

% Add last subtree at 240 degrees (or -120)
T240 = eul2tform([-2*pi/3, 0, 0]);
body = createBodyWithJoint('base_240', 'base_jnt_240', 'fixed', T240);
addBody(robot, body, 'base');
addSubtree(robot, 'base_240', arm3);

% Create an end effector body and attach it to the 6th body of the first
% arm at a fixed offset defined by T
T = trvec2tform([-sqrt(3)*0.2/3, 0, -0.1]);
eeBody = createBodyWithJoint('endEffector', 'endEffector_jnt_fixed', 'fixed', T);
addBody(robot, eeBody, 'arm1_body6');

% Optional: Display payload
if includePayload
    T = trvec2tform([0, 0, -0.25]);
    payloadBody = createBodyWithJoint('payload', 'payload_jnt_Fixed', 'fixed', T);
    addBody(robot, payloadBody, 'endEffector');
end
end

function arm = createArm(armPrefix)
%createArm Helper function to cream an individual arm of the delta robot
%   Each arm is modeled as an individual rigidBodyTree object with
%   identical dimensions. To ensure that the resultant trees can be
%   combined, the bodies and joints are named using a prefix, passed as the
%   input, so that the final robot will have bodies and joints that all
%   have unique names but follow the same standard.

% Creat the arm
arm = rigidBodyTree('DataFormat', 'Column');

% Body 1 is the driving joint
d1 = 0.3;
T1 = trvec2tform([d1, 0, 0]) * eul2tform([0, 0, pi/2]);
body1 = createBodyWithJoint([armPrefix '_body1'], [armPrefix '_jnt1'], 'revolute', T1);
addBody(arm, body1, 'base');

% Add body 2
d2 = 0.5;
T2 = trvec2tform([d2, 0, 0]);
body2 = createBodyWithJoint([armPrefix '_body2'], [armPrefix '_jnt2'], 'revolute', T2);
addBody(arm, body2, body1.Name);

% Add body 3
d3 = 0.1;
T3 = trvec2tform([0, 0, -d3]) * eul2tform([0, pi/2, 0]);
body3 = createBodyWithJoint([armPrefix '_body3'], [armPrefix '_jnt3'], 'revolute', T3);
addBody(arm, body3, body2.Name);

% Add body 4
d4 = 0.5;
T4 = trvec2tform([0, -d4, 0]);
body4 = createBodyWithJoint([armPrefix '_body4'], [armPrefix '_jnt4'], 'revolute', T4);
addBody(arm, body4, body3.Name);

% Add body 2
d5 = 0.1;
T5 = trvec2tform([-d5, 0, 0]) * eul2tform([0, -pi/2, 0]);
body5 = createBodyWithJoint([armPrefix '_body5'], [armPrefix '_jnt5'], 'revolute', T5);
addBody(arm, body5, body4.Name);

% Body 6 is a fixed body
d6 = 0.2;
T6 = trvec2tform([-d6, 0, 0]) * eul2tform([0, 0, -pi/2]);
body6 = createBodyWithJoint([armPrefix '_body6'], [armPrefix '_jnt6'], 'fixed', T6);
addBody(arm, body6, body5.Name);
end

function body = createBodyWithJoint(bodyName, jointName, jointType, jointTForm)

    body = rigidBody(bodyName);
    jnt = rigidBodyJoint(jointName, jointType);
    setFixedTransform(jnt, jointTForm);
    body.Joint = jnt;

end