%% Generate Robot Trajectory Using Interactive Rigid Body Tree Model
% Use the interactiveRigidBodyTree object to visualize a robot model and interactively create waypoints and use them to generate a smooth trajectory using cubicpolytraj.   For more information, see the interactiveRigidBodyTree object and  cubicpolytraj function.
%
%% Load the Robot Model
% Use the loadrobot function to access provided robot models as rigidBodyTree objects.

robot = loadrobot('abbIrb120'); 

%
%% Visualize Robot and Save Configurations
% Create an interactive tree object using the interactiveRigidBodyTree function. By default, the interactive marker is set to the body with the highest index in the RigidBodyTree property. To change this property using the figure, right-click a rigid body and select Set body as marker body. Alternatively, MarkerBodyName property for the interactiveRigidBodyTree can be set using name-value pairs.

iRBT = interactiveRigidBodyTree(robot); 

%
%% Interactively Add Configurations
% Click and drag the interactive marker to change the robot configuration. You can click and drag any of the axes for linear motion, rotate the body about an axis using the red, green, and blue circles, and drag the center of the interactive marker to position it in 3-D space.
% The interactiveRigidBodyTree object uses inverse kinematics to determine a configuration that achieves the desired end-effector pose. If the associated rigid body cannot reach the marker, the figure renders the best configuration from the inverse kinematics solver.
% When the robot is in a desired configuration use the addConfiguration object function to add the configuration to the StoredConfiguration property of the object.
% In this example, 6 waypoints are created using the interactive marker and addConfiguration object function.  They are saved in wayPoints.mat. Stored configurations can be accessed using iRBT.StoredConfigurations.

load("wayPts.mat");

%
%% Generate Smooth Trajectory Using the Waypoints
% Use the cubicpolytraj function to generate smooth trajectory between the waypoints. Define time points that correspond to each waypoint. Define the time vector for generating the trajectory. The cubicpolyTraj function generates a configuration for each timestep in the timevector tvec.

iRBT.StoredConfigurations = wayPts ;          % Waypoints 
tpts = [0 2 4 6 8 10];                        % Time Points 
tvec = 0:0.1:10;                              % Time Vector 
[q,qd,qdd,pp] = cubicpolytraj(iRBT.StoredConfigurations,tpts,tvec); 

%
%% Visualize Robot Motion on the Trajectory
% Define the simulation frequency using a rateControl object. Use the showFigure function to visualize the robot model and use a for loop to play all the configurations of the robot.

r = rateControl(10);
iRBT.ShowMarker = false;  % Hide the marker 


showFigure(iRBT)

for i = 1:size(q',1)
    iRBT.Configuration = q(:,i);
    waitfor(r);
end     

% Copyright 2020 The MathWorks, Inc.