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
qTask = zeros(numJoints,numel(trajTimes)); % Derived joint angles in task space trajectory
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
[posTask,velTask,accelTask] = trapveltraj(waypoints,numel(trajTimes), ...
'AccelTime',repmat(waypointAccelTimes,[3 1]), ...
'EndTime',repmat(diff(waypointTimes),[3 1]));
% Trajectory evaluation
for idx = 1:numel(trajTimes)
% Solve IK
tgtPose = trvec2tform(posTask(:,idx)');
[config,info] = ik(eeName,tgtPose,ikWeights,ikInitGuess);
ikInitGuess = config;
qTask(:,idx) = config;
end
taskTime = toc;
disp(['Task space trajectory time : ' num2str(taskTime) ' s']);
%% 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(posTask(1,:),posTask(2,:),posTask(3,:),'b-');
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]');
legend('Task Space Trajectory','Joint Space Trajectory','Waypoints');
grid on
view([45 45]);
CreateWaypointData:
% Create sample waypoint data for trajectory generation
%% Load robot Panda
load panda_ML
%% Common parameters
% Rigid Body Tree information
load Panda_MLpositions
eeName = 'panda_link8';
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);
%% Additional parameters
% 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;
```