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

在这里插入图片描述

作业任务

创建一个名为“VS050RobotDH”的类，该类代表Denso VS050机器人，其DH参数如下表所示，并且完全由旋转关节组成。（请记住第6课的内容）

$\theta$	$d$	$a$	$\alpha$
$-\pi$	0.345	0	$\frac{\pi}{2}$
$\frac{\pi}{2}$	0	0.25	0
$-\frac{\pi}{2}$	0	0.01	$-\frac{\pi}{2}$
0	0.255	0	$\frac{\pi}{2}$
$\pi$	0	0	$\frac{\pi}{2}$
0	0.07	0	0

末端执行器：在机器人上沿最后一个关节的 $z$ 轴附加一个20cm长的轴。

初始配置： $\begin{bmatrix} 0 & \frac{\pi}{3} & \frac{\pi}{3} & 0 & \frac{\pi}{3} & 0 \end{bmatrix}^T$ 。

classdef VS050RobotDH

    methods (Static)
        function ret = kinematics()
            %kinematics returns the kinematics of the Denso VS050 robot as DQ_SerialManipulatorDH
            DH_theta=  [-pi,pi/2,-pi/2,0,pi,0];
            DH_d =     [0.345,0,0,0.255,0,0.07];
            DH_a =     [0,0.25,0.01,0,0,0];
            DH_alpha = [pi/2,0,-pi/2,pi/2,pi/2,0];
            DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL,1,6);
            DH_matrix = [DH_theta;
                DH_d;
                DH_a;
                DH_alpha;
                DH_type];
            
            ret = DQ_SerialManipulatorDH(DH_matrix,'standard');
            ret.name = "Denso VS050 robot";
        end
    end
end

`vs050_plane_constraints.m`

创建一个平移控制器，其中 $\tau = 0.01$ 。选择其他控制器参数，使得控制平滑（参见本课和之前的课程示例）。
创建6个平面，中心点为 $p_1 = 0.45\hat{i} + 0.08\hat{k}$ ，使得所有法线都指向内侧，形成一个5cm的立方体。（请记住第4课的内容）
在控制过程中，使用点到平面的虚拟力（VFIs）来保持工具尖端始终在立方体区域内。
依次将末端执行器移动到以下四个期望位置：
$t_d = \left\{0.60\hat{i} + 0.08\hat{k}, 0.20\hat{i} + 0.15\hat{k}, 0.45\hat{i} + 0.08\hat{k}, 0.45\hat{i} + 0.0\hat{k}\right\}$
每个位置持续4个仿真时间秒。
在一个图中绘制任务误差。
使用子图在另一个图中绘制末端执行器到所有六个平面的距离。

tic;  % 记录程序运行时间的起始点
clear;clc;  % 清除工作区和命令窗口
close all;  % 关闭所有图形窗口
include_namespace_dq;  % 包含DQ库的命名空间

% 初始化六个平面的中心点（相对于基坐标系的偏移）
p1_center = DQ([0.025, 0, 0] + [0.45,0,0.08]); % 平面1中心
p2_center = DQ([-0.025, 0, 0] + [0.45,0,0.08]); % 平面2中心
p3_center = DQ([0, 0.025, 0] + [0.45,0,0.08]); % 平面3中心
p4_center = DQ([0, -0.025, 0] + [0.45,0,0.08]); % 平面4中心
p5_center = DQ([0, 0, 0.025] + [0.45,0,0.08]); % 平面5中心
p6_center = DQ([0, 0, -0.025] + [0.45,0,0.08]); % 平面6中心

% 定义平面方程列表
plane_list = {
    -i_ + E_*dot(-i_, p1_center),  % 平面1
    i_ + E_*dot(i_, p2_center),    % 平面2
    -j_ + E_*dot(-j_, p3_center),  % 平面3
    j_ + E_*dot(j_, p4_center),    % 平面4
    -k_ + E_*dot(-k_, p5_center),  % 平面5
    k_ + E_*dot(k_, p6_center)     % 平面6
};

% 初始化机器人
VS050Robot = VS050RobotDH.kinematics();  % 创建VS050机器人运动学模型
VS050Robot.set_effector(1 + 0.5 * E_ * (0.2 * k_));  % 设置末端执行器（附加一段轴）
q = [0, pi/3, pi/3, 0, pi/3, 0]';  % 初始关节角度

