Send Precomputed Trajectory to the Robot

Step 6 of 6 in Getting Started with ROS Connectivity

This section helps you to send commands to robot for following a precomputed joint trajectory. Ensure that the robot is at home position before executing the following commands.

The expected motion of the robot is same as shown in the example Generate a Trajectory Using a Set of Waypoints for KINOVA Gen3 Robot End-Effector.

Load the robot model and set the data format.

gen3 = loadrobot('kinovagen3'); 
gen3.DataFormat = 'column';

Read the current state of the robot.

jSub = rossubscriber('/my_gen3/joint_states'); 
jMsg = receive(jSub); 
jAngs =  jMsg.Position(1:7);

Display the robot in current configuration.

show(gen3,jAngs,'Frames','off'); 
T = getTransform(gen3,jAngs,'EndEffector_Link');

Define waypoint offset and the time instances to achieve the waypoint.

waypointTimes = [0 7 14 21 28]; 
waypointOffsets = [0    0    0   0     0   0;  
                   -0.1 0.2  0.2 pi/4 0 0;  
                   -0.2 0    0.1 0 0 pi/4;  
                   -0.1 -0.2 0.2 -pi/4 0 0;  
                   0    0    0   0 0 0]; % [X Y Z yaw pitch roll] 
           
startPose = [tform2trvec(T) tform2eul(T,'ZYX')]; 
taskWaypoints = startPose + waypointOffsets; 
jointWaypoints = zeros(7,numel(waypointTimes));

Calculate the joint configuration for each waypoint using inverse kinematic.

ik = inverseKinematics('RigidBodyTree',gen3); 
for idx = 1:numel(waypointTimes) 
    ikGoal = trvec2tform(taskWaypoints(idx,1:3)) * eul2tform(taskWaypoints(idx,4:6),'ZYX'); 
    ikSoln = ik('EndEffector_Link',ikGoal,[1 1 1 1 1 1],jAngs); 
    jointWaypoints(:,idx) = ikSoln(1:7); 
end

Plot the robot at calculated waypoints.

figure('Visible','on') 
show(gen3,jAngs,'Frames','off'); 
hold on; 
plot3(taskWaypoints(:,1),taskWaypoints(:,2),taskWaypoints(:,3),'ro:','LineWidth',2) 
for idx = 1:numel(waypointTimes) 
    show(gen3,jointWaypoints(:,idx),'Frames','off','PreservePlot',false); 
    pause(2) 
end

Package the trajectory and send to the robot.

sampleTime = 0.001; % Do not modify 
numSamples = waypointTimes(end)/sampleTime + 1; 
[q,qd,qdd] = trapveltraj(jointWaypoints,numSamples, ...  
    'EndTime',repmat(diff(waypointTimes),[7 1])); 
trajTimes = linspace(0,waypointTimes(end),numSamples);  

[trajAct,trajGoal] = rosactionclient( ...  
    '/my_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory'); 

jointNames = {'joint_1','joint_2','joint_3','joint_4','joint_5','joint_6','joint_7'}; 
packageJointTrajectory(trajGoal,jointNames,q,qd,qdd,trajTimes) 
sendGoal(trajAct,trajGoal); 

function packageJointTrajectory(msg,jointNames,q,qd,qdd,trajTimes) 

    % Initialize values 
    N = numel(trajTimes); 
    numJoints = size(q,1); 
    zeroVals = zeros(numJoints,1); 
 
    % Assign joint names to ROS message 
    msg.Trajectory.JointNames = jointNames; 
    
    % Assign constraints 
    for idx = 1:numJoints 
        msg.GoalTolerance(idx) = rosmessage('control_msgs/JointTolerance'); 
        msg.GoalTolerance(idx).Name = jointNames{idx}; 
        msg.GoalTolerance(idx).Position = 0; 
        msg.GoalTolerance(idx).Velocity = 0.1; 
        msg.GoalTolerance(idx).Acceleration = 0.1; 
    end 
   
    % Loop through waypoints and fill in their data 
    trajPts(N) = rosmessage('trajectory_msgs/JointTrajectoryPoint'); 
    for idx = 1:N 
        m = rosmessage('trajectory_msgs/JointTrajectoryPoint'); 
        m.TimeFromStart = rosduration(trajTimes(idx)); 
        m.Positions = q(:,idx); 
        m.Velocities = qd(:,idx); 
        m.Accelerations = qdd(:,idx); 
        m.Effort = zeroVals; 
        trajPts(idx) = m; 
        if mod(idx,round(N/10))==0 
           disp(['Packing waypoint ' num2str(idx) ' of ' num2str(N) '...']);  
        end 
    end   
    msg.Trajectory.Points = trajPts; 
end 
rosshutdown;

Getting Started with ROS Connectivity