% ---------------------------------------------------------------------

% In this script trajectory optimization otherwise called experiment

% design for dynamic paramters identification is carried out.?

% 在該腳本中，進(jìn)行了軌跡優(yōu)化，也稱為動(dòng)態(tài)參數(shù)識(shí)別的實(shí)驗(yàn)設(shè)計(jì)。

% First, specify cost function (traj_cost_lgr) and constraints?

% (traj_cnstr) for the optimziation. Then choose oprimization algorithm and

% specify trajectory parameters (duration, fundamental frequency, number of?

% harmonics, initial (= final) positionin) and max/min positions,

% velocities and accelerations.

% 首先，為優(yōu)化指定成本函數(shù)（traj_cost_lgr）和約束（traj_cnstr）。然后選擇優(yōu)化算法并指定

% 軌跡參數(shù)（持續(xù)時(shí)間、基頻、諧波數(shù)、初始（=最終）位置）和最大/最小位置、速度和加速度。

%

% Then script runs optimization, plots obtained trajectory and saves its

% parameters into a file.

% ---------------------------------------------------------------------

% get robot description

path_to_urdf = 'ur10e.urdf';

ur10 = parse_urdf(path_to_urdf);

% get mapping from full parameters to base parameters

include_motor_dynamics = 1;

[~, baseQR] = base_params_qr(include_motor_dynamics);

% Choose optimization algorithm: 'patternsearch', 'ga'

optmznAlgorithm = 'patternsearch';

% Trajectory parameters

traj_par.T = 30;? ? ? ? ? % period of signal? ?信號(hào)周期

traj_par.wf = 2*pi/traj_par.T;? ? % fundamental frequency? 基頻

traj_par.t_smp = 1e-2;? ? % sampling time? ? ? 采樣時(shí)間10ms,10ms下發(fā)一個(gè)關(guān)節(jié)位置

traj_par.t = 0:traj_par.t_smp:traj_par.T;? ?% time 時(shí)間

traj_par.N = 8;? ? ? ? ? ?% number of harmonics 諧波數(shù)

traj_par.q0 = deg2rad([0 -90 0 -90 -90 0 ]'); % 初始位型,★★☆☆機(jī)械臂的零位型

% Use different limit for positions for safety

traj_par.q_min = deg2rad([-180? -150? -90? -180? -90? -90]');

traj_par.q_max = deg2rad([ 180? ?-30? ?90? ? ?0? ?90? ?90]');

qd_max = deg2rad(120)*ones(6,1);? ?% deg2rad(120)=2.09 designed by xingjia 儲(chǔ)存在N8T30QR

traj_par.qd_max = qd_max;?

traj_par.q2d_max = [2 1 1 1 1 2]';