0
$\begingroup$

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.

enter image description here

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;
```
$\endgroup$
2
  • 2
    $\begingroup$ 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? $\endgroup$
    – Albert H M
    Sep 15 at 4:53
  • $\begingroup$ 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. $\endgroup$
    – user463102
    Sep 16 at 9:58

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.