Track Pre-Computed Trajectory of Kinova Gen3 Robot End-Effector Using Inverse Kinematics and KINOVA KORTEX MATLAB API

Required Products

Create the Robot Model

Create a Rigid Body Tree for Gen3 robot, set the home configuration and calculate the transformation at the home configuration:

gen3 = loadrobot("kinovaGen3");
gen3.DataFormat = 'column';
q_home = [0 15 180 -130 0 55 90]'*pi/180;
eeName = 'EndEffector_Link';
T_home = getTransform(gen3, q_home, eeName);

Visualize the robot at home configuration.

show(gen3,q_home);
axis auto;
view([60,10]); 

Create Inverse Kinematics Solver and Set Parameters

The inverseKinematics System object™ creates an inverse kinematic (IK) solver to calculate joint configurations for a desired end-effector pose based on a specified rigid body tree model.

ik = inverseKinematics('RigidBodyTree', gen3);

Disable random restarts for inverse kinematic solver. AllowRandomRestart parameter indicates if random restarts are allowed. Random restarts are triggered when the algorithm approaches a solution that does not satisfy the constraints. A randomly generated initial guess is used.

ik.SolverParameters.AllowRandomRestart = false;

Specify weight priorities for pose tolerances, as a six-element vector. The first three elements correspond to the weights on the error in orientation for the desired pose. The last three elements correspond to the weights on the error in xyz position for the desired pose.

weights = [1, 1, 1, 1, 1, 1];

Since the desired trajectory is selected to start near the home position, define initial guess for solver as the home position of the robot.

q_init = q_home;

Define Waypoints from the Desired Trajectory

This section helps you is to create a circular trajectory for the end effector to track. Keep the orientation of the end effector as constant for the whole range of motion. Define center and radius of the circular trajectory.

center = [0.5 0 0.4]; %[x y z]
radius = 0.1;

Define total time and time step, and based on that generate waypoints for the circular trajectory.

dt = 0.25;
t = (0:dt:10)';
theta = t*(2*pi/t(end))-(pi/2);
points = center + radius*[0*ones(size(theta)) cos(theta) sin(theta)];

Plot the waypoints along with the robot at home configuration.

hold on;
plot3(points(:,1),points(:,2),points(:,3),'-*g', 'LineWidth', 1.5);
xlabel('x');
ylabel('y');
zlabel('z');
axis auto;
view([60,10]);
grid('minor');

Solve the Inverse Kinematics for Each Waypoint

The inverse kinematics solver finds a joint configuration that achieves the specified end-effector pose. Specify an initial guess for the configuration and the desired weights on the tolerances for the six components of pose. The inverse kinematics solver returns information about its convergence and it is recommended to check the status of the solution.

This section helps you to find a joint configuration for a fixed end effector orientation and variable position defined by waypoints calculated in the previous sections. The current solution is used as the initial guess for the next waypoint to ensure smooth and continuous trajectory. Uncomment the display command to see the status of each inverse kinematics iteration.

numJoints = size(q_home,1);   
numWaypoints = size(points,1);
qs = zeros(numWaypoints,numJoints);
for i = 1:numWaypoints
    T_des = T_home;
    T_des(1:3,4) = points(i,:)';
    [q_sol, q_info] = ik(eeName, T_des, weights, q_init);
    
    % Display status of ik result
    %disp(q_info.Status);
    
    % Store the configuration
    qs(i,:) = q_sol(1:numJoints); 
    
    % Start from prior solution
    q_init = q_sol;
end

Visualize the Animation of the Solution

