# Joint space trajectory script unclear

I have a script provided by The MathWorks Inc. that makes a robot end-effector follow both a task space and a joint space trajectory through several waypoints. These trajectories are then plotted in the same figure (shown below). For some reason the joint space trajectory makes these huge loops. Can anybody explain this behaviour? I do know that the difference between joint space and task space is that in joint space, you know the start and last position of the joint configuration from inverse kinematics, and interpolate it. While in task space you know start and last position of end-effector, then interpolate it and the inverse kinematics in every point in the path to get the joint configuration for every point in the path.

Shown below is the part of the script that creates the joint space trajectory. I hope this can help somebody explain the weird curvy behaviour.

ikInitGuess = jointAnglesHome';\
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;\
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

% Solve Inverse Kinematics for all waypoints\
numWaypoints = size(waypoints,2);\
numJoints = numel(Robot.homeConfiguration);\
jointWaypoints = zeros(numJoints,numWaypoints);\
for idx = 1:numWaypoints\
tgtPose = trvec2tform(waypoints(:,idx)');\
[config,info] = ik(end-Effector-Name,tgtPose,ikWeights,ikInitGuess);\
cfgDiff = config - ikInitGuess;\
jointWaypoints(:,idx) = config';\
end

% Trajectory Generation\
[qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ...\
'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

%Trajectory evaluation (only needed to find end effector position)\
for idx = 1:numel(trajTimes)
eeTform = getTransform(gen3,qJoint(:,idx)',eeName);\
posJoint(:,idx) = tform2trvec(eeTform)';\
end


Thanks in advance for any help.

The complete script, This script also requires the CreateWaypointData script, which is shown in a different segment at the end.

% Compares joint space vs. task space trajectories
% Copyright 2019 The MathWorks, Inc.

%% Setup
clear; close; clc;
createWaypointData;
figure, hold on
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko:','LineWidth',2);
title('Trajectory Waypoints');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
grid on
view([45 45]);

% Define IK solver
ik = inverseKinematics('RigidBodyTree',Panda_ML);
ikWeights = [1 1 1 1 1 1];
% Use a small sample time for this example, so the difference between joint
% and task space is clear due to evaluation of IK in task space trajectory.
ts = 0.02;
trajTimes = 0:ts:waypointTimes(end);
% Initialize matrices for plots
posJoint = zeros(3,numel(trajTimes)); % Derived end effector positions in joint space trajectory

%% Create and evaluate a task space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running task space trajectory generation and evaluation...')
tic;

% Trajectory generation
'AccelTime',repmat(waypointAccelTimes,[3 1]), ...
'EndTime',repmat(diff(waypointTimes),[3 1]));

% Trajectory evaluation
for idx = 1:numel(trajTimes)
% Solve IK
[config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
ikInitGuess = config;
end

%% Create and evaluate a joint space trajectory
ikInitGuess = jointAnglesHome';
ikInitGuess(ikInitGuess > pi) = ikInitGuess(ikInitGuess > pi) - 2*pi;
ikInitGuess(ikInitGuess < -pi) = ikInitGuess(ikInitGuess < -pi) + 2*pi;

disp('Running joint space trajectory generation and evaluation...')
tic;

% Solve IK for all waypoints
numWaypoints = size(waypoints,2);
numJoints = numel(Panda_ML.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
tgtPose = trvec2tform(waypoints(:,idx)');
[config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
cfgDiff = config - ikInitGuess;
jointWaypoints(:,idx) = config';
end

% Trajectory Generation
[qJoint,qdJoint,qddJoint] = trapveltraj(jointWaypoints,numel(trajTimes), ...
'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ...
'EndTime',repmat(diff(waypointTimes),[numJoints 1]));

% Trajectory evaluation (only needed to find end effector position)
for idx = 1:numel(trajTimes)
eeTform = getTransform(Panda_ML,qJoint(:,idx)',eeName);
posJoint(:,idx) = tform2trvec(eeTform)';
end
jointTime = toc;
disp(['Joint space trajectory time : ' num2str(jointTime) ' s']);

%% Create comparison plots
% Compare trajectories in Cartesian space
close all
figure, hold on
plot3(posJoint(1,:),posJoint(2,:),posJoint(3,:),'r--');
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ko','LineWidth',2);
title('Trajectory Comparison');
xlabel('X [m]');
ylabel('Y [m]');
zlabel('Z [m]');
grid on
view([45 45]);


CreateWaypointData:

% Create sample waypoint data for trajectory generation

%% Common parameters
% Rigid Body Tree information
numJoints = numel(Panda_ML.homeConfiguration);
ikInitGuess = zeros(1,(numel(Panda_ML.homeConfiguration)));

% Maximum number of waypoints (for Simulink)
maxWaypoints = 20;

% Positions (X Y Z)
waypoints = toolPositionHome' + [-.1 -.2 -.3; -.3 -.5 -.2; -.6 -.5 0; -.9 -.4 .1; -1.1 -.6 .2]';

% Euler Angles (Z Y X) relative to the home orientation
orientations = [0 0 0; pi/8 0 0; 0 pi/2 0; -pi/8 0 0; 0 0 0]';

% Array of waypoint times
% Increase the speed of the robot
waypointTimes = 0:4:16;
% waypointTimes = [1 2 3 4 5 6 7 8 9 10];

% Trajectory sample time
ts = 0.2;
trajTimes = 0:ts:waypointTimes(end);

% Boundary conditions (for polynomial trajectories)
% All are chosen arbitrarily.
% Velocity (cubic and quintic)
waypointVels = 0.1 *[0 1 0; -1 0 0; 0 -1 0; 1 0 0; 0 1 0]';

% Acceleration (quintic only)
waypointAccels = zeros(size(waypointVels));

% Acceleration times (trapezoidal only)
waypointAccelTimes = diff(waypointTimes)/4;
$$$$
`
• My initial guess would be the method of interpolation, it seems you used quadratic interpolation (but have too high acceleration) that result on curvy motion between each point. Can you give me full code or link and library you use, so I could investigate more? Sep 15, 2021 at 4:53
• Thanks very much for your help. I will edit the question and add the full script. The Panda_ML is a rigid body tree robot model. But this behaviour is the same regardless of the used robot rigid body tree. Sep 16, 2021 at 9:58