【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 VS050RobotDHmethods (Static)function ret = kinematics()%kinematics returns the kinematics of the Denso VS050 robot as DQ_SerialManipulatorDHDH_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";endend
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),  % 平面1i_ + E_*dot(i_, p2_center),    % 平面2-j_ + E_*dot(-j_, p3_center),  % 平面3j_ + E_*dot(j_, p4_center),    % 平面4-k_ + E_*dot(-k_, p5_center),  % 平面5k_ + 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:4td = 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_counterstep_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:6subplot(2, 3, i);plot(stored_time, all_distances_to_planes(i, :), 'LineWidth', 1.5); % 绘制每个平面的距离title(['到平面 ' num2str(i) ' 的距离']);xlabel('时间 [s]');ylabel('距离 [m]');grid on;
endtoc; % 记录程序运行时间的结束点% 定义计算不等式约束的函数
function [W, w, distances] = compute_inequalities(VS050Robot, q, plane_list, eta_d)W = []; % 初始化不等式约束矩阵Ww = []; % 初始化不等式约束向量wdistances = 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_timestep_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:4td = td_list{td_counter};% Translation controller loop for each targetfor 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 robotplot(VS050Robot,q);title(['td' num2str(td_counter) '----Translation control' ' time = ' num2str(time) 's out of ' num2str(final_time) 's'])% Plot the desired posehold onplot3(td.q(2),td.q(3),td.q(4), 'ko');% % Plot the shaft in bluet_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 wallsplot(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 edgesedges = [1, 2; 2, 3; 3, 4; 4, 1;   % Bottom square5, 6; 6, 7; 7, 8; 8, 5;   % Top square1, 5; 2, 6; 3, 7; 4, 8    % Vertical edges connecting top and bottom];% Plot the edgesfor 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 labelsgrid onxlabel('X-axis');ylabel('Y-axis');zlabel('Z-axis');% Set the view to a 3D perspectiveview(3);hold off% [For animations only]drawnow limitrate % [For animations only] Ask MATLAB to draw the plot nowend
end

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

在这里插入图片描述

for td_counter = 1:4td = td_list{td_counter};% Translation controller loop for each targetfor time = 0:tau:final_timestep_counter = step_counter + 1; % 记录当前步数% Get the pose Jacobian and the poseJx = VS050Robot.pose_jacobian(q);x = VS050Robot.fkm(q);% Get the line Jacobian for the k-axisJl = DQ_Kinematics.line_jacobian(Jx, x, k_);% Get the line with respect to the baset = translation(x);r = rotation(x);l = Ad(r, k_);l_dq = l + E_*cross(t, l);% Get the line-to-point distance JacobianJl_p = DQ_Kinematics.line_to_point_distance_jacobian(Jl, l_dq, p);% Get the line-to-point square distanceDl_p = DQ_Geometry.point_to_line_squared_distance(p, l_dq);% Get the distance errorD_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 vectorW = Jl_p;w = eta_d*D_tilde;% Update the linear inequalities in the controllertranslation_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 robotq = q + u*tau;% Clear plotplot(VS050Robot, q)clf(f)% Plot the robotplot(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 posehold onplot3(td.q(2),td.q(3),td.q(4), 'ko');% Plot the shaft in bluet_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 mmx = 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 nowend
end