Trajectory generation using inverse kinematics

I have made a rigid body tree model of the UR10e using the following DH-parameters:

dhparams = [0         pi/2    0.1807  0;
            -0.6127   0       0       0;
            -0.57155  0       0       0;
            0        pi/2    0.17415  0;
            0        -pi/2   0.11985  0;
            0         0       0.11655  0];
robot = rigidBodyTree;

I then build the robot succesfully as explained in the rigidBodyTree documentation and am able to get inverse Kinematics solutions using

[configSoln, solInfo]

However, as I try to generate a trajectory for the end-effector or 'body6' to follow points in a coordinate frame according to the this example the robot is not displayed and the end-effector does not follow a trajectory along the selected waypoints. I made some changes to the example code of course in order to fit my robot.

body1 = rigidBody('body1');
body2 = rigidBody('body2');
body3 = rigidBody('body3');
body4 = rigidBody('body4');
body5 = rigidBody('body5');
body6 = rigidBody('body6');

joint1 = rigidBodyJoint('joint1', 'revolute');
joint2 = rigidBodyJoint('joint2', 'revolute');
joint3 = rigidBodyJoint('joint3', 'revolute');
joint4 = rigidBodyJoint('joint4', 'revolute');
joint5 = rigidBodyJoint('joint5', 'revolute');
joint6 = rigidBodyJoint('joint6', 'revolute');

setFixedTransform(joint1, dhparams(1,:), "dh");
setFixedTransform(joint2, dhparams(2,:), "dh");
setFixedTransform(joint3, dhparams(3,:), "dh");
setFixedTransform(joint4, dhparams(4,:), "dh");
setFixedTransform(joint5, dhparams(5,:), "dh");
setFixedTransform(joint6, dhparams(6,:), "dh");

body1.Joint = joint1;
body2.Joint = joint2;
body3.Joint = joint3;
body4.Joint = joint4;
body5.Joint = joint5;
body6.Joint = joint6;

addBody(robot, body1, 'base')
addBody(robot, body2, 'body1')
addBody(robot, body3, 'body2')
addBody(robot, body4, 'body3')
addBody(robot, body5, 'body4')
addBody(robot, body6, 'body5')

%show(robot);
%axis([-0.5,0.5,-0.5,0.5,-0.5,0.5])
%axis off

randConfig = robot.randomConfiguration;
tform = getTransform(robot, randConfig, 'body6', 'base');

show(robot, randConfig);

%create iK solver for robot
ik = inverseKinematics('RigidBodyTree', robot);
weights = [0.25 0.25 0.25 1 1 1];
initialguess = robot.homeConfiguration;

%find joint configurations for specified end-effector pose incl. weights
%i.e. tolerances
[configSoln, solInfo] = ik('body6', tform, weights, initialguess);

%show(robot, configSoln);

toolPositionHome = [0.455    0.001    0.434];
waypoints = toolPositionHome' + ... 
            [0 0 0 ; -0.1 0.2 0.2 ; -0.2 0 0.1 ; -0.1 -0.2 0.2 ; 0 0 0]';

orientations = [pi/2    0   pi/2;
               (pi/2)+(pi/4)      0   pi/2; 
                pi/2    0   pi;
               (pi/2)-(pi/4)       0    pi/2;
                pi/2    0   pi/2]';

waypointTimes = 0:7:28;
ts = 0.25;
trajTimes = 0:ts:waypointTimes(end);

waypointVels = 0.1 *[ 0  1  0;
                     -1  0  0;
                      0 -1  0;
                      1  0  0;
                      0	 1  0]';

waypointAccels = zeros(size(waypointVels));
waypointAccelTimes = diff(waypointTimes)/4;

ikWeights = [1 1 1 1 1 1];
ikInitGuess = initialguess';

