【Homework】【5】Learning resources for DQ Robotics in MATLAB

Lesson 5


代码-TwoDofPlanarRobot.m

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

matlab 复制代码
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

可视化

matlab 复制代码
% 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),直到误差达到设定的阈值为止。

matlab 复制代码
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_errort_error_dot 初始化为 DQ(1),可以确保循环一开始不会因为误差值太小而退出。只要在后续的计算中计算出真实的误差,循环即可继续执行。

作用总结

  • DQ(1) 的作用是初始化,不会因为值太小而导致不进入循环;
  • 代码后续将实际误差值更新为 t_errort_error_dot,从而让控制逻辑正常工作。
相关推荐
远望清一色1 小时前
基于MATLAB的图片中文字的提取及识别
算法·matlab
黑曼巴、。;12 小时前
直方图均衡化及Matlab实现
图像处理·计算机视觉·matlab
慕容复之巅16 小时前
基于MATLAB+opencv人脸疲劳检测
开发语言·opencv·matlab
MATLAB滤波17 小时前
鲁棒自适应滤波,MATLAB
开发语言·算法·matlab
摆烂仙君17 小时前
matlab建模入门指导
开发语言·matlab
十七算法实验室1 天前
Matlab实现鼠群优化算法(ROS)求解路径规划问题
开发语言·算法·决策树·支持向量机·matlab·动态规划·启发式算法
慕容复之巅1 天前
基于MATLAB刻度线表盘识别系统
图像处理·计算机视觉·matlab
阑梦清川2 天前
数学建模---利用Matlab快速实现机器学习(上)
机器学习·数学建模·matlab·预测算法
玖釉-2 天前
使用Matlab建立决策树
开发语言·决策树·matlab
MATLAB滤波2 天前
膜计算 MATLAB例程(仅例程,无背景)
开发语言·matlab