轨迹参数优化

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

% In this script trajectory optimization otherwise called experiment

% design for dynamic paramters identification is carried out. 

% 在该脚本中，进行了轨迹优化，也称为动态参数识别的实验设计。

% 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.

% 首先，为优化指定成本函数（traj_cost_lgr）和约束（traj_cnstr）。然后选择优化算法并指定

% 轨迹参数（持续时间、基频、谐波数、初始（=最终）位置）和最大/最小位置、速度和加速度。

%

% 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   信号周期

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

traj_par.t_smp = 1e-2;    % sampling time      采样时间10ms,10ms下发一个关节位置

traj_par.t = 0:traj_par.t_smp:traj_par.T;   % time 时间

traj_par.N = 8;           % number of harmonics 谐波数

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

% 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 储存在N8T30QR

traj_par.qd_max = qd_max; 

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