【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,从而让控制逻辑正常工作。
相关推荐
Matlab精灵5 小时前
Matlab函数中的隐马尔可夫模型
开发语言·matlab·统计学习
Matlab精灵13 小时前
Matlab科研绘图:自定义内置多款配色函数
算法·matlab
xrgs_shz17 小时前
MATLAB读入不同类型图像并显示图像和相关信息
图像处理·计算机视觉·matlab
火山口车神丶20 小时前
某车企ASW面试笔试题
c++·matlab
今天吃饺子20 小时前
2024年SCI一区最新改进优化算法——四参数自适应生长优化器,MATLAB代码免费获取...
开发语言·算法·matlab
Evand J1 天前
集合卡尔曼滤波(Ensemble Kalman Filter),用于二维滤波(模拟平面上的目标跟踪),MATLAB代码
matlab·平面·目标跟踪
我是博博啦2 天前
matlab例题
人工智能·算法·matlab
2402_871321952 天前
MATLAB方程组
gpt·学习·线性代数·算法·matlab
Matlab程序猿小助手2 天前
【MATLAB源码-第218期】基于matlab的北方苍鹰优化算法(NGO)无人机三维路径规划,输出做短路径图和适应度曲线.
开发语言·嵌入式硬件·算法·matlab·机器人·无人机
Matlab精灵3 天前
使用MATLAB进行字符串处理
开发语言·matlab