长安东莞网站推广,软件开发定制费用,邯郸房产信息网查询系统,农村电子商务网站建设方案Lesson 5 代码-TwoDofPlanarRobot.m
表示一个 2 自由度平面机器人。该类包含构造函数、计算正向运动学模型的函数、计算平移雅可比矩阵的函数#xff0c;以及在二维空间中绘制机器人的函数。
classdef TwoDofPlanarRobot%TwoDofPlanarRobot - 表示一个 2 自由度平面机器人类…Lesson 5 代码-TwoDofPlanarRobot.m
表示一个 2 自由度平面机器人。该类包含构造函数、计算正向运动学模型的函数、计算平移雅可比矩阵的函数以及在二维空间中绘制机器人的函数。
classdef TwoDofPlanarRobot%TwoDofPlanarRobot - 表示一个 2 自由度平面机器人类properties (Accessprivate)% 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(theta1theta2) j_*cos(theta1theta2));j2 obj.l2*(-i_*sin(theta1theta2) j_*cos(theta1theta2));% 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 oplot(0, 0, ko, MarkerSize, 8, MarkerFaceColor, k)% Mark the end effector with an xplot(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 plotendDQ(1) 表示创建了一个特殊的双四元数对象用来初始化误差和误差导数。这一初始化操作仅仅是为了确保控制循环在开始时能正确进入不会因为初始误差太小而导致循环直接退出。 DQ(1) 在 DQ Robotics 中表示一个双四元数初始值为 1。它是单位双四元数通常表示没有旋转或平移即一个默认、标准的双四元数。这一数值用于初始化误差和误差导数并不意味着其实际值很大而是让程序有一个初始化的起点。 在 DQ Robotics 中通过给 t_error 和 t_error_dot 初始化为 DQ(1)可以确保循环一开始不会因为误差值太小而退出。只要在后续的计算中计算出真实的误差循环即可继续执行。
作用总结
DQ(1) 的作用是初始化不会因为值太小而导致不进入循环代码后续将实际误差值更新为 t_error 和 t_error_dot从而让控制逻辑正常工作。
