Scribd Logo
Upload

Welcome to Scribd!

  • 19 views

    btvn3 Robot

    Uploaded by

    Thọ Đỗ
    The document contains Matlab code to simulate the forward kinematics of a 6 degree-of-freedom robotic arm. It defines functions to calculate the joint angles and transformation matrices between reference frames at each joint. The code then uses these functions and the Denavit-Hartenberg parameters of the arm to calculate the final position and orientation of the end effector. It plots the path of the end effector to visualize the forward kinematics simulation.

    Copyright:

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

    btvn3 Robot

    Uploaded by

    Thọ Đỗ
    19 views9 pages
    The document contains Matlab code to simulate the forward kinematics of a 6 degree-of-freedom robotic arm. It defines functions to calculate the joint angles and transformation matrices between reference frames at each joint. The code then uses these functions and the Denavit-Hartenberg parameters of the arm to calculate the final position and orientation of the end effector. It plots the path of the end effector to visualize the forward kinematics simulation.

    Original Title

    btvn3-robot

    Copyright

    © © All Rights Reserved

    Available Formats

    PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    The document contains Matlab code to simulate the forward kinematics of a 6 degree-of-freedom robotic arm. It defines functions to calculate the joint angles and transformation matrices between reference frames at each joint. The code then uses these functions and the Denavit-Hartenberg parameters of the arm to calculate the final position and orientation of the end effector. It plots the path of the end effector to visualize the forward kinematics simulation.

    Copyright:

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

    btvn3 Robot

    Uploaded by

    Thọ Đỗ
    The document contains Matlab code to simulate the forward kinematics of a 6 degree-of-freedom robotic arm. It defines functions to calculate the joint angles and transformation matrices between reference frames at each joint. The code then uses these functions and the Denavit-Hartenberg parameters of the arm to calculate the final position and orientation of the end effector. It plots the path of the end effector to visualize the forward kinematics simulation.

    Copyright:

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

    b) Kiểm chứng băng Matlab

    code Matlab:
    hàm basic_01
    function x = basic_01(a,b,d)
    alpha = atan2d(a/(sqrt(a*a+b*b)), b/(sqrt(a*a+b*b)));
    x1 = atan2d(d/(sqrt(a*a+b*b)), sqrt(1-(d*d/(a*a+b*b)))) -
    alpha;
    x2 = atan2d(d/(sqrt(a*a+b*b)), -sqrt(1-(d*d/(a*a+b*b)))) -
    alpha;
    x = [x1; x2];

    hàm HW3
    function the = HW3(d3, d4, a2, a3, x6_0_ORG, y6_0_ORG,
    z6_0_ORG, phi0_x, phi0_y, phi0_z)
    %l1c1 + l2c12 = x
    %l1s1 + l2s12 = y
    %y6_0_ORG * c1 - x_6_ORG * s1 = d3
    the1 = basic_01(y6_0_ORG, -x6_0_ORG, d3);
    %a3 * c3 - d4 *s3 = K = (Px^2 + Py^2 + Pz^2 - a2^2
    - a3^2 - d3^2
    %-d4^2)/(2 *a2)
    K = (x6_0_ORG^2 + y6_0_ORG^2 + z6_0_ORG^2 - a2^2 -
    a3^2 - d3^2 -d4^2)/(2*a2);
    the3 = basic_01(a3, -d4, K);

    the13 = [the1(1,1) the3(1,1);the1(1,1)


    the3(2,1);the1(2,1) the3(1,1);the1(2,1) the3(2,1)];

    the1 = the13(:,1);
    the3 = the13(:,2);
    %s23 = ((-a3 - a2c3)pz + (c1px + s1py)(a2s3 -
    d4))/(pz^2+ (c1px +
    %s1py)^2)
    %c23 = ((a2s3 - d4)pz - (a3 + a2c3)(c1px +
    s1py))/(pz^2+ (c1px +
    %s1py)^2)
    s23 = [(-a3 - a2*cos(the3(1,1)))*z6_0_ORG +
    (cos(the1(1,1))*x6_0_ORG +
    sin(the1(1,1))*y6_0_ORG)*(a2*sin(the3(1,1)) -
    d4)/((z6_0_ORG^2+ (cos(the1(1,1)))*x6_0_ORG +
    sin(the1(1,1))*y6_0_ORG)^2)
    (-a3 - a2*cos(the3(2,1)))*z6_0_ORG +
    (cos(the1(2,1))*x6_0_ORG +
    sin(the1(2,1))*y6_0_ORG)*(a2*sin(the3(2,1)) -
    d4)/((z6_0_ORG^2+ (cos(the1(2,1)))*x6_0_ORG +
    sin(the1(2,1))*y6_0_ORG)^2)
    (-a3 - a2*cos(the3(3,1)))*z6_0_ORG +
    (cos(the1(3,1))*x6_0_ORG +
    sin(the1(3,1))*y6_0_ORG)*(a2*sin(the3(3,1)) -
    d4)/((z6_0_ORG^2+ (cos(the1(3,1)))*x6_0_ORG +
    sin(the1(3,1))*y6_0_ORG)^2)
    (-a3 - a2*cos(the3(4,1)))*z6_0_ORG +
    (cos(the1(4,1))*x6_0_ORG +
    sin(the1(4,1))*y6_0_ORG)*(a2*sin(the3(4,1)) -
    d4)/((z6_0_ORG^2+ (cos(the1(4,1)))*x6_0_ORG +
    sin(the1(4,1))*y6_0_ORG)^2)];
    c23 = [(a2*sin(the3(1,1) - d4))*z6_0_ORG +
    (cos(the1(1,1))*x6_0_ORG + sin(the1(1,1))*y6_0_ORG)*(a3 +
    a2*cos(the3(1,1)))/((z6_0_ORG^2+ (cos(the1(1,1)))*x6_0_ORG
    + sin(the1(1,1))*y6_0_ORG)^2)
    (a2*sin(the3(2,1) - d4))*z6_0_ORG +
    (cos(the1(2,1))*x6_0_ORG + sin(the1(2,1))*y6_0_ORG)*(a3 +
    a2*cos(the3(2,1)))/((z6_0_ORG^2+ (cos(the1(2,1)))*x6_0_ORG
    + sin(the1(2,1))*y6_0_ORG)^2)
    (a2*sin(the3(3,1) - d4))*z6_0_ORG +
    (cos(the1(3,1))*x6_0_ORG + sin(the1(3,1))*y6_0_ORG)*(a3 +
    a2*cos(the3(3,1)))/((z6_0_ORG^2+ (cos(the1(3,1)))*x6_0_ORG
    + sin(the1(3,1))*y6_0_ORG)^2)
    (a2*sin(the3(4,1) - d4))*z6_0_ORG +
    (cos(the1(4,1))*x6_0_ORG + sin(the1(4,1))*y6_0_ORG)*(a3 +
    a2*cos(the3(4,1)))/((z6_0_ORG^2+ (cos(the1(4,1)))*x6_0_ORG
    + sin(the1(4,1))*y6_0_ORG)^2)];
    the23 = atan2(s23, c23);
    the2 = the23 - the3;
    the = [the13(:,1) the2 the13(:,2)];
    r11 = cos (phi0_x)*cos (phi0_y);r21 = sin
    (phi0_x)*cos (phi0_y);r31 = -sin (phi0_y);
    r12 = cos (phi0_x)*sin (phi0_y)*sin (phi0_z) - sin
    (phi0_x)*cos (phi0_z); r22 = sin (phi0_x)*sin (phi0_y)*sin
    (phi0_z) + cos (phi0_x)*cos (phi0_z); r32 = cos
    (phi0_y)*sin (phi0_z);
    r13 = cos (phi0_x)*sin (phi0_y)*cos (phi0_z) + sin
    (phi0_x)*sin (phi0_z); r23 = sin (phi0_x)*sin (phi0_y)*cos
    (phi0_z) - cos (phi0_x)*sin (phi0_z); r33 = cos
    (phi0_y)*cos (phi0_z);
    R = [r11 r12 r13; r21 r22 r23; r31 r32 r33];

    the4 = atan2( -r13 * sin(the(:,1)) + r23 *


    cos(the(:,1)), - r13 .* cos(the(:,1)) .* c23 - r23 .*
    sin(the(:,1)) .* c23 + r33 * s23);
    the = [the the4];
    s5 = -1 * ( r13 * (cos(the(:,1)) .* c23 .*
    cos(the(:,4)) + sin(the(:,1)) .* sin(the(:,4)))+ r23 *
    (sin(the(:,1)) .* c23 .* cos(the(:,4)) - cos(the(:,1)) .*
    sin(the(:,4))) - r33 * s23 .* cos(the(:,4)));
    c5 = r13 * (-cos(the(:,1)) .* s23 ) + r23 * (-
    sin(the(:,1)) .* s23 ) + r33 *( -c23);

    the5 = atan2 (s5, c5);


    the = [the the5];
    s6 = -r11 * (cos(the(:,1)).* c23 .* sin(the(:,4)) -
    sin(the(:,1)).* cos(the(:,4))) - r21 * (sin(the(:,1)).* c23
    .* sin(the(:,4)) + cos(the(:,1)).* cos(the(:,4))) + r31 *
    (s23.* sin(the(:,4)));
    c6 = r11 * ((cos(the(:,1)).* c23 .* cos(the(:,4)) +
    sin(the(:,1)).* sin(the(:,4))) .* cos(the(:,5))-
    cos(the(:,1)).* s23 .* sin(the(:,5)))+ r21 *
    ((sin(the(:,1)).* c23 .* cos(the(:,4)) - cos(the(:,1)).*
    sin(the(:,4))) .*cos(the(:,5)) - sin(the(:,1)).* s23
    .*sin(the(:,5))) - r31 .* (s23.* cos(the(:,4)).*
    cos(the(:,5)) + c23 .*sin (the(:,5)));
    the6 = atan2 (s6, c6);
    the = [the the6];
    the = [the; the];
    the (5,4) = the (5,4) + pi;the (6,4) = the (6,4) +
    pi;the (7,4) = the (7,4) + pi;the (8,4) = the (8,4) + pi;
    the (5,5) = -the (5,5);the (6,5) = -the (6,5);the
    (7,5) = -the (7,5);the (8,5) = -the (8,5);
    the (5,6) = the (5,6) + pi;the (6,6) = the (6,6) +
    pi;the (7,6) = the (7,6) + pi;the (8,6) = the (8,6) + pi;

    chương trình PUMA_560


    clc;
    clear all;
    close all;
    % SYSTEM PARAMETERS
    the = HW3(0.1, 1, 1, 0.1, 0.5, 1, 0, pi/6, 0, 0);
    d3 = 0.1;
    d4 = 1;
    a2 = 1;
    a3 = 0.1;
    for i = 1:8
    % DEFINE DH TABLE
    DH = [ 0 0 0 the(i,1);
    -pi/2 0 0 the(i,2);
    0 a2 d3 the(i,3);
    -pi/2 a3 d4 the(i,4);
    pi/2 0 0 the(i,5);
    -pi/2 0 0 the(i,6)];
    T_0_i = eye(4);
    T_0_i_all = zeros(4,4,6);
    P_0_0 = [0;0;0];

    for i = 1:6
    %NOTATION
    ci = cos(DH(i, 4)); si = sin(DH(i, 4));
    c_anpi_1 = cos(DH(i, 1)); s_anpi_1 = sin(DH(i, 1));
    ai_1 = DH(i, 2); di = DH(i, 3);

    T_i_1_i = [ ci -si 0 ai_1;


    si*c_anpi_1 ci*c_anpi_1 -s_anpi_1 -
    s_anpi_1*di;
    si*s_anpi_1 ci*s_anpi_1 c_anpi_1
    c_anpi_1*di;
    0 0 0 1];
    T_0_i = T_0_i*T_i_1_i;
    T_0_i_all(:,:,i) = T_0_i;
    end

    %T_0_i

    P_0_1 = T_0_i_all(1:3,4,1);
    P_0_2 = T_0_i_all(1:3,4,2);
    P_0_3 = T_0_i_all(1:3,4,3);
    P_0_4 = T_0_i_all(1:3,4,4);
    P_0_5 = T_0_i_all(1:3,4,5);
    P_0_6 = T_0_i_all(1:3,4,6);
    plot3([P_0_0(1);P_0_1(1)],[P_0_0(2);P_0_1(2)],[P_0_0(3);P_0
    _1(3)], '-rO',...
    [P_0_1(1);P_0_2(1)],[P_0_1(2);P_0_2(2)],...
    [P_0_1(3);P_0_2(3)],'-bO',...
    [P_0_2(1);P_0_3(1)],[P_0_2(2);P_0_3(2)],...
    [P_0_2(3);P_0_3(3)],'-bO',...
    [P_0_3(1);P_0_4(1)],[P_0_3(2);P_0_4(2)],...
    [P_0_3(3);P_0_4(3)],'-bO',...
    [P_0_4(1);P_0_5(1)],[P_0_4(2);P_0_5(2)],...
    [P_0_4(3);P_0_5(3)],'-bO',...
    [P_0_5(1);P_0_6(1)],[P_0_5(2);P_0_6(2)],...
    [P_0_5(3);P_0_6(3)],'-bO');
    hold on;
    grid on;
    xlim([-2.5 4]) ; ylim([-2.5 4]) ; zlim([-2.5 4]);
    xlabel('x') ; ylabel('y') ; zlabel('z') ;
    end

    hình mô phỏng

    You might also like