Currently I have programmed 13 motions for my robotic arm for a project. I want them all to move quickly so I am decreasing the time until I can find a limit for it. However, for some motions, one work while the other doesn't and I can't understand why. Does anyone have a clue why motion 13, 12, 10 and 8 don't work while 11 does? I checked the velocity and inffered acceleration from it, and I can't see any breach of limit based on what I got from kinova gen 3 user guide (should be around page 80 to 90).
count = 0;
year = zeros(10000,1);
month = zeros(10000,1);
day = zeros(10000,1);
hour = zeros(10000,1);
minute = zeros(10000,1);
seconds = zeros(10000,1);
motiontype = zeros(10000,1);
trialnumber = zeros(10000,1);
status = cell(10000,1);
commandprompt = cell(10000,1);
gen3 = loadrobot("kinovaGen3"); %Type 'rigidBodyTree'
gen3.DataFormat = 'column';
q_home = [0 60 180 -90 360 -30 180]'*pi/180;
% q_home = [0 15 180 -130 0 55 90]'*pi/180;
eeName = 'EndEffector_Link'; %Type 'char'
T_home = getTransform(gen3, q_home, eeName); %For more info use...
%https://www.mathworks.com/help/robotics/ref/rigidbodytree.html#:~:text=all%20in%20page-,Description,to%20import%20your%20robot%20model.
% show(gen3,q_home);
% axis auto;
% view([60,10]);
ik = inverseKinematics('RigidBodyTree', gen3);
ik.SolverParameters.AllowRandomRestart = false;
weights = [1, 1, 1, 1, 1, 1];
q_init = q_home;
%Mind the relation between trajectory and time for there are vel limit
center = [0.53 0 0.2]; %[x y z]
% center = [0.5 0 0.4]; %[x y z]
radius = 0.1; %Do not put radius to or above 0.9 with a duration of 10 seconds
dt = 0.25;
t = (0:dt:5)';
theta = t*(2*pi/t(end))-(pi/2);
repeat = true;
n = input(['Which protocol is being used?', '\n(1): Up to down (2): Left to right (3): Front to back', ...
'\n(4): Upper right to lower left (5): Upper left to lower right (6):upper right front to lower left back', ...
'\n(7): Upper left back to lower right front (8): Horizontal circle (9): Vertial circle movement on y/z place (back and forth)', ...
'\n(10): Upper right to lower left circle (11): Upper left to lower right circle (12): Upper front to Lower back circle', ...
'\n(13): Upper back to Lower front circle', '\n ']);
%NOTE: Tilting a trajectory may just mean adding a z coordinate that have
%an appropiate relation to x or y
clock_direction = 0;
while clock_direction ~=-1 && clock_direction ~=1
clock_direction = input(['Clock direction', '\n(1): CW', '\n(-1): CCW', '\n']);
end
while repeat
switch n
case 1
points = center + clock_direction*radius*[0*ones(size(theta)) 0*sin(theta) -sin(theta)];
%Points for up to down movement
repeat = false;
case 2
points = center + clock_direction*radius*[-sin(theta) 0*cos(theta) 0*ones(size(theta))];
%Points for left to right movement
% points = center + radius*[sin(theta)*sin(pi/4) 0*ones(size(theta)) sin(theta)*sin(pi/4)];
%Titled side to side movement
repeat = false;
case 3
points = center + clock_direction*radius*[0*ones(size(theta)) sin(theta) 0*sin(theta)];
%Points for front to back movement
repeat = false;
case 4
points = center + clock_direction*radius*[sin(theta) 0*cos(theta) -sin(theta)];
%Points for upper right to lower left movement
repeat = false;
case 5
points = center + clock_direction*radius*[-sin(theta) 0*cos(theta) -sin(theta)];
%Points for upper left to lower right movement
repeat = false;
case 6
points = center + clock_direction*radius*[sin(theta) sin(theta) -sin(theta)];
%Points for upper right front to lower left back movement
repeat = false;
case 7
points = center + clock_direction*radius*[-sin(theta) -sin(theta) -sin(theta)];
%Points for upper left back to lower right front movement
repeat = false;
case 8
points = center + radius*[sin(theta) clock_direction*cos(theta) 0.5*ones(size(theta))];
% horizontal circle movement
repeat = false;
case 9
% points = center + radius*[sin(theta) 0.5*ones(size(theta)) clock_direction*cos(theta)];
% vertial circle movement on x/z plane
points = center + radius*[0.5*ones(size(theta)) sin(theta) -clock_direction*cos(theta)];
% vertial circle movement on y/z place
repeat = false;
case 10
points = center + radius*[sin(theta)*cos(pi/4) clock_direction*cos(theta) -sin(theta)*sin(pi/4)];
%Upper right to lower left ( 45 degrees, require more review)
repeat = false;
case 11
points = center + radius*[-sin(theta)*cos(pi/4) -clock_direction*cos(theta) -sin(theta)*sin(pi/4)];
%Upper left to lower right( 45 degrees, require more review)
repeat = false;
case 12
points = center + radius*[-clock_direction*cos(theta) sin(theta)*cos(pi/4) -sin(theta)*sin(pi/4)];
%Upper front to Lower back. Tilted circle side to side( 45 degrees, require more review)
repeat = false;
case 13
points = center + radius*[clock_direction*cos(theta) -sin(theta)*cos(pi/4) -sin(theta)*sin(pi/4)];
%Upper back to Lower front. Tilted circle side to side( 45 degrees, require more review)
% points = center + radius*[sin(theta) clock_direction*cos(theta) cos(theta)*tan(pi/3)];
%Tilted circle(elispe?. require more review). Not running for unknown reasons (speed)?
% points = center + radius*[sin(theta) -clock_direction*sin(theta)*sin(pi/4) clock_direction*cos(theta)*cos(pi/4)];
%Interesting application
repeat = false;
otherwise
disp('invalid input, shutting down')
clear;
return
end
end
% hold on;
% plot3(points(:,1),points(:,2),points(:,3),'-*g', 'LineWidth', 1.5);
% xlabel('x');
% ylabel('y');
% zlabel('z');
% axis auto;
% % view([60,10]);
% view([0,60]);
% grid('minor');
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,:)'; %Desired position, unknown why only row 1-3 of column 4 is changed
[q_sol, q_info] = ik(eeName, T_des, weights, q_init);
%Solution information related to execution of the algorithm, q_info, is returned with the joint configuration solution, q_sol.
%eeName: End-effector name, specified as a character vector
%T_des: End-effector desired pose
%weights: Weights for pose tolerance 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.
%q_init: Initial guess of robot configuration, specified as a structure array or vector.
% 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='manual';
% hold on;
%
% % Plot waypoints
% plot3(points(:,1),points(:,2),points(:,3),'-g','LineWidth',2);
% % axis auto;
% axis ([-1 1 -0.4 0.4 -0.4 0.6]);
% 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
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'
disp('Operation halted')
clear;
return;
end
cycle = input('Number of cycles?');
%Calculate joint velocity and acceleration at each waypoint with 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
f = figure;
i = 0;
global a
a = 0;
while a < cycle
a = a+1
i = i+1;
set(f,'WindowKeyPressFcn',@keyPressCallback);
pause(3)
if a >= cycle + 1
disp('end of cycle')
clear;
return;
end
% %Verify if user wants to stop each loop
% prompt = 'Do you want to pause? y/n [n]:';
% str = input(prompt,'s');
% if isempty(str)
% str = 'n';
% end
%
% if str == 'y'
% pause
% end
%
%Send Robot to Starting Point of the Trajectory
jointCmd = wrapTo360(qs_deg(1,:));
constraintType = int32(0); % no_constraint: 0; duration: 1; joint_velocity: 2;
speed = 0; %max 25 degrees/s
duration = 9; %min 8 seconds
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
count = count + 1;
time = clock;
year(count,1) = time(1);
month(count,1) = time(2);
day(count,1) = time(3);
hour(count,1) = time(4);
minute(count,1) = time(5);
seconds(count,1) = round(time(6),1);
motiontype(count,1) = n;
trialnumber(count,1) = i;
status(count,1) = {' S'};
commandprompt(count,1) = {' motion start'};
pause(2)
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
count = count + 1;
time = clock;
year(count,1) = time(1);
month(count,1) = time(2);
day(count,1) = time(3);
hour(count,1) = time(4);
minute(count,1) = time(5);
seconds(count,1) = round(time(6),1);
motiontype(count,1) = n;
trialnumber(count,1) = i;
status(count,1) = {' E'};
commandprompt(count,1) = {' motion end'};
if a == cycle
disp('protocol finished, shutting down')
end
end
year = year(1:count,1);
month = month(1:count,1);
day = day(1:count,1);
hour = hour(1:count,1);
minute = minute(1:count,1);
seconds = seconds(1:count,1);
motiontype = motiontype(1:count,1);
trialnumber = trialnumber(1:count,1);
status = status(1:count,1);
commandprompt = commandprompt(1:count,1);
T = table(year,month,day,hour,minute, seconds, motiontype, trialnumber, status,commandprompt);
file_time = clock;
file_year = num2str(file_time(1));
file_month = num2str(file_time(2));
file_day = num2str(file_time(3));
file_hour = num2str(file_time(4));
file_minute = num2str(file_time(5));
file_seconds = num2str(round(file_time(6),1));
motion = num2str(n);
filename = append(file_year,'_',file_month,'_',file_day,'_',file_hour,'_',file_minute,'_',file_seconds,'_','Normal_motion','motiontype_',motion,'.xls');
%Colons are not allow in excel name. DO NOT DO IT
writetable(T,filename)
%Immediately skip to another cycle.
%For example, if currently on cycle 4, calling function will cause current
%cycle to be the input value
function keyPressCallback(source,eventdata)
global a
keyPressed = eventdata.Key;
prompt = 'Do you want to skip to a specific cycle? y/n [n]: ';
str = input(prompt,'s');
if isempty(str)
str = 'n';
end
if str == 'n'
return;
end
x = input('Skip to cycle:');
a = x;
end