% 初始化QP求解器和控制器
qp_solver = DQ_QuadprogSolver();  % 定义二次规划求解器
translation_controller = DQ_ClassicQPController(VS050Robot, qp_solver); % 创建经典QP控制器
translation_controller.set_control_objective(ControlObjective.Translation); % 设置控制目标为平移
translation_controller.set_gain(10);  % 设置增益
translation_controller.set_damping(1);  % 设置阻尼

% 定义虚拟力场增益和目标位置
eta_d = 1;  % 虚拟力场增益
td_list = { % 定义目标点列表
    0.60 * i_ + 0.08 * k_, 
    0.20 * i_ + 0.15 * k_, 
    0.45 * i_ + 0.08 * k_, 
    0.45 * i_ + 0.0 * k_
};

% 仿真参数
tau = 0.01;  % 采样时间
final_time = 4;  % 每个目标点的仿真时长

% 准备存储误差、时间和与平面距离的变量
total_steps = length(0:tau:final_time) * length(td_list); % 总的时间步长
stored_t_error = zeros(3, total_steps);  % 存储三维任务误差
stored_time = zeros(1, total_steps);    % 存储仿真时间
all_distances_to_planes = zeros(6, total_steps);  % 存储与六个平面的距离

step_counter = 1;% 初始化 step_counter

% 按目标点依次进行仿真
for td_counter = 1:4
    td = td_list{td_counter};  % 当前目标点
    
    % 针对当前目标点的控制循环
    for time = 0:tau:final_time
        [W, w, distances] = compute_inequalities(VS050Robot, q, plane_list, eta_d); % 计算不等式约束
        translation_controller.set_inequality_constraint(W, w); % 设置约束
        
        % 计算误差并存储
        t_error = vec3(translation(VS050Robot.fkm(q)) - td); % 计算任务误差
        stored_t_error(:, step_counter) = t_error; % 存储误差
        stored_time(step_counter) = time + (td_counter-1) * final_time; % 存储时间
        all_distances_to_planes(:, step_counter) = distances;  % 存储与平面的距离
        
        % 计算控制信号并更新关节角
        u = translation_controller.compute_setpoint_control_signal(q, vec4(td)); % 计算控制信号
        q = q + u * tau; % 更新关节状态

        % 增加 step_counter
        step_counter = step_counter + 1;
    end
end

% 绘制任务误差曲线
figure;
hold on;
plot(stored_time, stored_t_error(1,:), 'r', 'LineWidth', 1.5); % x轴误差（红色）
plot(stored_time, stored_t_error(2,:), 'g', 'LineWidth', 1.5); % y轴误差（绿色）
plot(stored_time, stored_t_error(3,:), 'b', 'LineWidth', 1.5); % z轴误差（蓝色）
hold off;
title('任务误差曲线');
xlabel('时间 [s]');
ylabel('任务误差 [m]');
legend({'x轴误差', 'y轴误差', 'z轴误差'});
grid on;
box off;

% 绘制与平面距离的子图
figure;
for i = 1:6
    subplot(2, 3, i);
    plot(stored_time, all_distances_to_planes(i, :), 'LineWidth', 1.5); % 绘制每个平面的距离
    title(['到平面 ' num2str(i) ' 的距离']);
    xlabel('时间 [s]');
    ylabel('距离 [m]');
    grid on;
end

toc; % 记录程序运行时间的结束点

% 定义计算不等式约束的函数
function [W, w, distances] = compute_inequalities(VS050Robot, q, plane_list, eta_d)
    W = []; % 初始化不等式约束矩阵W
    w = []; % 初始化不等式约束向量w
    distances = zeros(6, 1);  % 初始化到六个平面的距离数组
    
    Jx = VS050Robot.pose_jacobian(q); % 计算位姿雅可比矩阵
    x = VS050Robot.fkm(q); % 计算当前位姿
    Jt = DQ_Kinematics.translation_jacobian(Jx, x); % 提取平移部分的雅可比矩阵
    t = translation(x); % 提取当前位姿的平移部分
    
    % 针对每个平面计算约束和距离
    for plane_counter = 1:length(plane_list)
        workspace_plane = plane_list{plane_counter}; % 当前平面
        Jp_pi = DQ_Kinematics.point_to_plane_distance_jacobian(Jt, t, workspace_plane); % 计算点到平面的雅可比
        dp_pi = DQ_Geometry.point_to_plane_distance(t, workspace_plane); % 计算点到平面的距离
        W = [W; -Jp_pi]; % 更新约束矩阵
        w = [w; eta_d * dp_pi]; % 更新约束向量
        distances(plane_counter) = dp_pi; % 存储距离
    end
