Trajectory tracking

[CJ0510]

Dear author, I encountered some problems when using this repository. Mainly in the tracking of arc trajectories. Can this repository realize the tracking of arc trajectories in space? Can you give an example?Have a good day！

[BJCaasenbrood]

He, can you show me the issue? In theory, a single PCC segment should be able to reach any position in R3.

Thus, yes, an arc should be possible kinematically.

[BJCaasenbrood]

@CJ0510 do you still require help with the issue?

[CJ0510]

%% 圆弧轨迹生成
t_end = 15; % 总时间
t = linspace(0, t_end, t_end * 30); % 时间向量，假设频率为30Hz
radius = 0.1; % 圆弧的半径
center = [0.1, 0, 0.05]; % 圆弧的中心位置
omega = pi / t_end; % 角速度

% 圆弧轨迹
qd_arc = [center(1) + radius * cos(omega * t);
center(2) + radius * sin(omega * t);
center(3) * ones(1, length(t));
zeros(9, length(t))]; % 其他关节角度保持为零

%% generate model class
mdl = Model(4);

%% settings
mdl = mdl.set('Phi0',rotx(pi),'Tsim',15);
mdl = mdl.setElements(60);
mdl = mdl.setFrequency(30);
mdl = mdl.setLength(0.065);

% 将轨迹转置以适应时间步
qd_arc = qd_arc.';

%% 使用 setControl 函数设置控制器
mdl = setControl(mdl, @(mdl) ArcController(mdl, qd_arc, t));

%% 运行仿真
mdl = mdl.simulate;

%% 绘制仿真结果
figure(102);
P_values = zeros(length(t), 3); % 预分配数组大小，用于存储各个时刻的末端位置 P 值
t_values = zeros(length(t), 1); % 存储时间值

for ii = 1:length(t)
figure(102); cla;
mdl.show(mdl.q(ii,:), col(1));
mdl.show(qd_arc(ii,:), col(2));

groundplane(0.02);
axis equal; axis(0.2*[-0.75 0.75 -0.75 0.75 -1.5 0.1]);
view(30, 30); grid on; box on; drawnow(); 

end

%% 圆弧跟踪控制器
function tau = ArcController(mdl, qd_arc, t)
Kp = 1e-4 * eye(12);
Kd = 5e-5 * eye(12);
% 计算当前时间的目标位置
current_time = mdl.t;
[~, idx] = min(abs(t - current_time)); % 找到最接近当前时间的轨迹点索引
qd = qd_arc(idx, :)'; % 获取当前时间点的目标位置

tau = mdl.G + mdl.K * (mdl.q) - Kp * (mdl.q - qd) - Kd * (mdl.dq);

end
This is my code,but it does not work.You are a really good author