├── utilities ├── gen3.mat ├── cylinder.stl ├── gen3positions.mat ├── trajExampleUtils.slx ├── waypointPublisher.mlapp ├── publishWaypoints.m ├── createWaypointData.m ├── importGen3Model.m ├── visualizeRobot.m └── plotTrajectory.m ├── simulink ├── manipJointTrajectory.slx ├── manipCartesianTrajectory.slx ├── manipRotationTrajectory.slx ├── manipTransformTrajectory.slx ├── rosManipJointTrajectory.slx ├── rosManipCartesianTrajectory.slx ├── rosManipRotationTrajectory.slx ├── rosManipTransformTrajectory.slx ├── manipTransformTrajectoryTimeScaling.slx └── rosManipTransformTrajectoryTimeScaling.slx ├── startupExample.m ├── matlab ├── manipTrajTransform.m ├── manipTrajCartesian.m ├── manipTrajTransformTimeScaling.m ├── manipTrajLinearRotation.m ├── manipTrajJoint.m └── compareTaskVsJointTraj.m └── README.md /utilities/gen3.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/utilities/gen3.mat -------------------------------------------------------------------------------- /utilities/cylinder.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/utilities/cylinder.stl -------------------------------------------------------------------------------- /utilities/gen3positions.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/utilities/gen3positions.mat -------------------------------------------------------------------------------- /utilities/trajExampleUtils.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/utilities/trajExampleUtils.slx -------------------------------------------------------------------------------- /simulink/manipJointTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/manipJointTrajectory.slx -------------------------------------------------------------------------------- /utilities/waypointPublisher.mlapp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/utilities/waypointPublisher.mlapp -------------------------------------------------------------------------------- /simulink/manipCartesianTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/manipCartesianTrajectory.slx -------------------------------------------------------------------------------- /simulink/manipRotationTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/manipRotationTrajectory.slx -------------------------------------------------------------------------------- /simulink/manipTransformTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/manipTransformTrajectory.slx -------------------------------------------------------------------------------- /simulink/rosManipJointTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/rosManipJointTrajectory.slx -------------------------------------------------------------------------------- /simulink/rosManipCartesianTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/rosManipCartesianTrajectory.slx -------------------------------------------------------------------------------- /simulink/rosManipRotationTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/rosManipRotationTrajectory.slx -------------------------------------------------------------------------------- /simulink/rosManipTransformTrajectory.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/rosManipTransformTrajectory.slx -------------------------------------------------------------------------------- /simulink/manipTransformTrajectoryTimeScaling.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/manipTransformTrajectoryTimeScaling.slx -------------------------------------------------------------------------------- /simulink/rosManipTransformTrajectoryTimeScaling.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mathworks-robotics/trajectory-planning-robot-manipulators/HEAD/simulink/rosManipTransformTrajectoryTimeScaling.slx -------------------------------------------------------------------------------- /startupExample.m: -------------------------------------------------------------------------------- 1 | % Trajectory Planning of Robot Manipulators with MATLAB and Simulink 2 | % Startup script 3 | % 4 | % Copyright 2019 The Mathworks, Inc. 5 | 6 | clear, clc, close all; 7 | disp('Starting Robot Manipulator Trajectory Example...') 8 | 9 | % Set up the MATLAB search path 10 | rootDir = fileparts(which(mfilename)); 11 | addpath(genpath('matlab'),genpath('simulink'),genpath('utilities')); 12 | 13 | % Set the code generation and cache folders to a work folder 14 | % If the folder does not exist, create it 15 | if ~isfolder('work') 16 | mkdir('work'); 17 | end 18 | Simulink.fileGenControl('set','CacheFolder','work','CodeGenFolder','work'); -------------------------------------------------------------------------------- /utilities/publishWaypoints.m: -------------------------------------------------------------------------------- 1 | % Publishes waypoints as ROS messages to test ROS interface 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% Setup 5 | rosshutdown; 6 | rosinit; % You can add your own IP here if using an external ROS master 7 | [wpPub,wpMsg] = rospublisher('/waypoints','geometry_msgs/PoseArray'); 8 | [wpTimePub,wpTimeMsg] = rospublisher('/waypoint_times','std_msgs/Float64MultiArray'); 9 | 10 | %% Publish waypoint data 11 | % NOTE: Requires variables to be defines as in the |createWaypointData| script. 12 | wpMsg.Poses = []; % Clear out any old data 13 | for idx = 1:size(waypoints,2) 14 | poseMsg = rosmessage('geometry_msgs/Pose'); 15 | poseMsg.Position.X = waypoints(1,idx); 16 | poseMsg.Position.Y = waypoints(2,idx); 17 | poseMsg.Position.Z = waypoints(3,idx); 18 | quaternion = eul2quat(orientations(:,idx)'); 19 | poseMsg.Orientation.W = quaternion(1); 20 | poseMsg.Orientation.X = quaternion(2); 21 | poseMsg.Orientation.Y = quaternion(3); 22 | poseMsg.Orientation.Z = quaternion(4); 23 | wpMsg.Poses(idx) = poseMsg; 24 | end 25 | 26 | % Package and publish the waypoint times 27 | wpTimeMsg.Data = waypointTimes; 28 | 29 | % Send the messages 30 | send(wpPub,wpMsg); 31 | send(wpTimePub,wpTimeMsg); -------------------------------------------------------------------------------- /utilities/createWaypointData.m: -------------------------------------------------------------------------------- 1 | % Create sample waypoint data for trajectory generation 2 | % NOTE: Modify this script with your own rigid body tree and 3 | % trajectory reference points 4 | % (We recommend saving a copy of this file) 5 | % 6 | % Copyright 2019 The MathWorks, Inc. 7 | 8 | %% Common parameters 9 | % Rigid Body Tree information 10 | load gen3 11 | load gen3positions 12 | eeName = 'Gripper'; 13 | numJoints = numel(gen3.homeConfiguration); 14 | ikInitGuess = gen3.homeConfiguration; 15 | 16 | % Maximum number of waypoints (for Simulink) 17 | maxWaypoints = 20; 18 | 19 | % Positions (X Y Z) 20 | waypoints = toolPositionHome' + ... 21 | [0 0 0.2 ; -0.1 0.2 0.4 ; -0.2 0 0.1 ; -0.1 -0.2 0.4 ; 0 0 0.2]'; 22 | 23 | % Euler Angles (Z Y X) relative to the home orientation 24 | orientations = [0 0 0; 25 | pi/8 0 0; 26 | 0 pi/2 0; 27 | -pi/8 0 0; 28 | 0 0 0]'; 29 | 30 | % Array of waypoint times 31 | waypointTimes = 0:4:16; 32 | 33 | % Trajectory sample time 34 | ts = 0.2; 35 | trajTimes = 0:ts:waypointTimes(end); 36 | 37 | %% Additional parameters 38 | 39 | % Boundary conditions (for polynomial trajectories) 40 | % Velocity (cubic and quintic) 41 | waypointVels = 0.1 *[ 0 1 0; 42 | -1 0 0; 43 | 0 -1 0; 44 | 1 0 0; 45 | 0 1 0]'; 46 | 47 | % Acceleration (quintic only) 48 | waypointAccels = zeros(size(waypointVels)); 49 | 50 | % Acceleration times (trapezoidal only) 51 | waypointAccelTimes = diff(waypointTimes)/4; 52 | -------------------------------------------------------------------------------- /utilities/importGen3Model.m: -------------------------------------------------------------------------------- 1 | function importGen3Model 2 | % Imports Kinova Gen3 Ultra lightweight robot model into a Rigid Body Tree 3 | % 4 | % Copyright 2019 The MathWorks, Inc. 5 | 6 | % Load the URDF file 7 | % 8 | % NOTE: This requires you to have the kortex_description folder on 9 | % your path, which you can download from 10 | % https://github.com/Kinovarobotics/ros_kortex.git 11 | % 12 | % Then, you have to convert the gen3.xacro file to a URDF file using 13 | % the following commands in a ROS enabled terminal: 14 | % $ cd PATH/TO/ros_kortex/kortex_description/robots 15 | % $ rosrun xacro xacro --inorder -o gen3.urdf gen3.xacro 16 | addpath(genpath('kortex_description')) 17 | gen3 = importrobot('gen3.urdf'); 18 | 19 | % Add a "dummy" gripper link 20 | gripperLength = 0.1; % Gripper length in meters 21 | gripperBody = rigidBody('Gripper'); 22 | gripperJoint = rigidBodyJoint('GripperLink','fixed'); 23 | T = rotm2tform([0 1 0;0 0 1;1 0 0]) * trvec2tform([gripperLength 0 0]); 24 | setFixedTransform(gripperJoint,T); % Move and orient the gripper 25 | gripperBody.Joint = gripperJoint; 26 | addBody(gen3,gripperBody,'end_effector_link'); 27 | % Add a "dummy" mesh 28 | addVisual(gen3.Bodies{9},'Mesh','cylinder.stl', ... 29 | trvec2tform([-0.1 0 0]) * axang2tform([0 1 0 pi/2])); 30 | 31 | % Configure the data format and show the robot in the home position 32 | gen3.DataFormat = 'row'; 33 | load gen3positions 34 | show(gen3,jointAnglesHome'); 35 | 36 | % Save the Rigid Body Tree to a file 37 | curDir = pwd; 38 | saveDir = fileparts(mfilename('fullpath')); 39 | cd(saveDir) 40 | save gen3 gen3 41 | cd(curDir) 42 | 43 | end -------------------------------------------------------------------------------- /utilities/visualizeRobot.m: -------------------------------------------------------------------------------- 1 | function visualizeRobot(rbtName,eeName,jointAngles,waypoints,orientations,mode) 2 | % Helper function to visualize rigid body tree in Simulink 3 | % Copyright 2019 The MathWorks, Inc. 4 | 5 | % Load rigid body tree and create graphics objects on the first function call 6 | persistent robot hTraj hWaypt ax prevWaypoints prevOrientations 7 | if isempty(robot) 8 | close all 9 | figure 10 | robot = evalin('base',rbtName); 11 | robot.DataFormat = 'row'; 12 | end 13 | 14 | % Create graphics objects on the first function call or if waypoints arechanged 15 | if isempty(prevWaypoints) || ~isequal(prevWaypoints,waypoints) || ~isequal(prevOrientations,orientations) 16 | % Clear the axes 17 | clf('reset'); 18 | 19 | % Show the rigid body tree and points again 20 | ax = show(robot,robot.homeConfiguration,'Frames','off'); 21 | hold on 22 | title('Robot Trajectory Visualization'); 23 | 24 | if mode == 2 % Trajectory points 25 | handTform = getTransform(robot,jointAngles',eeName); 26 | handPos = tform2trvec(handTform); 27 | hTraj = plot3(ax,handPos(1),handPos(2),handPos(3),'b.-'); 28 | hWaypt = scatter3(ax,waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 29 | end 30 | 31 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 32 | drawnow; 33 | 34 | prevWaypoints = waypoints; 35 | prevOrientations = orientations; 36 | end 37 | 38 | % Display the rigid body tree 39 | show(robot,jointAngles','PreservePlot',false,'Frames','off','Parent',ax); 40 | handTform = getTransform(robot,jointAngles',eeName); 41 | 42 | % Display the trajectory and waypoint locations 43 | if mode == 2 % Trajectory points 44 | handPos = tform2trvec(handTform); 45 | set(hTraj,'XData',[get(hTraj,'XData') handPos(1)], ... 46 | 'YData',[get(hTraj,'YData') handPos(2)], ... 47 | 'ZData',[get(hTraj,'ZData') handPos(3)]); 48 | elseif mode == 3 % Coordinate frames 49 | plotTransforms(tform2trvec(handTform),tform2quat(handTform),'FrameSize',0.05); 50 | end 51 | 52 | if ~isscalar(waypoints) 53 | set(hWaypt,'XData',waypoints(1,:),'YData',waypoints(2,:),'ZData',waypoints(3,:)); 54 | end 55 | 56 | drawnow; 57 | -------------------------------------------------------------------------------- /matlab/manipTrajTransform.m: -------------------------------------------------------------------------------- 1 | % MANIPULATOR TRAJECTORY GENERATION 2 | % Generates combined transform (rotation and translation) trajectories 3 | % using linear interpolation. 4 | % 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | %% Setup 8 | clear, clc, close all 9 | 10 | % Define waypoint information 11 | createWaypointData; 12 | 13 | % Define IK 14 | ik = inverseKinematics('RigidBodyTree',gen3); 15 | ikWeights = [1 1 1 1 1 1]; 16 | ikInitGuess = gen3.homeConfiguration; 17 | 18 | % Set up plot 19 | plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames 20 | show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false); 21 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 22 | hold on 23 | if plotMode == 1 24 | hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-'); 25 | end 26 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 27 | 28 | %% Generate and follow trajectory 29 | % Loop through segments one at a time 30 | trajType = 'trap'; 31 | numWaypoints = size(waypoints,2); 32 | for w = 1:numWaypoints-1 33 | 34 | % Get the initial and final transforms and times for the segment 35 | T0 = trvec2tform(waypoints(:,w)') * eul2tform(orientations(:,w)'); 36 | Tf = trvec2tform(waypoints(:,w+1)') * eul2tform(orientations(:,w+1)'); 37 | timeInterval = waypointTimes(w:w+1); 38 | trajTimes = timeInterval(1):ts:timeInterval(2); 39 | 40 | % Find the transforms from trajectory generation 41 | [T,vel,acc] = transformtraj(T0,Tf,timeInterval,trajTimes); 42 | 43 | % Trajectory visualization for the segment 44 | if plotMode == 1 45 | eePos = tform2trvec(T); 46 | set(hTraj,'xdata',eePos(:,1),'ydata',eePos(:,2),'zdata',eePos(:,3)); 47 | elseif plotMode == 2 48 | plotTransforms(tform2trvec(T),tform2quat(T),'FrameSize',0.05); 49 | end 50 | for idx = 1:numel(trajTimes) 51 | % Solve IK 52 | tgtPose = T(:,:,idx); 53 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 54 | ikInitGuess = config; 55 | 56 | % Show the robot 57 | show(gen3,config,'Frames','off','PreservePlot',false); 58 | title(['Trajectory at t = ' num2str(trajTimes(idx))]) 59 | drawnow 60 | end 61 | 62 | end -------------------------------------------------------------------------------- /matlab/manipTrajCartesian.m: -------------------------------------------------------------------------------- 1 | % MANIPULATOR TRAJECTORY GENERATION 2 | % Generates Cartesian only (no rotation) trajectories 3 | % 4 | % Copyright 2019 The MathWorks, Inc. 5 | 6 | %% Setup 7 | clear, clc, close all 8 | 9 | % Define waypoint information 10 | createWaypointData; 11 | 12 | % Define IK 13 | ik = inverseKinematics('RigidBodyTree',gen3); 14 | ikWeights = [1 1 1 1 1 1]; 15 | ikInitGuess = gen3.homeConfiguration; 16 | 17 | % Set up plot 18 | plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames 19 | show(gen3,jointAnglesHome','Frames','off','PreservePlot',false); 20 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 21 | hold on 22 | if plotMode == 1 23 | hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-'); 24 | end 25 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 26 | 27 | %% Generate trajectory 28 | % Cartesian Motion only 29 | trajType = 'trap'; 30 | switch trajType 31 | case 'trap' 32 | [q,qd,qdd] = trapveltraj(waypoints,numel(trajTimes), ... 33 | 'AccelTime',repmat(waypointAccelTimes,[3 1]), ... 34 | 'EndTime',repmat(diff(waypointTimes),[3 1])); 35 | 36 | case 'cubic' 37 | [q,qd,qdd] = cubicpolytraj(waypoints,waypointTimes,trajTimes, ... 38 | 'VelocityBoundaryCondition',waypointVels); 39 | 40 | case 'quintic' 41 | [q,qd,qdd] = quinticpolytraj(waypoints,waypointTimes,trajTimes, ... 42 | 'VelocityBoundaryCondition',waypointVels, ... 43 | 'AccelerationBoundaryCondition',waypointAccels); 44 | 45 | case 'bspline' 46 | ctrlpoints = waypoints; % Can adapt this as needed 47 | [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes); 48 | 49 | otherwise 50 | error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline'''); 51 | end 52 | 53 | % Show the full trajectory with the rigid body tree 54 | if plotMode == 1 55 | set(hTraj,'xdata',q(1,:),'ydata',q(2,:),'zdata',q(3,:)); 56 | elseif plotMode == 2 57 | plotTransforms(q',repmat([1 0 0 0],[size(q,2) 1]),'FrameSize',0.05); 58 | end 59 | 60 | % To visualize the trajectory, run the following line 61 | % plotTrajectory(trajTimes,q,qd,qdd,'Names',["X","Y","Z"],'WaypointTimes',waypointTimes) 62 | 63 | %% Trajectory following loop 64 | for idx = 1:numel(trajTimes) 65 | % Solve IK 66 | tgtPose = trvec2tform(q(:,idx)'); 67 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 68 | ikInitGuess = config; 69 | 70 | % Show the robot 71 | show(gen3,config,'Frames','off','PreservePlot',false); 72 | title(['Trajectory at t = ' num2str(trajTimes(idx))]) 73 | drawnow 74 | end -------------------------------------------------------------------------------- /utilities/plotTrajectory.m: -------------------------------------------------------------------------------- 1 | function plotTrajectory(t,q,varargin) 2 | % Plots trajectory given waypoints 3 | % The rules are: 4 | % a. Each joint/coordinate is plotted in a separate figure window 5 | % b. If derivatives (qd and qdd) are optionally specified, they will be 6 | % placed in additional axes of a subplot 7 | % c. Optional name-value pairs can be used after the above arguments 8 | % 9 | % Examples: 10 | % plotTrajectory(t,q) 11 | % plotTrajectory(t,q,qd,'Names',["ShoulderAngle","ElbowAngle","WristAngle"]) 12 | % plotTrajectory(t,q,qd,qdd,'WaypointTimes',waypointTimes,'Names',["X","Y","Z"]) 13 | % 14 | % Copyright 2019 The MathWorks, Inc. 15 | 16 | % Find the order of the trajectory (how many derivatives are provided) 17 | order = 1; % Position only 18 | if nargin > 2 && isnumeric(varargin{1}) 19 | order = 2; % Position and velocity 20 | if nargin > 3 && isnumeric(varargin{2}) 21 | order = 3; % Position, velocity, and acceleration 22 | end 23 | end 24 | 25 | % Initialize default plot options 26 | numCoords = size(q,1); 27 | configNames = "Coordinate " + string(1:numCoords); % Coordinate names 28 | waypointTimes = []; 29 | 30 | % Now look for additional property-value pairs 31 | 32 | if (nargin > order + 1) 33 | searchIdx = order; 34 | while searchIdx <= nargin-order 35 | propName = lower(varargin{searchIdx}); 36 | switch propName 37 | case 'waypointtimes' % Waypoint times 38 | waypointTimes = varargin{searchIdx+1}; 39 | case 'names' % Set coordinate names 40 | configNames = varargin{searchIdx+1}; 41 | end 42 | searchIdx = searchIdx + 2; 43 | end 44 | end 45 | 46 | % Loop through all the coordinates and plot 47 | for idx = 1:numCoords 48 | figure; 49 | 50 | % Always plot time and position only 51 | subplot(order,1,1), hold on; 52 | plot(t,q(idx,:)); 53 | plotTimeLines(waypointTimes); 54 | ylabel('Position'); 55 | title("Trajectory for " + configNames(idx)) 56 | 57 | % Plot velocity if provided 58 | if nargin > 2 59 | subplot(order,1,2), hold on; 60 | qd = varargin{1}; 61 | plot(t,qd(idx,:)); 62 | plotTimeLines(waypointTimes); 63 | ylabel('Velocity'); 64 | 65 | % Plot acceleration if provided 66 | if nargin > 3 67 | subplot(order,1,3), hold on; 68 | qdd = varargin{2}; 69 | plot(t,qdd(idx,:)); 70 | plotTimeLines(waypointTimes); 71 | ylabel('Acceleration'); 72 | end 73 | end 74 | 75 | % Finally, label the time axis 76 | xlabel('Time'); 77 | 78 | end 79 | 80 | end 81 | 82 | % Helper function to plot vertical lines for the waypoint times 83 | function plotTimeLines(t) 84 | for idx = 1:numel(t) 85 | xline(t(idx),'r--'); 86 | end 87 | end 88 | 89 | -------------------------------------------------------------------------------- /matlab/manipTrajTransformTimeScaling.m: -------------------------------------------------------------------------------- 1 | % MANIPULATOR TRAJECTORY GENERATION 2 | % Generates combined transform (rotation and translation) trajectories 3 | % using custom time scaling from a separate trajectory. 4 | % 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | %% Setup 8 | clear, clc, close all 9 | 10 | % Define waypoint information 11 | createWaypointData; 12 | 13 | % Define IK 14 | ik = inverseKinematics('RigidBodyTree',gen3); 15 | ikWeights = [1 1 1 1 1 1]; 16 | ikInitGuess = gen3.homeConfiguration; 17 | 18 | % Set up plot 19 | plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames 20 | show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false); 21 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 22 | hold on 23 | if plotMode == 1 24 | hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-'); 25 | end 26 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 27 | 28 | %% Generate and follow trajectory 29 | % Loop through segments one at a time 30 | trajType = 'trap'; 31 | numWaypoints = size(waypoints,2); 32 | for w = 1:numWaypoints-1 33 | 34 | % Get the initial and final transforms and times for the segment 35 | T0 = trvec2tform(waypoints(:,w)') * eul2tform(orientations(:,w)'); 36 | Tf = trvec2tform(waypoints(:,w+1)') * eul2tform(orientations(:,w+1)'); 37 | timeInterval = waypointTimes(w:w+1); 38 | trajTimes = timeInterval(1):ts:timeInterval(2); 39 | 40 | % Generate time scaling trajectory for the segment on the range [0 1] 41 | switch trajType 42 | case 'trap' 43 | [s,sd,sdd] = trapveltraj([0 1],numel(trajTimes), ... 44 | 'EndTime',diff(timeInterval)); 45 | case 'cubic' 46 | [s,sd,sdd] = cubicpolytraj([0 1],timeInterval,trajTimes); 47 | case 'quintic' 48 | [s,sd,sdd] = quinticpolytraj([0 1],timeInterval,trajTimes); 49 | otherwise 50 | error('Invalid trajectory type! Use ''trap'', ''cubic'', or ''quintic'''); 51 | end 52 | 53 | % Find the transforms from trajectory generation 54 | [T,vel,acc] = transformtraj(T0,Tf,timeInterval,trajTimes, ... 55 | 'TimeScaling',[s;sd;sdd]); 56 | 57 | % Trajectory visualization for the segment 58 | if plotMode == 1 59 | eePos = tform2trvec(T); 60 | set(hTraj,'xdata',eePos(:,1),'ydata',eePos(:,2),'zdata',eePos(:,3)); 61 | elseif plotMode == 2 62 | plotTransforms(tform2trvec(T),tform2quat(T),'FrameSize',0.05) 63 | end 64 | 65 | % Trajectory following for the segment 66 | for idx = 1:numel(trajTimes) 67 | % Solve IK 68 | tgtPose = T(:,:,idx); 69 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 70 | ikInitGuess = config; 71 | 72 | % Show the robot 73 | show(gen3,config,'Frames','off','PreservePlot',false); 74 | title(['Trajectory at t = ' num2str(trajTimes(idx))]) 75 | drawnow 76 | end 77 | 78 | end -------------------------------------------------------------------------------- /matlab/manipTrajLinearRotation.m: -------------------------------------------------------------------------------- 1 | % MANIPULATOR TRAJECTORY GENERATION 2 | % Generates Cartesian trajectories with independent 3 | % linearly interpolated rotation trajectories. 4 | % 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | %% Setup 8 | clear, clc, close all 9 | 10 | % Define waypoint information 11 | createWaypointData; 12 | 13 | % Define IK 14 | ik = inverseKinematics('RigidBodyTree',gen3); 15 | ikWeights = [1 1 1 1 1 1]; 16 | ikInitGuess = gen3.homeConfiguration; 17 | 18 | % Set up plot 19 | plotMode = 2; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames 20 | show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false); 21 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 22 | hold on 23 | if plotMode == 1 24 | hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-'); 25 | end 26 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 27 | 28 | %% Generate and follow trajectory 29 | % Loop through segments one at a time 30 | trajType = 'trap'; 31 | numWaypoints = size(waypoints,2); 32 | for w = 1:numWaypoints-1 33 | % Get the initial and final rotations and times for the segment 34 | R0 = eul2quat(orientations(:,w)'); 35 | Rf = eul2quat(orientations(:,w+1)'); 36 | timeInterval = waypointTimes(w:w+1); 37 | trajTimes = timeInterval(1):ts:timeInterval(2); 38 | 39 | % Cartesian Motion only 40 | switch trajType 41 | case 'trap' 42 | [q,qd,qdd] = trapveltraj(waypoints(:,w:w+1),numel(trajTimes), ... 43 | 'AccelTime',waypointAccelTimes(w), ... 44 | 'EndTime',diff(waypointTimes(w:w+1))); 45 | 46 | case 'cubic' 47 | [q,qd,qdd] = cubicpolytraj(waypoints(:,w:w+1),waypointTimes(w:w+1),trajTimes, ... 48 | 'VelocityBoundaryCondition',waypointVels(:,w:w+1)); 49 | 50 | case 'quintic' 51 | [q,qd,qdd] = quinticpolytraj(waypoints(:,w:w+1),waypointTimes(w:w+1),trajTimes, ... 52 | 'VelocityBoundaryCondition',waypointVels(:,w:w+1), ... 53 | 'AccelerationBoundaryCondition',waypointAccels(:,w:w+1)); 54 | 55 | case 'bspline' 56 | ctrlpoints = waypoints(:,idx:idx+1); % Can adapt this as needed 57 | [q,qd,qdd] = bsplinepolytraj(ctrlpoints,timeInterval,trajTimes); 58 | 59 | otherwise 60 | error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline'''); 61 | end 62 | 63 | % Find the quaternions from trajectory generation 64 | [R, omega, alpha] = rottraj(R0, Rf, timeInterval, trajTimes); 65 | 66 | % Plot trajectory 67 | if plotMode == 1 68 | set(hTraj,'xdata',q(1,:),'ydata',q(2,:),'zdata',q(3,:)); 69 | elseif plotMode == 2 70 | plotTransforms(q',R','FrameSize',0.05) 71 | end 72 | 73 | % Trajectory following loop 74 | for idx = 1:numel(trajTimes) 75 | % Solve IK 76 | tgtPose = trvec2tform(q(:,idx)') * quat2tform(R(:,idx)'); 77 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 78 | ikInitGuess = config; 79 | 80 | % Show the robot 81 | show(gen3,config,'Frames','off','PreservePlot',false); 82 | title(['Trajectory at t = ' num2str(trajTimes(idx))]) 83 | drawnow 84 | end 85 | 86 | 87 | end -------------------------------------------------------------------------------- /matlab/manipTrajJoint.m: -------------------------------------------------------------------------------- 1 | % MANIPULATOR TRAJECTORY GENERATION 2 | % Generates Joint space trajectories by performing inverse kinematics on 3 | % each waypoint and interpolating between the joint angles. 4 | % 5 | % Copyright 2019 The MathWorks, Inc. 6 | 7 | %% Setup 8 | clear, clc, close all 9 | 10 | % Define waypoint information 11 | createWaypointData; 12 | 13 | % Define IK 14 | ik = inverseKinematics('RigidBodyTree',gen3); 15 | ikWeights = [1 1 1 1 1 1]; 16 | ikInitGuess = jointAnglesHome'; 17 | ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi; 18 | ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi; 19 | 20 | % Set up plot 21 | plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames 22 | show(gen3,gen3.homeConfiguration,'Frames','off','PreservePlot',false); 23 | xlim([-1 1]), ylim([-1 1]), zlim([0 1.2]) 24 | hold on 25 | if plotMode == 1 26 | hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-'); 27 | end 28 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2); 29 | 30 | %% Solve IK for all waypoints 31 | includeOrientation = false; % Set this to use zero vs. nonzero orientations 32 | 33 | numWaypoints = size(waypoints,2); 34 | numJoints = numel(gen3.homeConfiguration); 35 | jointWaypoints = zeros(numJoints,numWaypoints); 36 | 37 | for idx = 1:numWaypoints 38 | if includeOrientation 39 | tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)'); 40 | else 41 | tgtPose = trvec2tform(waypoints(:,idx)'); 42 | end 43 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 44 | jointWaypoints(:,idx) = config'; 45 | end 46 | 47 | %% Generate trajectory on joint space 48 | trajType = 'trap'; 49 | switch trajType 50 | case 'trap' 51 | [q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ... 52 | 'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ... 53 | 'EndTime',repmat(diff(waypointTimes),[numJoints 1])); 54 | 55 | case 'cubic' 56 | [q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 57 | 'VelocityBoundaryCondition',zeros(numJoints,numWaypoints)); 58 | 59 | case 'quintic' 60 | [q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 61 | 'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ... 62 | 'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints)); 63 | 64 | case 'bspline' 65 | ctrlpoints = jointWaypoints; % Can adapt this as needed 66 | [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes); 67 | 68 | otherwise 69 | error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline'''); 70 | end 71 | 72 | % To visualize the trajectory, run the following line 73 | % plotTrajectory(trajTimes,q,qd,qdd,'Names',"Joint " + string(1:numJoints),'WaypointTimes',waypointTimes) 74 | 75 | %% Trajectory following loop 76 | for idx = 1:numel(trajTimes) 77 | 78 | config = q(:,idx)'; 79 | 80 | % Find Cartesian points for visualization 81 | eeTform = getTransform(gen3,config,eeName); 82 | if plotMode == 1 83 | eePos = tform2trvec(eeTform); 84 | set(hTraj,'xdata',[hTraj.XData eePos(1)], ... 85 | 'ydata',[hTraj.YData eePos(2)], ... 86 | 'zdata',[hTraj.ZData eePos(3)]); 87 | elseif plotMode == 2 88 | plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05); 89 | end 90 | 91 | % Show the robot 92 | show(gen3,config,'Frames','off','PreservePlot',false); 93 | title(['Trajectory at t = ' num2str(trajTimes(idx))]) 94 | drawnow 95 | 96 | end -------------------------------------------------------------------------------- /matlab/compareTaskVsJointTraj.m: -------------------------------------------------------------------------------- 1 | % Compares joint space vs. task space trajectories 2 | % Copyright 2019 The MathWorks, Inc. 3 | 4 | %% Setup 5 | clc 6 | createWaypointData; 7 | figure, hold on 8 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko:','LineWidth',2); 9 | title('Trajectory Waypoints'); 10 | xlabel('X [m]'); 11 | ylabel('Y [m]'); 12 | zlabel('Z [m]'); 13 | grid on 14 | view([45 45]); 15 | % Define IK solver 16 | ik = inverseKinematics('RigidBodyTree',gen3); 17 | ikWeights = [1 1 1 1 1 1]; 18 | % Use a small sample time for this example, so the difference between joint 19 | % and task space is clear due to evaluation of IK in task space trajectory. 20 | ts = 0.02; 21 | trajTimes = 0:ts:waypointTimes(end); 22 | % Initialize matrices for plots 23 | qTask = zeros(numJoints,numel(trajTimes)); % Derived joint angles in task space trajectory 24 | posJoint = zeros(3,numel(trajTimes)); % Derived end effector positions in joint space trajectory 25 | 26 | %% Create and evaluate a task space trajectory 27 | ikInitGuess = jointAnglesHome'; 28 | ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi; 29 | ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi; 30 | 31 | disp('Running task space trajectory generation and evaluation...') 32 | tic; 33 | 34 | % Trajectory generation 35 | [posTask,velTask,accelTask] = trapveltraj(waypoints,numel(trajTimes), ... 36 | 'AccelTime',repmat(waypointAccelTimes,[3 1]), ... 37 | 'EndTime',repmat(diff(waypointTimes),[3 1])); 38 | 39 | % Trajectory evaluation 40 | for idx = 1:numel(trajTimes) 41 | % Solve IK 42 | tgtPose = trvec2tform(posTask(:,idx)'); 43 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 44 | ikInitGuess = config; 45 | qTask(:,idx) = config; 46 | end 47 | 48 | taskTime = toc; 49 | disp(['Task space trajectory time : ' num2str(taskTime) ' s']); 50 | 51 | %% Create and evaluate a joint space trajectory 52 | ikInitGuess = jointAnglesHome'; 53 | ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi; 54 | ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi; 55 | 56 | disp('Running joint space trajectory generation and evaluation...') 57 | tic; 58 | 59 | % Solve IK for all waypoints 60 | numWaypoints = size(waypoints,2); 61 | numJoints = numel(gen3.homeConfiguration); 62 | jointWaypoints = zeros(numJoints,numWaypoints); 63 | for idx = 1:numWaypoints 64 | tgtPose = trvec2tform(waypoints(:,idx)'); 65 | [config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess); 66 | cfgDiff = config - ikInitGuess; 67 | jointWaypoints(:,idx) = config'; 68 | end 69 | 70 | % Trajectory Generation 71 | [qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ... 72 | 'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ... 73 | 'EndTime',repmat(diff(waypointTimes),[numJoints 1])); 74 | 75 | % Trajectory evaluation (only needed to find end effector position) 76 | for idx = 1:numel(trajTimes) 77 | eeTform = getTransform(gen3,qJoint(:,idx)',eeName); 78 | posJoint(:,idx) = tform2trvec(eeTform)'; 79 | end 80 | jointTime = toc; 81 | disp(['Joint space trajectory time : ' num2str(jointTime) ' s']); 82 | 83 | %% Create comparison plots 84 | % Compare trajectories in Cartesian space 85 | close all 86 | figure, hold on 87 | plot3(posTask(1,:),posTask(2,:),posTask(3,:),'b-'); 88 | plot3(posJoint(1,:),posJoint(2,:),posJoint(3,:),'r--'); 89 | plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko','LineWidth',2); 90 | title('Trajectory Comparison'); 91 | xlabel('X [m]'); 92 | ylabel('Y [m]'); 93 | zlabel('Z [m]'); 94 | legend('Task Space Trajectory','Joint Space Trajectory','Waypoints'); 95 | grid on 96 | view([45 45]); 97 | 98 | % Compare joint angles 99 | % Plot each joint trajectory 100 | for idx = 1:numJoints 101 | figure, hold on; 102 | plot(trajTimes,qTask(idx,:),'b-'); 103 | plot(trajTimes,qJoint(idx,:),'r-'); 104 | for wIdx = 1:numWaypoints 105 | xline(waypointTimes(wIdx),'k--'); 106 | end 107 | title(['Joint ' num2str(idx) ' Trajectory']); 108 | xlabel('Time [s]'); 109 | ylabel('Joint Angle [rad]'); 110 | legend('Task Space Trajectory','Joint Space Trajectory'); 111 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Trajectory Planning of Robot Manipulators with MATLAB and Simulink 2 | Copyright 2019 The MathWorks, Inc. 3 | 4 | ## Description 5 | 6 | This submission consists of educational MATLAB and Simulink examples for 7 | trajectory generation and evaluation of robot manipulators. 8 | 9 | All examples feature the 7-DOF Kinova Gen3 Ultra lightweight robotic manipulator: 10 | https://www.kinovarobotics.com/en/products/robotic-arms/gen3-ultra-lightweight-robot 11 | 12 | There is a presaved MATLAB rigid body tree model of the Kinova Gen3; however, you can 13 | access the 3D model description from the Kinova Kortex GitHub repository: 14 | https://github.com/Kinovarobotics/ros_kortex 15 | 16 | For more information on the Robotics System Toolbox functionality for manipulators, 17 | see the documentation: https://www.mathworks.com/help/ros/robotic-manipulators-applications.html 18 | 19 | For more background information on trajectory planning, refer to this presentation: 20 | https://cw.fel.cvut.cz/old/_media/courses/a3m33iro/080manipulatortrajectoryplanning.pdf 21 | 22 | If you have any questions, email us at roboticsarena@mathworks.com. 23 | 24 | ## Files 25 | To get started, run the `startupExample.m` script. This will configure the MATLAB search path so all the examples run correctly. 26 | 27 | ### `matlab` Folder 28 | Contains MATLAB examples for trajectory planning. 29 | 30 | * `manipTrajCartesian.m` - Task space (translation only) trajectories 31 | * `manipTrajJoint.m` - Joint space trajectories. Contains an `includeOrientation` variable to toggle waypoint orientations on or off. 32 | * `manipTrajLinearRotation.m` - Task space (translation only) trajectories with linearly interpolated orientation 33 | * `manipTrajTransform.m` - Linearly interpolated transform trajectories (translation and orientation) 34 | * `manipTrajTransformTimeScaling.m` - Transform trajectories (translation and orientation) interpolated using nonlinear time scaling 35 | * `compareTaskVsJointTraj.m` - Comparison script that illustrates the difference between task space and joint space trajectories 36 | 37 | **NOTE:** All the scripts above are configurable: 38 | * `createWaypointData.m` script - Generates sample waypoints, trajectory times, and other necessary planning variables. 39 | * `trajType` variable - Used to switch the trajectory type 40 | * `plotMode` variable - Used to switch the waypoint/trajectory visualization type 41 | 42 | ### `simulink` Folder 43 | Contains Simulink examples for trajectory planning. 44 | 45 | * `manipCartesianTrajectory.slx` - Task space (translation only) trajectories 46 | * `manipJointTrajectory.slx` - Joint space trajectories. 47 | * `manipRotationTrajectory.slx` - Task space (translation only) trajectories with linearly interpolated orientation 48 | * `manipTransformTrajectory.slx` - Linearly interpolated transform trajectories (translation and orientation) 49 | * `manipTransformTrajectoryTimeScaling.slx` - Transform trajectories (translation and orientation) interpolated using nonlinear time scaling 50 | 51 | **NOTE:** There are also models that work with Robot Operating System (ROS), which are identically named with the `ros` prefix. 52 | Instead of using variables in the MATLAB base workspace, waypoint information is communicated using ROS messages. 53 | To test this, you can use the Waypoint Publisher App or the `publishWaypoints` script (see the next section). 54 | 55 | The ROS topics and message types are: 56 | * `/waypoints` - List of waypoints, message type `geometry_msgs/PoseArray` 57 | * `/waypoint_times` - List of waypoint target times, message type `std_msgs/Float64MultiArray` 58 | 59 | ### `utilities` Folder 60 | Contains several utilities for the MATLAB and Simulink examples above. 61 | 62 | * `createWaypointData.m` - Creates sample waypoints, waypoint times, and other necessary planning variables. If you want to change the waypoints or other trajectory reference values, modify this script (or we suggest creating a copy) 63 | * `cylinder.stl` - "Dummy" mesh file representing the end effector attached the arm 64 | * `gen3.mat` - Presaved rigid body tree containing the 3D model of the robot arm 65 | * `gen3positions.mat` - Presaved joint and end effector configurations for the "home" and "retract" positions of the robot arm 66 | * `importGen3Model.m` - Function to import the Kinova Gen3 manipulator model. Not needed by this example; you can use this if you want to import a new model yourself from the source URDF file. 67 | * `plotTrajectory.m` - Utility function to plot generated trajectory profiles (used with MATLAB examples) 68 | * `publishWaypoints.m` - Tests the publishing of waypoint information as ROS messages 69 | * `trajExampleUtils.slx` - Block library containing common components for the Simulink examples 70 | * `visualizeRobot.m` - Utility function used by the library above to plot the manipulator from a Simulink model 71 | * `waypointPublisher.mlapp` - MATLAB app used to modify waypoints and publish them to the base workspace or as ROS messages 72 | --------------------------------------------------------------------------------