end

在这里插入图片描述

`vs050_entry_sphere_constraint.m`

创建一个平移控制器，其中 $\tau = 0.01$ 。选择其他控制器参数，使得控制平滑（参见本课和之前的课程示例）。
考虑入口球体的中心位于 $p_2 = 0.45\hat{i} + 0.2\hat{k}$ 。考虑入口球体的半径为5 mm。
在控制过程中，使用线到点的虚拟力（VFIs）来保持轴始终在入口球体内。
依次将末端执行器移动到以下四个期望位置：
$t_d = \left\{0.45\hat{i} + 0.03\hat{k}, 0.48\hat{i} + 0.03\hat{k}, 0.41\hat{i} + 0.03\hat{k}, 0.40\hat{i} + 0.01\hat{k}\right\}$
每个位置持续4个仿真时间秒。
在一个图中绘制任务误差。
在另一个图中绘制轴到入口球体中心的距离。

tic; % 开始计时
clear; clc; close all; % 清理工作区，关闭所有图形窗口
include_namespace_dq; % 包含双四元数命名空间

% 初始化中心点位置
p = 0.45*i_ + 0.2*k_; % 中心点坐标
d_safe = 0.005; % 安全距离（单位：米）

% 机器人配置
VS050Robot = VS050RobotDH.kinematics(); % 初始化 VS050 机器人
VS050Robot.set_effector(1 + 0.5 * E_ * (0.2 * k_)); % 设置机器人末端执行器
q = [0, pi/3, pi/3, 0, pi/3, 0]'; % 初始关节角度（单位：弧度）

% 解算器和控制器配置
qp_solver = DQ_QuadprogSolver(); % 初始化二次规划解算器
translation_controller = DQ_ClassicQPController(VS050Robot, qp_solver); % 初始化经典二次规划控制器
translation_controller.set_control_objective(ControlObjective.Translation); % 设置为平移控制目标
translation_controller.set_gain(10); % 设置增益
translation_controller.set_damping(1); % 设置阻尼系数

% 虚拟力场增益和目标位置列表
eta_d = 1; % 虚拟力场增益
td_list = {0.45 * i_ + 0.03 * k_, 0.48 * i_ + 0.03 * k_, 0.41 * i_ + 0.03 * k_, 0.40 * i_ + 0.01 * k_}; % 目标点列表

% 仿真参数
tau = 0.01; % 采样时间（单位：秒）
final_time = 4; % 每个目标点的仿真时间（单位：秒）

% 预分配存储任务误差、时间和距离的数组
total_steps = length(0:tau:final_time) * length(td_list); % 总步数
stored_t_error = zeros(3, total_steps); % 存储任务误差（x、y、z 三维）
stored_time = zeros(1, total_steps); % 存储时间
all_distances = zeros(1, total_steps); % 存储距离
step_counter = 0; % 初始化步数计数器

for td_counter = 1:4 % 遍历目标点
    td = td_list{td_counter}; % 当前目标点
    
    % 对每个目标点执行平移控制
    for time = 0:tau:final_time
        step_counter = step_counter + 1; % 增加步数计数器
        
        % 计算姿态雅可比矩阵和姿态
        Jx = VS050Robot.pose_jacobian(q); % 获取雅可比矩阵
        x = VS050Robot.fkm(q); % 正向运动学计算当前位置
        
        % 计算 k 轴的直线雅可比矩阵
        Jl = DQ_Kinematics.line_jacobian(Jx, x, k_);
        
        % 计算相对于基坐标的直线
        t = translation(x); % 平移向量
        r = rotation(x); % 旋转矩阵
        l = Ad(r, k_); % 直线方向
        l_dq = l + E_ * cross(t, l); % 双四元数形式的直线
        
        % 计算点到直线的距离雅可比矩阵
        Jl_p = DQ_Kinematics.line_to_point_distance_jacobian(Jl, l_dq, p);
        
        % 计算点到直线的平方距离
        Dl_p = DQ_Geometry.point_to_line_squared_distance(p, l_dq);
        
        % 计算距离误差
        D_safe = d_safe^2; % 安全距离的平方
        D_tilde = D_safe - Dl_p; % 距离误差

        % 存储误差和距离
        t_error = vec3(translation(VS050Robot.fkm(q)) - td); % 计算任务误差
        stored_t_error(:, step_counter) = t_error; % 存储任务误差
        stored_time(step_counter) = time + (td_counter - 1) * final_time; % 存储时间
        all_distances(:, step_counter) = sqrt(Dl_p); % 存储实际距离
        
        % 设置不等式约束
        W = Jl_p; % 不等式约束矩阵
        w = eta_d * D_tilde; % 不等式约束向量
        translation_controller.set_inequality_constraint(W, w); % 更新控制器约束
        
        % 计算控制信号
        u = translation_controller.compute_setpoint_control_signal(q, vec4(td)); % 计算控制输入
        
        % 更新关节位置
        q = q + u * tau; % 按采样时间步进
    end
