机器人初始位置为[0° 0° 0m]T期望位置为[30° 60° 0.15m]T ，初始和期望关节速度、加速度均为0。机械手末端的期望刚性和阻尼参数为Kx=diag（[500,500,500]）,Kb=diag（[50,50,30]） 设计阻抗控制器，绘制出关节位置、速度、加速度以及控制力矩曲线。提供matlab代码

下面是机器人阻抗控制器的MATLAB代码，其中假设机器人为三自由度，末端质量为1千克：

%% 机器人阻抗控制器
clear all;
close all;

%% 机器人参数
m = 1; % 末端质量
g = 9.81; % 重力加速度
L1 = 0.3; % 第一根臂长
L2 = 0.2; % 第二根臂长
L3 = 0.1; % 第三根臂长

%% 初始位置和期望位置
q0 = [0 0 0]'; % 初始位置
qe = [30 60 0.15]' * pi / 180; % 期望位置

%% 初始速度和期望速度
dq0 = [0 0 0]'; % 初始速度
dqe = [0 0 0]'; % 期望速度

%% 初始加速度和期望加速度
ddq0 = [0 0 0]'; % 初始加速度
ddqe = [0 0 0]'; % 期望加速度

%% 控制参数
Kx = diag([500 500 500]); % 位置刚性
Kb = diag([50 50 30]); % 姿态刚性
Dx = 2 * sqrt(Kx * m); % 位置阻尼
Db = 2 * sqrt(Kb * m); % 姿态阻尼

%% 时间参数
T = 10; % 控制时间
dt = 0.01; % 时间步长
t = 0:dt:T; % 时间向量
N = length(t); % 时间步数

%% 初始化变量
q = zeros(3, N); % 关节位置
dq = zeros(3, N); % 关节速度
ddq = zeros(3, N); % 关节加速度
F = zeros(3, N); % 控制力矩

%% 控制器
for i = 1:N
    % 当前时间
    ti = t(i);
    
    % 期望位置和姿态
    xe = [qe(1) + 0.01 * sin(2 * pi * ti / T);
          qe(2) + 0.01 * sin(2 * pi * ti / T + pi / 3);
          qe(3)];
    Re = rpy2r([0 0 0]) * rpy2r([xe(1) xe(2) xe(3)]);
    
    % 当前位置和姿态
    x = fk(q(:, i), L1, L2, L3);
    R = rpy2r([0 0 0]) * rpy2r(q(:, i));
    
    % 误差
    dx = x - xe;
    dR = 0.5 * (R' * Re - Re' * R);
    dtheta = r2rpy(dR)';
    d = [dx; dtheta];
    
    % 期望加速度
    ddxe = [ddqe(1) + 0.01 * (2 * pi / T)^2 * cos(2 * pi * ti / T);
            ddqe(2) + 0.01 * (2 * pi / T)^2 * cos(2 * pi * ti / T + pi / 3);
            ddqe(3)];
    dde = [ddxe; zeros(3, 1)];
    
    % 动力学模型
    H = jacobian(q(:, i), L1, L2, L3)' * diag([m m m]) * jacobian(q(:, i), L1, L2, L3);
    C = coriolis(q(:, i), dq(:, i), L1, L2, L3);
    G = gravity(q(:, i), L1, L2, L3, g, m);
    
    % 控制力矩
    F(:, i) = H * (dde + Kb * dtheta + Db * (dq(:, i) - dqe) + Kx * dx + Dx * (q(:, i) - qe)) + C * dq(:, i) + G;
    
    % 关节加速度
    ddq(:, i) = H \ (F(:, i) - C * dq(:, i) - G);
    
    % 关节速度和位置
    if i < N
        dq(:, i + 1) = dq(:, i) + ddq(:, i) * dt;
        q(:, i + 1) = q(:, i) + dq(:, i + 1) * dt;
    end
end

%% 绘制关节位置、速度、加速度、控制力矩曲线
figure;
subplot(4, 1, 1);
plot(t, q(1, :) * 180 / pi, 'r', 'LineWidth', 2);
ylabel('q_1 (deg)');
grid on;

subplot(4, 1, 2);
plot(t, q(2, :) * 180 / pi, 'g', 'LineWidth', 2);
ylabel('q_2 (deg)');
grid on;

subplot(4, 1, 3);
plot(t, q(3, :), 'b', 'LineWidth', 2);
ylabel('q_3 (m)');
grid on;

subplot(4, 1, 4);
plot(t, F(1, :), 'r', t, F(2, :), 'g', t, F(3, :), 'b', 'LineWidth', 2);
ylabel('F (N)');
xlabel('Time (s)');
grid on;

figure;
subplot(3, 1, 1);
plot(t, dq(1, :) * 180 / pi, 'r', 'LineWidth', 2);
ylabel('dq_1 (deg/s)');
grid on;

subplot(3, 1, 2);
plot(t, dq(2, :) * 180 / pi, 'g', 'LineWidth', 2);
ylabel('dq_2 (deg/s)');
grid on;

subplot(3, 1, 3);
plot(t, dq(3, :), 'b', 'LineWidth', 2);
ylabel('dq_3 (m/s)');
xlabel('Time (s)');
grid on;

figure;
subplot(3, 1, 1);
plot(t, ddq(1, :) * 180 / pi, 'r', 'LineWidth', 2);
ylabel('ddq_1 (deg/s^2)');
grid on;

subplot(3, 1, 2);
plot(t, ddq(2, :) * 180 / pi, 'g', 'LineWidth', 2);
ylabel('ddq_2 (deg/s^2)');
grid on;

subplot(3, 1, 3);
plot(t, ddq(3, :), 'b', 'LineWidth', 2);
ylabel('ddq_3 (m/s^2)');
xlabel('Time (s)');
grid on;

请注意，此代码仅提供了一个基本的实现，可能需要进行更多的调整和优化，以适应实际应用场景。