Send Precomputed Trajectory to the Robot
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;