Robotics Lab

Lecture #5

Purpose of This Lecture:

- Exponential Mapping and k– Ø Procedure

- Learn about the basics of Cartesian Motion Planning.
Programs Practice:
Practice num. 1 :

Draw the axis of the following Frames in MATLAB :-

1 0 0 0 0 −1 0 1 1 0 0 −1
𝐻00 =(0 1 0 0) , 𝐻1 =( 0 0 1 2) , 𝐻 2 =(0
0 0
0 −1 3 )
0 0 1 0 −1 0 0 2 0 1 0 1
0 0 0 1 0 0 0 1 0 0 0 1

The Code :-

function exp1()
H00=eye(4);
H10=[0 -1 0 1; 0 0 1 2;-1 0 0 2; 0 0 0 1];
H20=[1 0 0 -1; 0 0 -1 3; 0 1 0 1; 0 0 0 1];
draw_axis(H00);
draw_axis(H10);
draw_axis(H20);
axis([-1.5 1.5 -0.5 3.5 -0.5 2.5]),grid;
xlabel('X');ylabel('Y');zlabel('Z');
end

function draw_axis(H10)
p0 =[H10(1,4) H10(2,4) H10(3,4)];
H10=H10*0.5;
for i=1:3
p1 = [p0(1)+H10(1,i) p0(2)+H10(2,i) p0(3)+H10(3,i)];
vectarrow(p0,p1,i);
end
end

