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

Lesson 5

代码-TwoDofPlanarRobot.m

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

classdef TwoDofPlanarRobot%TwoDofPlanarRobot - 表示一个 2 自由度平面机器人类properties (Access=private)% The lengths of the two links, in metersl1l2endmethodsfunction obj = TwoDofPlanarRobot(l1, l2)% TwoDofPlanarRobot creates a 2-DoF planar robot with link lengths l1 and l2obj.l1 = l1;obj.l2 = l2;endfunction 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 functioninclude_namespace_dq% Rotation and translation for the first linkx_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 linkx_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);endfunction Jt = translation_jacobian(obj, theta1, theta2)% Calculates the translation Jacobian of the 2-DoF planar robot.% Include the namespace inside the functioninclude_namespace_dq% Partial derivatives of the end effector positionj1 = 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 matrixJt = [vec3(j1), vec3(j2)];endfunction plot(obj, theta1, theta2)% Plot the 2-DoF planar robot in the xy-plane% Calculate the positions of each joint and the end effectorx1 = 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 linksplot([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 offtitle('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 onendend
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 translationt = two_dof_planar_robot.fkm(theta(1),theta(2));% Calculate the error and old errort_error_old = t_error;t_error = (t-td);t_error_dot = (t_error-t_error_old)/tau;% Get the translation jacobian, based on thetaJt = two_dof_planar_robot.translation_jacobian(theta(1),theta(2));% Calculate the IDKMtheta_dot = -eta*pinv(Jt)*vec3(t_error);% Move the robottheta = theta + theta_dot*tau;% Plot the robottwo_dof_planar_robot.plot(theta(1),theta(2))% Plot the desired positionhold onplot(tx,ty,'bx')hold off% [For animations only]drawnow; % [For animations only] Ask MATLAB to draw the plot nowpause(0.001) % [For animations only] Pause so that MATLAB has enough time to draw the plotend

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

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

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

作用总结

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