plotMode = 1; % 0 = None, 1 = Trajectory, 2 = Coordinate Frames
show(robot, initialguess','Frames','off','PreservePlot',false);
hold on
if plotMode == 1
    hTraj = plot3(waypoints(1,1),waypoints(2,1),waypoints(3,1),'b.-');
end
plot3(waypoints(1,:),waypoints(2,:),waypoints(3,:),'ro','LineWidth',2);
axis auto;
view([30 15]);

includeOrientation = true; 
numWaypoints = size(waypoints,2);
numJoints = numel(robot.homeConfiguration);
jointWaypoints = zeros(numJoints,numWaypoints);
for idx = 1:numWaypoints
    if includeOrientation
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform(orientations(:,idx)');
    else
        tgtPose = trvec2tform(waypoints(:,idx)') * eul2tform([pi/2 0 pi/2]); %#ok<UNRCH> 
    end
    [config, info] = ik('body6', tgtPose, ikWeights, ikInitGuess);
    jointWaypoints(:,idx) = config.JointPosition;
end

trajType = 'trap';
switch trajType
    case 'trap'
        [q,qd,qdd] = trapveltraj(jointWaypoints,numel(trajTimes), ...
            'AccelTime',repmat(waypointAccelTimes,[numJoints 1]), ... 
            'EndTime',repmat(diff(waypointTimes),[numJoints 1]));
                            
    case 'cubic'
        [q,qd,qdd] = cubicpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints));
        
    case 'quintic'
        [q,qd,qdd] = quinticpolytraj(jointWaypoints,waypointTimes,trajTimes, ... 
            'VelocityBoundaryCondition',zeros(numJoints,numWaypoints), ...
            'AccelerationBoundaryCondition',zeros(numJoints,numWaypoints));
        
    case 'bspline'
        ctrlpoints = jointWaypoints; % Can adapt this as needed
        [q,qd,qdd] = bsplinepolytraj(ctrlpoints,waypointTimes([1 end]),trajTimes);
        
    otherwise
        error('Invalid trajectory type! Use ''trap'', ''cubic'', ''quintic'', or ''bspline''');
end



for idx = 1:numel(trajTimes)  
 
    for joint_num=1:1:size(config, 2)
        config(joint_num).JointPosition = q(joint_num, idx)';
    end
    
    % Find Cartesian points for visualization
    eeTform = getTransform(robot,config,'body6');
    if plotMode == 1
        eePos = tform2trvec(eeTform);
        set(hTraj,'xdata',[hTraj.XData eePos(1)], ...
                  'ydata',[hTraj.YData eePos(2)], ...
                  'zdata',[hTraj.ZData eePos(3)]);
    elseif plotMode == 2
        plotTransforms(tform2trvec(eeTform),tform2quat(eeTform),'FrameSize',0.05);
    end
 
    % Show the robot
    show(robot,config','Frames','off','PreservePlot',false);
    title(['Trajectory at t = ' num2str(trajTimes(idx))])
    drawnow   
    
end

The output looks quite strange as follows:

I appreciate any and all help!

Answer 1

[Karsh Tharyani]

Hi,

You can't see the robot because the lines which should show the robot have been commented out. Additionally, if you wish to visualize the robot and the cartesian frame origins with each joint configuration, you should have to call "hold on".

Please read about axis holding here: https://www.mathworks.com/help/matlab/ref/hold.html

Hope that helps,

Karsh

[Johann]

Thank you very much for your help, do you also know why the trajectory does not follow the waypoints selected?

[Karsh Tharyani]

Hi Johann,

I would suggest you reach out to our Technical Support team with the reproducible of the issue. That will enable us to diagnose this in an efficient manner.

Best,

Karsh

Answer 2

[Kosei Noda]

The problem here is the calculation of jointWaypoints.

I, and you may expect that 'jointWaypoints' has 5 sets of joint angles for each waypoint,

however, all of its rows consists of same values.

The reason why same values are recorded is operation of struct object.

If you access to struct array like >> var = struct.member, it returns only one value.

One idea to avoid the issue is obtaining joint angles like below.

   jointWaypoints(:,idx) = arrayfun(@(x) x.JointPosition, config)';

Hope this helps!

P.S.

If you encounter unexpected behavior, debug mode is very helpful tool to solve the issue by yourself.

https://jp.mathworks.com/help/matlab/matlab_prog/debugging-process-and-features.html