Lesson 5

代码-TwoDofPlanarRobot.m

表示一个 2 自由度平面机器人。该类包含构造函数、计算正向运动学模型的函数、计算平移雅可比矩阵的函数，以及在二维空间中绘制机器人的函数。

classdef TwoDofPlanarRobot
    %TwoDofPlanarRobot - 表示一个 2 自由度平面机器人类
    
    properties (Access=private)
        % The lengths of the two links, in meters
        l1
        l2
    end
    
    methods
        function obj = TwoDofPlanarRobot(l1, l2)
            % TwoDofPlanarRobot creates a 2-DoF planar robot with link lengths l1 and l2
            obj.l1 = l1;
            obj.l2 = l2;
        end
        
        function t_w_r = fkm(obj, theta1, theta2)
            % fkm calculates the FKM for the 2-DoF planar robot.
            % Input: theta1, theta2 - Joint angles in radians
            
            % Include the namespace inside the function
            include_namespace_dq
            
            % Rotation and translation for the first link
            x_w_1 = cos(theta1/2.0) + k_*sin(theta1/2.0);
            x_1_r1 = 1 + 0.5*E_*i_*obj.l1;
            x_w_r1 = x_w_1 * x_1_r1;
            
            % Rotation and translation for the second link
            x_r1_r2 = cos(theta2/2.0) + k_*sin(theta2/2.0);
            x_r2_r = 1 + 0.5*E_*i_*obj.l2;
            x_w_r = x_w_r1 * x_r1_r2 * x_r2_r;
            
            % Get the translation (end-effector position)
            t_w_r = translation(x_w_r);
        end
        
        function Jt = translation_jacobian(obj, theta1, theta2)
            % Calculates the translation Jacobian of the 2-DoF planar robot.
            
            % Include the namespace inside the function
            include_namespace_dq
            
            % Partial derivatives of the end effector position
            j1 = obj.l1*(-i_*sin(theta1) + j_*cos(theta1)) + obj.l2*(-i_*sin(theta1+theta2) + j_*cos(theta1+theta2));
            j2 = obj.l2*(-i_*sin(theta1+theta2) + j_*cos(theta1+theta2));
            
            % Construct the Jacobian matrix
            Jt = [vec3(j1), vec3(j2)];
        end
        
        function plot(obj, theta1, theta2)
            % Plot the 2-DoF planar robot in the xy-plane
            
            % Calculate the positions of each joint and the end effector
            x1 = obj.l1 * cos(theta1);
            y1 = obj.l1 * sin(theta1);
            x2 = x1 + obj.l2 * cos(theta1 + theta2);
            y2 = y1 + obj.l2 * sin(theta1 + theta2);
            
            % Plot the links
            plot([0 x1 x2], [0 y1 y2], 'r-o', 'LineWidth', 2, 'MarkerSize', 6)
            hold on
            % Mark the base with an 'o'
            plot(0, 0, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k')
            % Mark the end effector with an 'x'
            plot(x2, y2, 'bx', 'MarkerSize', 8, 'LineWidth', 2)
            hold off
            title('The Two DoF planar robot in the xy-plane')
            xlim([-obj.l1 - obj.l2, obj.l1 + obj.l2])
            xlabel('x [m]')
            ylim([-obj.l1 - obj.l2, obj.l1 + obj.l2])
            ylabel('y [m]')
            grid on
        end
    end
end

可视化

% Length
l1 = 1;
l2 = 1;

% Create robot
two_dof_planar_robot = TwoDofPlanarRobot(l1,l2);

% Choose theta freely
theta1 =3.55;%添加滑动条进行修改
theta2 =-4.19;%添加滑动条进行修改

% Get the fkm, based on theta
t_w_r = two_dof_planar_robot.fkm(theta1,theta2)

% Plot the robot in the xy-plane
two_dof_planar_robot.plot(theta1,theta2);

代码-two_dof_planar_robot_position_control.m

实现了一个 2 自由度平面机器人的任务空间位置控制，旨在让机器人的末端执行器移动到指定的目标位置 (tx, ty)，直到误差达到设定的阈值为止。

clear all;
close all;

% Length
l1 = 1;
l2 = 1;

% Sampling time [s]
tau = 0.001; 

% Control threshold [m/s]
control_threshold = 0.01;

% Control gain
eta = 200;

% Create robot
two_dof_planar_robot = TwoDofPlanarRobot(l1,l2);

% Initial joint value [rad]
theta1 = 0;
theta2 = pi/2;
theta=[theta1;theta2];

% Desired task-space position
% tx [m]
tx =0;
% ty [m]
ty =2;

% Compose the end effector position into a pure quaternion
td = DQ([tx ty 0]);

% Position controller. The simulation ends when the 
% derivative of the error is below a threshold
time = 0;
t_error_dot = DQ(1); % Give it an initial high value
t_error = DQ(1); % Give it an initial high value
while norm(vec4(t_error_dot)) > control_threshold
    % Get the current translation
    t = two_dof_planar_robot.fkm(theta(1),theta(2));
    
    % Calculate the error and old error
    t_error_old = t_error;
    t_error = (t-td);
    t_error_dot = (t_error-t_error_old)/tau;
    
    % Get the translation jacobian, based on theta
    Jt = two_dof_planar_robot.translation_jacobian(theta(1),theta(2));
    
    % Calculate the IDKM
    theta_dot = -eta*pinv(Jt)*vec3(t_error);
    
    % Move the robot
    theta = theta + theta_dot*tau;
    
    % Plot the robot
    two_dof_planar_robot.plot(theta(1),theta(2))
    
    % Plot the desired position
    hold on
    plot(tx,ty,'bx')
    hold off
    
    % [For animations only]
    drawnow; % [For animations only] Ask MATLAB to draw the plot now
    pause(0.001) % [For animations only] Pause so that MATLAB has enough time to draw the plot
    
end

DQ(1) 表示创建了一个特殊的双四元数对象，用来初始化误差和误差导数。这一初始化操作仅仅是为了确保控制循环在开始时能正确进入，不会因为初始误差太小而导致循环直接退出。

• DQ(1) 在 DQ Robotics 中表示一个双四元数，初始值为 1。它是单位双四元数，通常表示没有旋转或平移，即一个默认、标准的双四元数。这一数值用于初始化误差和误差导数，并不意味着其实际值很大，而是让程序有一个初始化的起点。

• 在 DQ Robotics 中，通过给 t_error 和 t_error_dot 初始化为 DQ(1)，可以确保循环一开始不会因为误差值太小而退出。只要在后续的计算中计算出真实的误差，循环即可继续执行。

作用总结

• DQ(1) 的作用是初始化，不会因为值太小而导致不进入循环；
• 代码后续将实际误差值更新为 t_error 和 t_error_dot，从而让控制逻辑正常工作。