end

 % 绘制任务误差曲线
figure;
hold on;
plot(stored_time, stored_t_error(1,:), 'r', 'LineWidth', 1.5); % 绘制 x 轴误差
plot(stored_time, stored_t_error(2,:), 'g', 'LineWidth', 1.5); % 绘制 y 轴误差
plot(stored_time, stored_t_error(3,:), 'b', 'LineWidth', 1.5); % 绘制 z 轴误差
hold off;
title('任务误差曲线');
xlabel('时间 [秒]');
ylabel('误差 [米]');
legend({'x 轴误差', 'y 轴误差', 'z 轴误差'});
grid on;

% 绘制距离曲线
figure;
plot(stored_time, all_distances, 'LineWidth', 1.5); % 绘制距离
title('末端轴到入口球体中心的距离');
xlabel('时间 [秒]');
ylabel('距离 [米]');
grid on;

toc; % 结束计时

在这里插入图片描述

生成动画

平面距离约束，点到面距离约束

在这里插入图片描述

for td_counter = 1:4
    td = td_list{td_counter};
     
    % Translation controller loop for each target
    for time = 0:tau:final_time
        
        
        [W, w, distances] = compute_inequalities(VS050Robot, q, plane_list, eta_d);
        translation_controller.set_inequality_constraint(W, w);
        step_counter = step_counter +1;
        % 记录误差
        t_error = vec3(translation(VS050Robot.fkm(q)) - td);
        stored_t_error(:, step_counter) = t_error;
        stored_time(step_counter) = time + (td_counter-1) * final_time;
        all_distances_to_planes(:, step_counter) = distances;  % 存储距离
        
        % 计算控制信号并更新关节状态
        u = translation_controller.compute_setpoint_control_signal(q, vec4(td));
        q = q + u * tau;

        % Plot
        % Plot the robot
        plot(VS050Robot,q);
        title(['td' num2str(td_counter) '----Translation control' ' time = ' num2str(time) 's out of ' num2str(final_time) 's'])
        % Plot the desired pose
        hold on
        plot3(td.q(2),td.q(3),td.q(4), 'ko');
        % % Plot the shaft in blue
        t_1 = translation(VS050Robot.raw_fkm(q));
        t_2 = translation(VS050Robot.fkm(q));
        plot3([t_1.q(2) t_2.q(2)],[t_1.q(3) t_2.q(3)],[t_1.q(4) t_2.q(4)],'g','LineWidth',2)
        % Plot the walls
        plot(plane_list{1}, 'plane', 0.10, 'location', p1_center);  
        plot(plane_list{2}, 'plane', 0.10, 'location', p2_center); 
        plot(plane_list{3}, 'plane', 0.10, 'location', p3_center);  
        plot(plane_list{4}, 'plane', 0.10, 'location', p4_center); 
        plot(plane_list{5}, 'plane', 0.10, 'location', p5_center);  
        plot(plane_list{6}, 'plane', 0.10, 'location', p6_center); 
        % Define the 8 vertices of a cube with side length 5 (from -2.5 to 2.5)
        vertices = 0.01.*[
            -2.5, -2.5, -2.5;
            2.5, -2.5, -2.5;
            2.5, 2.5, -2.5;
            -2.5, 2.5, -2.5;
            -2.5, -2.5, 2.5;
            2.5, -2.5, 2.5;
            2.5, 2.5, 2.5;
            -2.5, 2.5, 2.5
        ]+repmat([0.45,0,0.08],8,1);
        % Define the edges connecting the vertices to form the cube's edges
        edges = [
            1, 2; 2, 3; 3, 4; 4, 1;   % Bottom square
            5, 6; 6, 7; 7, 8; 8, 5;   % Top square
            1, 5; 2, 6; 3, 7; 4, 8    % Vertical edges connecting top and bottom
        ];
        % Plot the edges
        for i = 1:size(edges, 1)
            plot3([vertices(edges(i, 1), 1), vertices(edges(i, 2), 1)], ...
                  [vertices(edges(i, 1), 2), vertices(edges(i, 2), 2)], ...
                  [vertices(edges(i, 1), 3), vertices(edges(i, 2), 3)], 'k-', 'LineWidth', 2);
        end
        % Add grid and labels
        grid on
        xlabel('X-axis');
        ylabel('Y-axis');
        zlabel('Z-axis');
        % Set the view to a 3D perspective
        view(3);

        hold off
        % [For animations only]
        drawnow limitrate % [For animations only] Ask MATLAB to draw the plot now
    end