figure; set(gcf,'Visible','on');
ax = show(gen3,qs(1,:)');
ax.CameraPositionMode='auto';
hold on;
 
% Plot waypoints
plot3(points(:,1),points(:,2),points(:,3),'-g','LineWidth',2);
axis auto;
view([60,10]);
grid('minor');
hold on;
 
title('Simulated Movement of the Robot');
% Animate
framesPerSecond = 30;
r = robotics.Rate(framesPerSecond);
for i = 1:numWaypoints
    show(gen3, qs(i,:)','PreservePlot',false);
    drawnow;
    waitfor(r);
end

Send the Trajectory to the Hardware

Expected Motion of the robot (Assuming you start from the retract position)

Press ‘y’ and press 'Enter' key on the keyboard if you want to send commands to the Kinova Gen3 robot to track the calculated trajectory or press 'Enter' to finish the example.

prompt = 'Do you want to send same trajectory to the hardware? y/n [n]: ';
str = input(prompt,'s');

if isempty(str)
    str = 'n';
end
 
if str == 'n'
    clear;
    return;
end

Command Kinova Gen3 Robot to Track the Pre-Computed Trajectory

As explained in the example Connect to Kinova Gen3 Robot and Manipulate the Arm Using MATLAB, the MATLAB API for Kinova Gen3 robot supports multiple modes to command the robot. The high-level commands to reach a desired joint configuration or Cartesian pose cannot be used to follow a smooth trajectory as the robot always reaches the destination before processing the next command. Hence, the end-effector velocity between the successive commands reaches to zero, which results into a choppy motion. The pre-computed joint trajectories can be used to command the robot to track set of waypoints ensuring smooth motion.

A valid pre-computed joint trajectory is a set of timestamp, angular position, angular velocity, and angular acceleration for each joint at each waypoint. You must adhere to certain constraints while calculating the trajectory. For more information, see SendPreComputedTrajectory and Precomputed Joint Trajectories.

Calculate joint velocity and acceleration at each waypoint using the numerical differentiation

qs_deg = qs*180/pi;
vel = diff(qs_deg)/dt;
vel(1,:) = 0;
vel(end+1,:) = 0;
acc = diff(vel)/dt;
acc(1,:) = 0;
acc(end+1,:) = 0;

Interpolate the joint position, velocity and acceleration to ensure the 0.001 seconds time step between two trajectory points

timestamp = 0:0.001:t(end);
qs_deg = interp1(t,qs_deg,timestamp);
vel = interp1(t,vel,timestamp);
acc = interp1(t,acc,timestamp);

Connect to the Robot

Simulink.importExternalCTypes(which('kortex_wrapper_data.h'));
gen3Kinova = kortex();
gen3Kinova.ip_address = '192.168.1.10';
 
isOk = gen3Kinova.CreateRobotApisWrapper();
if isOk
   disp('You are connected to the robot!'); 
else
   error('Failed to establish a valid connection!'); 
end

Visualize the Actual Movement of the Robot

title('Actual Movement of the Robot');
[~,~, actuatorFb, ~] = gen3Kinova.SendRefreshFeedback();
show(gen3, ((actuatorFb.position)*pi/180)','PreservePlot',false);
drawnow;

Send Robot to Starting Point of the Trajectory

Note that the valid range of input for SendJointAngles is 0 to 360 degrees while the computed angles are in a range of -180 to 180 degrees. Hence the joint angles must be wrapped around 0 to 360 degrees.

jointCmd = wrapTo360(qs_deg(1,:));
constraintType = int32(0);
speed = 0;
duration = 0;
 
isOk = gen3Kinova.SendJointAngles(jointCmd, constraintType, speed, duration);
if isOk
    disp('success');
else
    disp('SendJointAngles cmd error');
    return;
end

Check if the robot has reached the starting position

while 1
    [isOk,~, actuatorFb, ~] = gen3Kinova.SendRefreshFeedback();
    show(gen3, ((actuatorFb.position)*pi/180)','PreservePlot',false);
    drawnow;
    if isOk
        if max(abs(wrapTo360(qs_deg(1,:))-actuatorFb.position)) < 0.1
            disp('Starting point reached.')
            break;
        end 
    else
        error('SendRefreshFeedback error')
    end
end

Send Pre-Computed Trajectory

isOk = gen3Kinova.SendPreComputedTrajectory(qs_deg.', vel.', acc.', timestamp, size(timestamp,2));
if isOk
    disp('SendPreComputedTrajectory success');
else
    disp('SendPreComputedTrajectory command error');
end

Check if the robot has reached the end position

while 1
    [isOk,~, actuatorFb, ~] = gen3Kinova.SendRefreshFeedback();
    show(gen3, ((actuatorFb.position)*pi/180)','PreservePlot',false);
    drawnow;
    if isOk
        if max(abs(wrapTo360(qs_deg(end,:))-actuatorFb.position)) < 0.1
            disp('End Point reached.')
            break;
        end 
    else
        error('SendRefreshFeedback error')
    end
end

Disconnect from the Robot

Use this command to disconnect from Kinova Gen3 robot Robot.

isOk = gen3Kinova.DestroyRobotApisWrapper();

Clear workspace

clear;