%% Modify a Robot Rigid Body Tree Model
% Make changes to an existing rigidBodyTree object. You can get replace joints, bodies and subtrees in the rigid body tree.
% Load example robots as rigidBodyTree objects.

load exampleRobots.mat

%
% View the details of the Puma robot using showdetails.

showdetails(puma1)

%
% Get a specific body to inspect the properties. The only child of the L3 body is the L4 body. You can copy a specific body as well.

body3 = getBody(puma1,'L3');
childBody = body3.Children{1}
body3Copy = copy(body3);

%
% Replace the joint on the L3 body. You must create a new Joint object and use replaceJoint to ensure the downstream body geometry is unaffected. Call setFixedTransform if necessary to define a transform between the bodies instead of with the default identity matrices.

newJoint = rigidBodyJoint('prismatic');
replaceJoint(puma1,'L3',newJoint);

showdetails(puma1)

%
% Remove an entire body and get the resulting subtree using removeBody. The removed body is included in the subtree.

subtree = removeBody(puma1,'L4')

%
% Remove the modified L3 body. Add the original copied L3 body to the  L2 body, followed by the returned subtree. The robot model remains the same. See a detailed comparison through showdetails.

removeBody(puma1,'L3');
addBody(puma1,body3Copy,'L2')
addSubtree(puma1,'L3',subtree)

showdetails(puma1)

% Copyright 2019 The MathWorks, Inc.