1
function vectarrow(p0,p1,x)
x0 = p0(1);y0 = p0(2);z0 = p0(3);
x1 = p1(1);y1 = p1(2);z1 = p1(3);
if x==1
plot3([x0;x1],[y0;y1],[z0;z1],'color','red','LineWidt
h', 4); % Draw a line between p0 and p1
elseif x==2
plot3([x0;x1],[y0;y1],[z0;z1],'color','black','LineWi
dth', 4); % Draw a line between p0 and p1
else
plot3([x0;x1],[y0;y1],[z0;z1],'color','blue','LineWid
th', 4); % Draw a line between p0 and p1
end
hold on;
end

Practice num. 2 :

The Exponential Mapping and k– Ø Procedure

In many application cases, we wish to perform a rotation for a given frame by rotating itself
with a certain angle Ø about an instantaneously fixed unit vector k. Such a rotation approach
is intuitively more natural, effective and easier for sampling in robotic path planning
applications because the multiplication of matrices in a successive rotation process is not
commutable in general.

Given that k is a unit vector and k=[k1 k2 k3]’ , then :

0 −𝑘3 𝑘2
𝑘
S(k) = K = [ 3 0 −𝑘1 ]
−𝑘2 𝑘1 0

Where:

1. K is skew-symmetric with tr(K) = 0;

2. K2 is symmetric with tr(K2) = −2;

3. K3 = −K.

exp(ØK)= I + (sinØ)K + (1 – [cosØ])K2 = R

The whole k- Ø procedure is written into MATLAB code as a function called kthetatoR
where you send k and Ø, and the function will return the rotational matrix R .

2
function R=KthetatoR(k,angle)
k=k/(k(1)^2+k(2)^2+k(3)^2)^0.5;
K=[0 -1*k(3) k(2);k(3) 0 -1*k(1);-1*k(2) k(1) 0];
R=eye(3)+sind(angle)*K+(1-cosd(angle))*(K^2);
end
The following equations offer an inverse formula in a recursive order to determine Ø and k in
terms of a given rotation matrix R:

𝑡𝑟(𝑅)−1
Ø = cos −1 ( )
2

𝑅−𝑅 𝑇
K=
2 sin Ø

The whole inverse formula of the k- Ø procedure is written into MATLAB code as a function
called Rtoktheta where you send the rotational matrix R, and the function will return the
angle Ø and the vector k .

function [angle,k] =Rtoktheta(R)

angle=acosd((trace(R)-1) /2);
if angle==180
[vec,val]=eig(R);
if val(1,1)==1
k=vec(:,1);
else if val(2,2)==1
k=vec(:,2);
else
k=vec(:,3);
end
end
else
K=(R-R')/(2*sind(angle));
k=[-1*K(2,3);K(1,3);-1*K(1,2)];
end
end

Testing the functions :-

function exp2()
R=[1 0 0;
0 cosd(90) -sind(90);
0 sind(90) cosd(90)]
[angle,k] =Rtoktheta(R)
KthetatoR(k,angle)
end

3
Practice num. 3 :

In a robotic task/path planning phase, it is often required to determine a trajectory, along

which the robotic hand or end-eff ector frame travels from a starting point to an ending or
destination point. To uniquely represent either the starting or the ending point of the
trajectory, we can now easily adopt a 4 by 4 homogeneous transformation matrix to do so.

Cartesian motion planning algorithm :-

It’s a trajectory sampling process, where you have the chosen end-effector position and
orientation ( labeled 𝑇06 (N) ) and the present end-effector position and orientation (𝑇06 (0) ).

In order to sample the trajectory into N uniform sampling points, we have to take position
and orientation into distinct consideration due to the uncommon natures between them, i.e.,
the former is additive and the latter is multiplicative. For the position part, if the trajectory is
a straight line, we may first find the difference between the last columns of 𝐻02 and 𝐻01 , i.e.,
𝑝12 (0)
2 2 1
𝑝1 (0) = 𝑝0 −𝑝0 , and then to determine the sampling interval Δp0= ⁄ . Finally, at the i-th
𝑁
sampling step (0 ≤ i ≤ N), the position vector of the robotic hand with respect to the base is
determined by
(𝑖) 𝑖
𝑝0 = 𝑝01 + i · Δp0 = 𝑝01 + 𝑁 · (𝑝02 − 𝑝01 )

(0) (𝑁)
Obviously, 𝑝0 = 𝑝01 and 𝑝0 = 𝑝02 .

In contrast, for the orientation part, we must first find the “difference” of the orientations
between frame 1 and frame 2, i.e. R21 = R01 · R20 . Then, using the k−Ø procedure, as
introduced above, to convert the “difference” R21 into the equivalent k and Ø , and further to
Ø
sample the angle Ø into N intervals. Namely, ΔØ = N , and then using its forward conversion
(i) 𝑖
to determine R1 by substituting the same unit vector k but a different angle i · ΔØ =𝑁 · Ø into
the forward k- Ø procedure. Therefore, the orientation of the hand frame with respect to the
base at the i-th sampling point on the trajectory is given by
(i) (i)
R 0 = R10 · R1
(i)
It can also be verified that R 0 matches R10 when i = 0 and reaches R20 when i = N.

The following MATLAB code can be used as a function to calculate a specific i-th sample in
(i)
any program to get the resulting H0 :

4
function HF0=cartesianmotion(H10,H20,N,t)
pF0=H10(1:3,4)+t/N*(H20(1:3,4)-H10(1:3,4));
H21=(H10(1:3,1:3)')*H20(1:3,1:3);
[angle,k]=Rtoktheta(H21(1:3,1:3));
RF1=KthetatoR(k,t*angle/N);
RF0=H10(1:3,1:3)*RF1;
HF0=[RF0,pF0;[0 0 0 1]];
end

Run this code to better understand the functions above :-

function exp3()
H00=eye(4);
H10=[0 -1 0 1;
0 0 1 2;
-1 0 0 2;
0 0 0 1];
H20=[1 0 0 -1;
0 0 -1 3;
0 1 0 1;
0 0 0 1];
N1=40;
for t=1:N1
if t>1
[az,el] = view;
else
az=51;el=8;
end
HF0=cartesianmotion(H10,H20,N1,t);
draw_axis(H00);
draw_axis(H10);
draw_axis(H20);
draw_axis(HF0);
axis([-1.5 1.5 -0.5 3.5 -0.5
2.5]),grid,view(az,el);
xlabel('X');
ylabel('Y');
zlabel('Z');
drawnow;
pause(0.1);
hold off;
end
end