%% Create Task-Space Motion Model
% This example shows how to create and use a taskSpaceMotionModel object for a manipulator robot arm in task-space.
%% Create the Robot

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);

%% Set Up the Simulation
% Set the time span to be 1 second with a timestep size of 0.02 seconds. Set the initial state to the home configuration of the robot, with a velocity of zero.

tspan = 0:0.02:1;
initialState = [homeConfiguration(robot);zeros(7,1)];

% Define a reference state with a target position and zero velocity.

refPose = trvec2tform([0.6 -.1 0.5]);
refVel = zeros(6,1);

%% Create the Motion Model
% Model the behavior as a system under proportional-derivative (PD) control.

motionModel = taskSpaceMotionModel("RigidBodyTree",robot,"EndEffectorName","EndEffector_Link");

%% Simulate the Robot
% Simulate the behavior over 1 second using a stiff solver to more efficiently capture the robot dynamics. Using ode15s enables higher precision around the areas with a high rate of change.

[t,robotState] = ode15s(@(t,state)derivative(motionModel,state,refPose,refVel),tspan,initialState);

%% Plot the Response
% Plot the robot's initial position and mark the target with an X.

figure
show(robot,initialState(1:7));
hold all
plot3(refPose(1,4),refPose(2,4),refPose(3,4),"x","MarkerSize",20)

% Observe the response by plotting the robot in a 5 Hz loop.

r = rateControl(5);
for i = 1:size(robotState,1)
    show(robot,robotState(i,1:7)',"PreservePlot",false);
    waitfor(r);
end

% Copyright 2019 The MathWorks, Inc.