In this exercise, based on the trajectory generated from Lab 8. Analyze the given code using ABB IRB140 robot and perform the following tasks:
1. Comment the code to
iefly explain its functions.
2. Change the initial T1 and T2 and simulate the motions of the robot.
3. In the program, you will find the following code (line 73-75):
dist = sqrt(sum((sol(:,i-1)-qinv(:,1:end)).^2));
[~,minInd] = min(dist);
sol(:,i) = qinv(:,minInd);
What do these three lines of code do?
4. For the inversekinematic() function, why does the program generate different joint parameters?
%% Program for end effector trajectory
% Clear Screen
clc
clear all
%% noa matrix
%initial position
T1=[ XXXXXXXXXX; XXXXXXXXXX; XXXXXXXXXX;0 0 0 1];
% Final position
T2=[ XXXXXXXXXX; XXXXXXXXXX; XXXXXXXXXX;0 0 0 1];
% translation without rotation
tTimes = linspace(0,1,50);
% Time interval
tInterval=[0 10];
% parabolic blend technique transform
[s,sd,sdd,st]=trapveltraj([0.3 0.5;0.1 0.4;0.4 0.9],numel(tTimes));
[T,dT,ddT]=transformtraj(T1,T2,tInterval,tTimes);
figure(1)
plotTransforms(tform2trvec(T),tform2quat(T))
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Plot Transform')
%% Magnitude of velocity as a function of time
% Trajectory for X Y Z
figure(2)
subplot(3,1,1)
plot(st,s)
xlabel('t')
ylabel('Position')
legend('X','Y','Z')
subplot(3,1,2)
plot(st,sd)
xlabel('t')
ylabel('Velocity')
legend('X','Y','Z')
subplot(3,1,3)
plot(st,sdd)
xlabel('t')
ylabel('Acceleration')
legend('X','Y','Z')
sgtitle('Trajectory for X Y Z')
fprintf('Max velocity in V_x = %0.1f\n',max(sd(1,:)))
fprintf('Max velocity in V_y = %0.1f\n',max(sd(2,:)))
fprintf('Max velocity in V_z = %0.1f\n\n',max(sd(3,:)))
%% Vector Pointing from initial position to final position
figure(3)
plot3([0.3 0.5],[0.1 0.4],[0.4 0.9],'-b',[0.3 0.5],[0.1 0.4],[0.4 0.9],'.k','Markersize',25)
axis equal
xlabel('X')
ylabel('Y')
zlabel('Z')
title('Ploting Vector')
fprintf('vector T1->T2 = %0.1fi + %0.1fj + %0.1fk\n\n',0.5-0.3,0.4-0.1,0.9-0.4)
PRACTICAL
SESSION
2:
INVERSE
KINEMATICS
Arturo
Gil
Aparicio
XXXXXXXXXX
OBJECTIVES
After the practical session, the student should be able to:
- Solving the inverse kinematic problema of a serial manipulator using
geometric methods.
- Implementing this code in Matlab for a chosen robotic arm.
- Generating joint trajectories so that the end effector follows a linear
trajectory in the cartesian space.
- Analyzing singular points by means of the Jacobian matrix.
Index
1. First
steps
2. The
inverse
kinematic
problem
3. Solving
the
inverse
kinematic
problem
for
a
6
DOF
robot
4. Adding
your
robot
to
the
li
ary
5. Direct
and
inverse
Jacobian
6. Follow
a
line
in
space
1 First steps
We first start with a kinematic analysis of the SCARA robot arm represented in
Figure 1. The table with the Denavit-Hartenberg parameters is specified next:
Theta
D
A
Alfa
1
q1
l1=0.5m
l2=0.5m
0
2
q2
0
l3=0.3m
π
3
0
q3
0
0
4
q4
0
0
0
Figure
1
To begin with, we are going to compute the direct position and orientation of
end effector as a function of the joint coordinates. You can type the following
commands at the Matlab prompt:
init_lib
robot=load_robot('example','scara')
T=directkinematic(robot, [ XXXXXXXXXX])
T =
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
drawrobot3d(robot, [ XXXXXXXXXXpi/4])
Figure 2 presents the result of the drawrobot3d function.
Figure
2
2 The inverse kinematic problem
Given a position and orientation of the end effector, we would like to compute
the joint coordinates that
ing the robot’s end effector to it. In the case of the
example SCARA we can achieve it with the following commands. Generally,
there exist more than one solution to achieve it.
pwd
ans =
Users/arturogilaparicio/Desktop/arte3.1.6
init_lib
T=directkinematic(robot, [0 0 0 0])
qinv = inversekinematic(robot, T)
Computing inverse kinematics for the Scara example 4GDL arm robot
qinv =
XXXXXXXXXX
XXXXXXXXXX
XXXXXXXXXX
XXXXXXXXXX
T=directkinematic(robot, [pi/2 pi/4 0.1 pi/8])
qinv = inversekinematic(robot, T)
Computing inverse kinematics for the Scara example 4GDL arm robot
qinv =
XXXXXXXXXX
XXXXXXXXXX
XXXXXXXXXX
XXXXXXXXXX
Note that, after initializing the li
ary, we compute the direct kinematics for the
obot’s initial position q=[0 0 0 0]. A call to the function qinv =
inversekinematic(robot, T) takes two arguments. The first stores the robot
parameters, whereas the second, T, specifies the position and orientation of the
end effector. The function qinv = inversekinematic(robot, T) is common for
all the robots in the li
ary, however, the function calls internally a different
function, named inversekinematic_scara that is placed at the robot’s home
directory
arte3.1.6
obots/example/scara.
The
name
of
the
inverse
kinematic
function
is
specified
in
the
parameters.m
file,
in
the
line
obot.inversekinematic_fn = 'inversekinematic_scara(robot, T)'.
function robot = parameters()
%Path where everything is stored for this robot
obot.path = 'robots/example/scara';
%Kinematic parameters
obot.DH.theta= '[q(1) q(2) 0 q(4)]';
obot.DH.d='[ XXXXXXXXXXq(3) 0]';
obot.DH.a='[ XXXXXXXXXX]';
obot.DH.alpha= '[ 0 pi 0 0]';
%Jacobian matrix. Variation of (X, Y, Z) as a function of (w1, w2, w3)
obot.J='[-a(2)*sin(q(1)+q(2))-a(1)*sin(q(1)) -a(2)*sin(q(1)+q(2)) 0;
a(2)*cos(q(1)+q(2))+a(1)*cos(q(1)) a(2)*cos(q(1)+q(2)) 0; 0 0 -1];';
obot.name='Scara example 4GDL arm';
obot.inversekinematic_fn = 'inversekinematic_scara(robot, T)';
It is important to note that the inversekinematic function returns two posible
solutions for qinv, only one of them matches the initial q specified in the direct
kinematic function. However, both solutions for qinv match the original matrix T.
You can test these ideas with:
T=directkinematic(robot, [pi/2 pi/4 0.1 pi/8])
T =
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
qinv = inversekinematic(robot, T)
T=directkinematic(robot, qinv(:,1))
T =
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
T=directkinematic(robot, qinv(:,2))
T =
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
XXXXXXXXXX XXXXXXXXXX
You can observe this by calling drawrobot3d for both solutions to the inverse
kinematics.
drawrobot3d(robot, qinv(:,1))
drawrobot3d(robot, qinv(:,2))
3 Solving the inverse kinematic problem for a 6
DOF robot
Next, Figure 3 presents the ABB IRB 140 robot. Next, its co
esponding D-H
table is presented.
Figure
3
θ
d
a
α
q1
0.352
0.070
-‐
π/2
q2
-‐
π/2
0
0.360
0
q3
0
0
-‐
π/2
q4
0.380
0
π/2
q5
0
0
-‐
π/2
q6
0.065
0
0
You can observe these parameters in the
parameters.m
file, which resides in
arte3.1.6
obots/a
i
140
obot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)]';
obot.DH.d='[ XXXXXXXXXX065]';
obot.DH.a='[ XXXXXXXXXX]';
obot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
obot.J=[];
obot.inversekinematic_fn = 'inversekinematic_i
140(robot, T)';
The inverse kinematic problem is solved by the function
inversekinematic_i
140(robot, T) that is placed at
arte3.1.6
obots/a
i
140. You can find some more information about
solving this inverse kinematic problem in the ARTE reference manual.