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_error
和t_error_dot
初始化为DQ(1)
,可以确保循环一开始不会因为误差值太小而退出。只要在后续的计算中计算出真实的误差,循环即可继续执行。
作用总结
DQ(1)
的作用是初始化,不会因为值太小而导致不进入循环;- 代码后续将实际误差值更新为
t_error
和t_error_dot
,从而让控制逻辑正常工作。