end

入口球体，点到线距离约束

在这里插入图片描述

for td_counter = 1:4
    td = td_list{td_counter};
    
    % Translation controller loop for each target
    for time = 0:tau:final_time
        step_counter = step_counter + 1; % 记录当前步数
        
        % Get the pose Jacobian and the pose
        Jx = VS050Robot.pose_jacobian(q);
        x = VS050Robot.fkm(q);
        
        % Get the line Jacobian for the k-axis
        Jl = DQ_Kinematics.line_jacobian(Jx, x, k_);
        
        % Get the line with respect to the base
        t = translation(x);
        r = rotation(x);
        l = Ad(r, k_);
        l_dq = l + E_*cross(t, l);
        
        % Get the line-to-point distance Jacobian
        Jl_p = DQ_Kinematics.line_to_point_distance_jacobian(Jl, l_dq, p);
        
        % Get the line-to-point square distance
        Dl_p = DQ_Geometry.point_to_line_squared_distance(p, l_dq);
        
        % Get the distance error
        D_safe = d_safe^2;
        D_tilde = D_safe - Dl_p;

        % 记录误差
        t_error = vec3(translation(VS050Robot.fkm(q)) - td);
        stored_t_error(:, step_counter) = t_error;
        stored_time(step_counter) = time + (td_counter-1) * final_time;
        all_distances(:, step_counter) = sqrt(Dl_p);
        
        % The inequality matrix and vector
        W = Jl_p;
        w = eta_d*D_tilde;
        
        % Update the linear inequalities in the controller
        translation_controller.set_inequality_constraint(W, w);
        
        % Get the next control signal [rad/s]
        u = translation_controller.compute_setpoint_control_signal(q,vec4(td));
        
        % Move the robot
        q = q + u*tau;

        % Clear plot
        plot(VS050Robot, q)
        clf(f)
        % Plot the robot
        plot(VS050Robot, q)
        view(3)
        title(['target td' num2str(td_counter) '---Translation control' ' time = ' num2str(time) 's out of ' num2str(final_time) 's'])
        hold on
        % Plot the desired pose
        hold on
        plot3(td.q(2),td.q(3),td.q(4), 'ko');
        % Plot the shaft in blue
        t_1 = translation(VS050Robot.raw_fkm(q));
        t_2 = translation(VS050Robot.fkm(q));
        plot3([t_1.q(2) t_2.q(2)],[t_1.q(3) t_2.q(3)],[t_1.q(4) t_2.q(4)],'b','LineWidth',2)
        % Plot the entry sphere
        % 绘制一个半径为 5 mm 的球体
        [x, y, z] = sphere(50); % 生成球体数据，50 表示分辨率
        sphere_radius = d_safe; % 球体半径为 5 mm
        x = sphere_radius * x + p.q(2); % 球体中心偏移
        y = sphere_radius * y + p.q(3);
        z = sphere_radius * z + p.q(4);

        % 使用 surf 绘制球体
        surf(x, y, z, 'FaceAlpha', 0.3, 'EdgeColor', 'none', 'FaceColor', 'r'); % 半透明红色
        axis equal; % 固定坐标轴比例
        grid on; % 启用网格线方便观察


        % [For animations only]
        drawnow limitrate % [For animations only] Ask MATLAB to draw the plot now
    end
end