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

Lesson 6

代码

(1) 创建 NDofPlanarRobotDH.m 类

该类用于创建任意自由度的平面机器人，所有连杆长度设为单位长度，并包含一个 kinematics 方法，可以接受所需的自由度数量并返回相应的 DQ_SerialManipulatorDH 实例。

classdef NDofPlanarRobotDH
    %NDofPlanarRobotDH 用于表示任意自由度的平面机器人
    
    methods (Static)
        function ret = kinematics(n)
            %kinematics 返回指定自由度的DQ_SerialManipulatorDH实例
            DH_theta = zeros(1, n);
            DH_d = zeros(1, n);
            DH_a = ones(1, n); % 所有连杆长度均为1
            DH_alpha = zeros(1, n);
            DH_type = repmat(DQ_SerialManipulatorDH.JOINT_ROTATIONAL, 1, n);
            
            DH_matrix = [DH_theta;
                         DH_d;
                         DH_a;
                         DH_alpha;
                         DH_type];
            
            ret = DQ_SerialManipulatorDH(DH_matrix, 'standard');
            ret.name = [num2str(n) ' DoF Planar Robot'];
        end
    end
end

(2) 实现 7 自由度平面机器人的平移、旋转和姿态控制

平移控制器脚本：seven_dof_planar_robot_translation_control.m

clear all;
close all;
include_namespace_dq

% 期望的平移和旋转
td = 5*i_ + 2*j_;
gamma = -pi / 4;
rd = cos(gamma / 2) + k_ * sin(gamma / 2);
xd = rd + 0.5 * E_ * td * rd;

% 初始关节值 [弧度]
theta_init = pi / 8 * ones(7, 1);

% 实例化7自由度机器人
seven_dof_robot = NDofPlanarRobotDH.kinematics(7);

% 创建平移控制器
translation_controller = DQ_PseudoinverseController(seven_dof_robot);
translation_controller.set_control_objective(ControlObjective.Translation);
translation_controller.set_gain(5.0);
translation_controller.set_damping(0);

% 平移控制器循环
q = theta_init;
tau = 0.01;
final_time = 1;
for time = 0:tau:final_time
    % 获取控制信号
    u = translation_controller.compute_setpoint_control_signal(q, vec4(td));
    q = q + u * tau;

    % 绘图
    seven_dof_robot.plot(q);
    title(['平移控制，时间=' num2str(time) 's']);
    hold on;
    plot(xd);
    hold off;
    drawnow;
end

旋转控制器脚本：seven_dof_planar_robot_rotation_control.m

clear all;
close all;
include_namespace_dq

% 期望的平移和旋转
td = 5*i_ + 2*j_;
gamma = -pi / 4;
rd = cos(gamma / 2) + k_ * sin(gamma / 2);
xd = rd + 0.5 * E_ * td * rd;

% 初始关节值 [弧度]
theta_init = pi / 8 * ones(7, 1);

% 实例化7自由度机器人
seven_dof_robot = NDofPlanarRobotDH.kinematics(7);

% 创建旋转控制器
rotation_controller = DQ_PseudoinverseController(seven_dof_robot);
rotation_controller.set_control_objective(ControlObjective.Rotation);
rotation_controller.set_gain(5.0);
rotation_controller.set_damping(0);

% 旋转控制器循环
q = theta_init;
tau = 0.01;
final_time = 1;
for time = 0:tau:final_time
    % 获取控制信号
    u = rotation_controller.compute_setpoint_control_signal(q, vec4(rd));
    q = q + u * tau;

    % 绘图
    seven_dof_robot.plot(q);
    title(['旋转控制，时间=' num2str(time) 's']);
    hold on;
    plot(xd);
    hold off;
    drawnow;
end

姿态控制器脚本：seven_dof_planar_robot_pose_control.m

clear all;
close all;
include_namespace_dq

% 期望的平移和旋转
td = 5*i_ + 2*j_;
gamma = -pi / 4;
rd = cos(gamma / 2) + k_ * sin(gamma / 2);
xd = rd + 0.5 * E_ * td * rd;

% 初始关节值 [弧度]
theta_init = pi / 8 * ones(7, 1);

% 实例化7自由度机器人
seven_dof_robot = NDofPlanarRobotDH.kinematics(7);

% 创建姿态控制器
pose_controller = DQ_PseudoinverseController(seven_dof_robot);
pose_controller.set_control_objective(ControlObjective.Pose);
pose_controller.set_gain(5.0);
pose_controller.set_damping(0);

% 姿态控制器循环
q = theta_init;
tau = 0.01;
final_time = 1;
for time = 0:tau:final_time
    % 获取控制信号
    u = pose_controller.compute_setpoint_control_signal(q, vec8(xd));
    q = q + u * tau;

    % 绘图
    seven_dof_robot.plot(q);
    title(['姿态控制，时间=' num2str(time) 's']);
    hold on;
    plot(xd);
    hold off;
    drawnow;
end