Location via proxy:   [ UP ]  
[Report a bug]   [Manage cookies]                
Scribd Logo
Upload

Welcome to Scribd!

  • 10 views

    Robotics Lab Lecture #5: Purpose of This Lecture

    Uploaded by

    Sonoso
    This document describes the process of Cartesian motion planning using homogeneous transformations. It discusses exponential mapping and the k-θ procedure to represent rotations. It provides MATLAB code to calculate a trajectory between starting and ending frames by sampling the position linearly and orientation using k-θ conversion at intervals between 0 to 1. The code returns the homogeneous transformation matrix for any sample point on the planned trajectory.

    Copyright:

    © All Rights Reserved
    Download as PDF, TXT or read online from Scribd

    Robotics Lab Lecture #5: Purpose of This Lecture

    Uploaded by

    Sonoso
    10 views5 pages
    This document describes the process of Cartesian motion planning using homogeneous transformations. It discusses exponential mapping and the k-θ procedure to represent rotations. It provides MATLAB code to calculate a trajectory between starting and ending frames by sampling the position linearly and orientation using k-θ conversion at intervals between 0 to 1. The code returns the homogeneous transformation matrix for any sample point on the planned trajectory.

    Original Title

    RoLab 5

    Copyright

    © © All Rights Reserved

    Available Formats

    PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    This document describes the process of Cartesian motion planning using homogeneous transformations. It discusses exponential mapping and the k-θ procedure to represent rotations. It provides MATLAB code to calculate a trajectory between starting and ending frames by sampling the position linearly and orientation using k-θ conversion at intervals between 0 to 1. The code returns the homogeneous transformation matrix for any sample point on the planned trajectory.

    Copyright:

    © All Rights Reserved
    Download as PDF, TXT or read online from Scribd
    Download as pdf or txt
    10 views5 pages

    Robotics Lab Lecture #5: Purpose of This Lecture

    Uploaded by

    Sonoso
    This document describes the process of Cartesian motion planning using homogeneous transformations. It discusses exponential mapping and the k-θ procedure to represent rotations. It provides MATLAB code to calculate a trajectory between starting and ending frames by sampling the position linearly and orientation using k-θ conversion at intervals between 0 to 1. The code returns the homogeneous transformation matrix for any sample point on the planned trajectory.

    Copyright:

    © All Rights Reserved
    Download as PDF, TXT or read online from Scribd
    Download as pdf or txt
    of 5

    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

    You might also like