open_system('IKTrajectoryControlExample.slx');

% Import the manipulator as a rigidBodyTree Object
sawyer = importrobot('sawyer.urdf');
sawyer.DataFormat = 'column';% Define end-effector body name
eeName = 'right_hand';% Define the number of joints in the manipulator
numJoints = 8;% Visualize the manipulator
show(sawyer);
xlim([-1.00 1.00])
ylim([-1.00 1.00]);
zlim([-1.02 0.98]);
view([128.88 10.45]);

% Initialize size of q0, the robot joint configuration at t=0. This will
% later be replaced by the first waypoint.
q0 = zeros(numJoints,1);% Define a sampling rate for the simulation.
Ts = .01;% Define a [1x6] vector of relative weights on the orientation and
% position error for the inverse kinematics solver.
weights = ones(1,6);% Transform the first waypoint to a Homogenous Transform Matrix for initialization
initTargetPose = eul2tform(wayPoints(1,4:6));
initTargetPose(1:3,end) = wayPoints(1,1:3)';% Solve for q0 such that the manipulator begins at the first waypoint
ik = inverseKinematics('RigidBodyTree',sawyer);
[q0,solInfo] = ik(eeName,initTargetPose,weights,q0);

% Create two platforms
platform1 = collisionBox(0.5,0.5,0.25);
platform1.Pose = trvec2tform([-0.5 0.4 0.2]);platform2 = collisionBox(0.5,0.5,0.25);
platform2.Pose = trvec2tform([0.5 0.2 0.2]);% Add a light fixture, modeled as a sphere
lightFixture = collisionSphere(0.1);
lightFixture.Pose = trvec2tform([.2 0 1]);% Store in a cell array for collision-checking
worldCollisionArray = {platform1 platform2 lightFixture};% Visualize the environment using a helper function that iterates through the collision array.
exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);

robot = loadrobot("kinovaGen3","DataFormat","column","Gravity",[0 0 -9.81]);
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,homeConfiguration(robot),"Parent",ax);

% Generate an array of collision objects from the visuals of the associated tree
collisionArray = exampleHelperManipCollisionsFromVisuals(robot);

startPose = trvec2tform([-0.5,0.5,0.4])*axang2tform([1 0 0 pi]);
endPose = trvec2tform([0.5,0.2,0.4])*axang2tform([1 0 0 pi]);% Use a fixed random seed to ensure repeatable results
rng(0);
ik = inverseKinematics("RigidBodyTree",robot);
weights = ones(1,6);
startConfig = ik("EndEffector_Link",startPose,weights,robot.homeConfiguration);
endConfig = ik("EndEffector_Link",endPose,weights,robot.homeConfiguration);% Show initial and final positions
show(robot,startConfig);
show(robot,endConfig);

% Use a trapezoidal velocity profile to generate a smooth trajectory from the home position to the start position, and then to the end position. Use collision checking to see whether this results in any collisions.
q = trapveltraj([homeConfiguration(robot),startConfig,endConfig],200,"EndTime",2);%Initialize outputs
isCollision = false(length(q),1); % Check whether each pose is in collision
selfCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
worldCollisionPairIdx = cell(length(q),1); % Provide the bodies that are in collisionfor i = 1:length(q)[isCollision(i),selfCollisionPairIdx{i},worldCollisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

problemIdx1 = find(isCollision,1);
problemIdx2 = find(isCollision,1,"last");% Identify the problem rigid bodies
problemBodies1 = [selfCollisionPairIdx{problemIdx1} worldCollisionPairIdx{problemIdx1}*[1 0]'];
problemBodies2 = [selfCollisionPairIdx{problemIdx2} worldCollisionPairIdx{problemIdx2}*[1 0]'];% Visualize the environment
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Add the robots & highlight the problem bodies
show(robot,q(:,problemIdx1),"Parent",ax,"PreservePlot",false);
exampleHelperHighlightCollisionBodies(robot,problemBodies1,ax);
show(robot,q(:,problemIdx2),"Parent"',ax);
exampleHelperHighlightCollisionBodies(robot,problemBodies2,ax);

intermediatePose1 = trvec2tform([-.3 -.2 .6])*axang2tform([0 1 0 -pi/4]); % Out and around the sphere
intermediatePose2 = trvec2tform([0.2,0.2,0.6])*axang2tform([1 0 0 pi]); % Come in from aboveintermediateConfig1 = ik("EndEffector_Link",intermediatePose1,weights,q(:,problemIdx1));
intermediateConfig2 = ik("EndEffector_Link",intermediatePose2,weights,q(:,problemIdx2));% Show the new intermediate poses
ax = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);
show(robot,intermediateConfig1,"Parent",ax,"PreservePlot",false);
show(robot,intermediateConfig2,"Parent",ax);% Generate a new trajectory.
[q,qd,qdd,t] = trapveltraj([homeConfiguration(robot),intermediateConfig1,startConfig,intermediateConfig2,endConfig],200,"EndTime",2);% Verify that it is collision-free.
% Initialize outputs
isCollision = false(length(q),1); % Check whether each pose is in collision
collisionPairIdx = cell(length(q),1); % Provide the bodies that are in collision
for i = 1:length(q)[isCollision(i),collisionPairIdx{i}] = exampleHelperManipCheckCollisions(robot,collisionArray,worldCollisionArray,q(:,i),false);
end
isTrajectoryInCollision = any(isCollision)

% Plot the environment
ax2 = exampleHelperVisualizeCollisionEnvironment(worldCollisionArray);% Visualize the robot in its home configuration
show(robot,startConfig,"Parent",ax2);% Update the axis size
axis equal% Loop through the other positions
for i = 1:length(q)show(robot,q(:,i),"Parent",ax2,"PreservePlot",false);% Update the figure    drawnow
end

% This example uses a model of the Rethink Sawyer, a 7 degree-of-freedom robot manipulator. Call importrobot to generate a rigidBodyTree model from a description stored in a Unified Robot Description Format (URDF) file. Set the DataFormat and Gravity properties to be consistent with Simscape.
sawyer = importrobot('sawyer.urdf');
sawyer.removeBody('head');
sawyer.DataFormat = 'column';
sawyer.Gravity = [0, 0, -9.80665];

% First, assign the start time and duration for the trajectory.
tStart = 0.5;
tDuration = 3;% Next, assign the initial and target configuration. q0 is the initial configuration and q1 is the target configuration.
q0 = [0; -1.18; 0; 2.18; 0; -1.0008; 3.3161];
q1 = zeros(7,1);

% Use exampleHelperPolynomialTrajectory to generate the desired motion trajectory for each joint. exampleHelperPolynomialTrajectory generates the polynomial coefficients of a trajectory that satisfies the desired joint position, zero velocity and zero acceleration based on the initial and target configurations and the trajectory duration.
p = exampleHelperPolynomialTrajectory(q0,q1,tDuration);

open_system('robotSafeTrajectoryTracking.slx');