added file that check if optimized trajectory collides with table on which the robot is fixed. Moreover kinematics of UR is derived is analytical form, so it can be used in trajectory optimization in order to stay away from table, however is considerably slows down the optimization. Even on sever with 40 cpus patter search computes it for hours. Moreover, added files for identification of the pendubot.
This commit is contained in:
Shamil Mamedov
parent
141b563911
commit
fe3cc01102
|
|
@ -0,0 +1,21 @@
|
|||
function pos = position_fk_UR10E(in1)
|
||||
%POSITION_FK_UR10E
|
||||
% POS = POSITION_FK_UR10E(IN1)
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 8.2.
|
||||
% 31-Jan-2020 07:44:23
|
||||
|
||||
q1 = in1(1,:);
|
||||
q2 = in1(2,:);
|
||||
q3 = in1(3,:);
|
||||
q4 = in1(4,:);
|
||||
t2 = sin(1.57079632679);
|
||||
t3 = cos(q1);
|
||||
t4 = cos(q2);
|
||||
t5 = cos(q3);
|
||||
t6 = sin(q2);
|
||||
t7 = sin(q3);
|
||||
t8 = sin(q1);
|
||||
t9 = cos(q4);
|
||||
t10 = sin(q4);
|
||||
pos = [t8.*(-8.7e1./5.0e2)-t2.*t9.*(t2.*t3.*t4.*t7+t2.*t3.*t5.*t6).*(3.0./2.5e1)-t2.*t10.*(t2.*t3.*t4.*t5-t2.*t3.*t6.*t7).*(3.0./2.5e1)+t2.*t3.*t4.*(6.13e2./1.0e3)+t2.*t3.*t4.*t5.*(5.71e2./1.0e3)-t2.*t3.*t6.*t7.*(5.71e2./1.0e3);t3.*(8.7e1./5.0e2)-t2.*t9.*(t2.*t4.*t7.*t8+t2.*t5.*t6.*t8).*(3.0./2.5e1)-t2.*t10.*(t2.*t4.*t5.*t8-t2.*t6.*t7.*t8).*(3.0./2.5e1)+t2.*t4.*t8.*(6.13e2./1.0e3)+t2.*t4.*t5.*t8.*(5.71e2./1.0e3)-t2.*t6.*t7.*t8.*(5.71e2./1.0e3);t2.*t6.*(-6.13e2./1.0e3)-t2.*t9.*(t2.*t4.*t5-t2.*t6.*t7).*(3.0./2.5e1)+t2.*t10.*(t2.*t4.*t7+t2.*t5.*t6).*(3.0./2.5e1)-t2.*t4.*t7.*(5.71e2./1.0e3)-t2.*t5.*t6.*(5.71e2./1.0e3)+1.81e2./1.0e3];
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
BIN
driveGains.mat
BIN
driveGains.mat
Binary file not shown.
|
|
@ -3,8 +3,9 @@ function frctn = frictionRegressor(qd_fltrd)
|
|||
% The function computes friction regressor for each joint of the robot.
|
||||
% Fv*qd + Fc*sign(qd) + F0, and the second one is continous,
|
||||
% ---------------------------------------------------------------------
|
||||
frctn = zeros(6,18);
|
||||
for i = 1:6
|
||||
noJoints = size(qd_fltrd,1);
|
||||
frctn = zeros(noJoints, noJoints*3);
|
||||
for i = 1:noJoints
|
||||
frctn(i,3*i-2:3*i) = [qd_fltrd(i), sign(qd_fltrd(i)), 1];
|
||||
end
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,24 @@
|
|||
function pos = ur_fk(q, ur)
|
||||
|
||||
T_pk = zeros(4,4,6); % transformation between links
|
||||
T_0k = zeros(4,4,7); T_0k(:,:,1) = eye(4);
|
||||
for i = 1:6
|
||||
jnt_axs_k = str2num(ur.robot.joint{i}.axis.Attributes.xyz)';
|
||||
% Transformation from parent link frame p to current joint frame
|
||||
rpy_k = str2num(ur.robot.joint{i}.origin.Attributes.rpy);
|
||||
R_pj = RPY(rpy_k);
|
||||
p_pj = str2num(ur.robot.joint{i}.origin.Attributes.xyz)';
|
||||
T_pj = [R_pj, p_pj; zeros(1,3), 1]; % to avoid numerical errors
|
||||
% Tranformation from joint frame of the joint that rotaties body k to
|
||||
% link frame. The transformation is pure rotation
|
||||
R_jk = Rot(q(i), jnt_axs_k);
|
||||
p_jk = zeros(3,1);
|
||||
T_jk = [R_jk, p_jk; zeros(1,3),1];
|
||||
% Transformation from parent link frame p to current link frame k
|
||||
T_pk(:,:,i) = T_pj*T_jk;
|
||||
% Transformation to base
|
||||
T_0k(:,:,i+1) = T_0k(:,:,i)*T_pk(:,:,i);
|
||||
end
|
||||
pos = T_0k(1:3,4,7);
|
||||
|
||||
% (T_0k(1:3,4,7)-T_0k(1:3,4,5))'
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
clc; clear all;
|
||||
|
||||
name = 'current_A_0.7_v_1.bag';
|
||||
bag = rosbag(name);
|
||||
end_time = 20;
|
||||
|
||||
% extract the desired topics
|
||||
start = bag.StartTime;
|
||||
bagselect_current = select(bag, 'Time', [start start + end_time], 'Topic', '/current_synch' );
|
||||
bagselect_torque = select(bag, 'Time', [start start + end_time], 'Topic','/netft_data_synch' );
|
||||
bagselect_command = select(bag, 'Time', [start start + end_time], 'Topic', '/command_synch' );
|
||||
bagselect_Jstates = select(bag, 'Time', [start start + end_time], 'Topic','/joint_states_synch' );
|
||||
bagselect_position = select(bag, 'Time', [start start + end_time], 'Topic', '/position_synch' );
|
||||
bagselect_velocity = select(bag, 'Time', [start start + end_time], 'Topic','/velocity_synch' );
|
||||
|
||||
%
|
||||
% msgs = readMessages(bagselect, [1 2]);
|
||||
% extract message data as time series
|
||||
ts_cur = timeseries(bagselect_current, 'Data');
|
||||
ts_trq = timeseries(bagselect_torque, 'Wrench.Torque.Z');
|
||||
ts_cmd = timeseries(bagselect_command, 'Data');
|
||||
ts_pos = timeseries(bagselect_position, 'Data');
|
||||
ts_vel = timeseries(bagselect_velocity, 'Data');
|
||||
|
||||
% extract the joint states as time series
|
||||
msgs = readMessages(bagselect_Jstates);
|
||||
js_pos = [];
|
||||
for i= 1:numel(msgs)
|
||||
js_pos = [js_pos msgs{i}.Position];
|
||||
end
|
||||
|
||||
js_vel = [];
|
||||
for i= 1:numel(msgs)
|
||||
js_vel = [js_vel msgs{i}.Velocity];
|
||||
end
|
||||
|
||||
ts_Js_time = timeseries(bagselect_Jstates, 'Header.Stamp.Sec');
|
||||
time = ts_Js_time.Time;
|
||||
%
|
||||
ts_Js_pos_shldr = timeseries(js_pos(1,:)',time);
|
||||
ts_Js_pos_elbw = timeseries(js_pos(2,:)',time);
|
||||
|
||||
ts_Js_vel_shldr = timeseries(js_vel(1,:)',time);
|
||||
ts_Js_vel_elbw = timeseries(js_vel(2,:)',time);
|
||||
|
||||
len = min([ts_Js_vel_elbw.Length, ts_Js_vel_shldr.Length, ts_Js_pos_elbw.Length, ...
|
||||
ts_Js_pos_shldr.Length, ts_cur.Length, ts_trq.Length, ts_cmd.Length, ts_pos.Length, ts_vel.Length]);
|
||||
|
||||
% building the table of all the topics
|
||||
time = ts_cur.Time(1:len);
|
||||
current = ts_cur.Data(1:len);
|
||||
torque = ts_trq.Data(1:len);
|
||||
command = ts_cmd.Data(1:len);
|
||||
position = ts_pos.Data(1:len);
|
||||
velocity = ts_vel.Data(1:len);
|
||||
shldr_position = ts_Js_pos_shldr.Data(1:len);
|
||||
elbw_position = ts_Js_pos_elbw.Data(1:len);
|
||||
shldr_velocity = ts_Js_vel_shldr.Data(1:len);
|
||||
elbw_velocity = ts_Js_vel_elbw.Data(1:len);
|
||||
|
||||
T = table(time, current, torque, command, position, velocity, shldr_position, elbw_position, shldr_velocity, elbw_velocity);
|
||||
data = T.Variables;
|
||||
|
||||
|
||||
save(strcat('planar2DOF/data_pndbt/',name(1:end-4),'.mat'),'data');
|
||||
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -0,0 +1,24 @@
|
|||
function Y = full_regressor_plnr(in1,in2,in3)
|
||||
%FULL_REGRESSOR_PLNR
|
||||
% Y = FULL_REGRESSOR_PLNR(IN1,IN2,IN3)
|
||||
|
||||
% This function was generated by the Symbolic Math Toolbox version 8.2.
|
||||
% 03-Feb-2020 10:03:33
|
||||
|
||||
q1 = in1(1,:);
|
||||
q2 = in1(2,:);
|
||||
q2d1 = in3(1,:);
|
||||
q2d2 = in3(2,:);
|
||||
qd1 = in2(1,:);
|
||||
qd2 = in2(2,:);
|
||||
t2 = sin(1.5708);
|
||||
t3 = cos(q2);
|
||||
t4 = sin(q2);
|
||||
t5 = cos(q1);
|
||||
t6 = sin(q1);
|
||||
t7 = qd2.^2;
|
||||
t8 = t2.*t5.*(9.81e2./1.0e2);
|
||||
t9 = q2d1+q2d2;
|
||||
t10 = t2.*t3.*t5.*(9.81e2./1.0e2);
|
||||
t11 = qd1.^2;
|
||||
Y = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,q2d1,0.0,t8,0.0,t2.*t6.*(-9.81e2./1.0e2),0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t9,t9,t10+q2d1.*t3.*2.0+q2d2.*t3-t4.*t7-qd1.*qd2.*t4.*2.0-t2.*t4.*t6.*(9.81e2./1.0e2),t10+q2d1.*t3+t4.*t11-t2.*t4.*t6.*(9.81e2./1.0e2),q2d1.*t4.*-2.0-q2d2.*t4-t3.*t7-qd1.*qd2.*t3.*2.0-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),-q2d1.*t4+t3.*t11-t2.*t3.*t6.*(9.81e2./1.0e2)-t2.*t4.*t5.*(9.81e2./1.0e2),0.0,0.0,q2d1+t8,0.0],[2,20]);
|
||||
|
|
@ -14,17 +14,17 @@
|
|||
|
||||
<link name = "link 1">
|
||||
<inertial>
|
||||
<origin xyz = "0.5 0 0"
|
||||
<origin xyz = "0.125 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
<mass value = "1" />
|
||||
<mass value = "0.29" />
|
||||
<inertia ixx = "1" iyy = "1" izz = "0.5"
|
||||
ixy = "0.01" ixz = "0.01" iyz = "0.01" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz = "0.5 0 0"
|
||||
<origin xyz = "0.125 0 0"
|
||||
rpy = "0 1.5708 0"/>
|
||||
<geometry>
|
||||
<cylinder radius = "0.1" length = "1" />
|
||||
<cylinder radius = "0.1" length = "0.25" />
|
||||
</geometry>
|
||||
<material name = "gray C">
|
||||
<color rgba = "0.3 0.6 0.1 1" />
|
||||
|
|
@ -34,17 +34,17 @@
|
|||
|
||||
<link name = "link 2">
|
||||
<inertial>
|
||||
<origin xyz = "0.5 0 0"
|
||||
<origin xyz = "0.125 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
<mass value = "0.45" />
|
||||
<mass value = "0.26" />
|
||||
<inertia ixx = "1" iyy = "1" izz = "0.3"
|
||||
ixy = "0.01" ixz = "0.01" iyz = "0.01" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz = "0.5 0 0"
|
||||
<origin xyz = "0.125 0 0"
|
||||
rpy = "0 1.5708 0"/>
|
||||
<geometry>
|
||||
<cylinder radius = "0.1" length = "1" />
|
||||
<cylinder radius = "0.1" length = "0.25" />
|
||||
</geometry>
|
||||
<material name = "gray C">
|
||||
<color rgba = "0.22 0.67 0.95 1" />
|
||||
|
|
@ -68,7 +68,7 @@
|
|||
<joint name = "joint 2" type = "revolute">
|
||||
<parent link = "link 1" />
|
||||
<child link = "link 2" />
|
||||
<origin xyz = "1 0 0"
|
||||
<origin xyz = "0.25 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
<axis xyz = "0 0 1" />
|
||||
<limit effort="100.0" lower="-6.2831853" upper="6.2831853" velocity="2.16"/>
|
||||
|
|
@ -77,7 +77,7 @@
|
|||
<joint name = "ee_fixed_joint" type = "fixed">
|
||||
<parent link = "link 2" />
|
||||
<child link = "ee_link" />
|
||||
<origin xyz = "1 0 0"
|
||||
<origin xyz = "0.25 0 0"
|
||||
rpy = "0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
|
|
|||
Binary file not shown.
|
|
@ -0,0 +1,150 @@
|
|||
% get robot description
|
||||
run('plnr_idntfcn.m')
|
||||
|
||||
% Seed the random number generator based on the current time
|
||||
rng('shuffle');
|
||||
|
||||
% some parameters
|
||||
includeMotorDynamics = 1;
|
||||
|
||||
|
||||
% limits on positions, velocities, accelerations
|
||||
q_min = -deg2rad(160);
|
||||
q_max = 0;
|
||||
qd_min = -10;
|
||||
qd_max = 10;
|
||||
q2d_min = -100;
|
||||
q2d_max = 100;
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Find relation between independent columns and dependent columns
|
||||
% -----------------------------------------------------------------------
|
||||
% Get observation matrix of identifiable paramters
|
||||
W = [];
|
||||
for i = 1:20
|
||||
q_rnd = q_min + (q_max - q_min).*rand(2,1);
|
||||
qd_rnd = -qd_max + 2*qd_max.*rand(2,1);
|
||||
q2d_rnd = -q2d_max + 2*q2d_max.*rand(2,1);
|
||||
|
||||
if includeMotorDynamics
|
||||
Yi = regressorWithMotorDynamicsPndbt(q_rnd, qd_rnd, q2d_rnd);
|
||||
else
|
||||
Yi = full_regressor_plnr(q_rnd, qd_rnd, q2d_rnd);
|
||||
end
|
||||
W = vertcat(W,Yi);
|
||||
end
|
||||
|
||||
% QR decomposition with pivoting: W*E = Q*R
|
||||
% R is upper triangular matrix
|
||||
% Q is unitary matrix
|
||||
% E is permutation matrix
|
||||
[Q,R,E] = qr(W);
|
||||
|
||||
% matrix W has rank bb which is number number of base parameters
|
||||
bb = rank(W);
|
||||
|
||||
% R = [R1 R2;
|
||||
% 0 0]
|
||||
% R1 is bbxbb upper triangular and reguar matrix
|
||||
% R2 is bbx(c-bb) matrix where c is number of standard parameters
|
||||
R1 = R(1:bb,1:bb);
|
||||
R2 = R(1:bb,bb+1:end);
|
||||
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
|
||||
beta(abs(beta)<sqrt(eps)) = 0; % get rid of numerical errors
|
||||
% W2 = W1*beta
|
||||
|
||||
% Make sure that the relation holds
|
||||
W1 = W*E(:,1:bb);
|
||||
W2 = W*E(:,bb+1:end);
|
||||
if norm(W2 - W1*beta) > 1e-6
|
||||
fprintf('Found realationship between W1 and W2 is not correct\n');
|
||||
return
|
||||
end
|
||||
|
||||
|
||||
% Defining parameters symbolically
|
||||
m = sym('m%d',[2,1],'real');
|
||||
hx = sym('h%d_x',[2,1],'real');
|
||||
hy = sym('h%d_y',[2,1],'real');
|
||||
hz = sym('h%d_z',[2,1],'real');
|
||||
ixx = sym('i%d_xx',[2,1],'real');
|
||||
ixy = sym('i%d_xy',[2,1],'real');
|
||||
ixz = sym('i%d_xz',[2,1],'real');
|
||||
iyy = sym('i%d_yy',[2,1],'real');
|
||||
iyz = sym('i%d_yz',[2,1],'real');
|
||||
izz = sym('i%d_zz',[2,1],'real');
|
||||
im = sym('im',[1,1],'real');
|
||||
|
||||
% Vector of symbolic parameters
|
||||
pi_pndbt_sym = {};
|
||||
for i = 1:2
|
||||
pi_pndbt_sym{i} = [ixx(i),ixy(i),ixz(i),iyy(i),iyz(i),izz(i),...
|
||||
hx(i),hy(i),hz(i),m(i)]';
|
||||
end
|
||||
|
||||
if includeMotorDynamics
|
||||
pi_pndbt_sym{1} = [pi_pndbt_sym{1}; im];
|
||||
end
|
||||
pi_pndbt_sym = [pi_pndbt_sym{1}; pi_pndbt_sym{2}];
|
||||
|
||||
|
||||
% Find base parmaters
|
||||
pi1 = E(:,1:bb)'*pi_pndbt_sym; % independent paramters
|
||||
pi2 = E(:,bb+1:end)'*pi_pndbt_sym; % dependent paramteres
|
||||
|
||||
% all of the expressions below are equivalent
|
||||
pi_lgr_base = pi1 + beta*pi2;
|
||||
pi_lgr_base2 = [eye(bb) beta]*[pi1;pi2];
|
||||
pi_lgr_base3 = [eye(bb) beta]*E'*pi_pndbt_sym;
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Validation of obtained mappings
|
||||
% -----------------------------------------------------------------------
|
||||
fprintf('Validation of mapping from standard parameters to base ones\n')
|
||||
if includeMotorDynamics
|
||||
plnr.pi = [plnr.pi(:,1); rand; plnr.pi(:,2)];
|
||||
else
|
||||
plnr.pi = [plnr.pi(:,1); plnr.pi(:,2)];
|
||||
end
|
||||
|
||||
% On random positions, velocities, aceeleations
|
||||
for i = 1:100
|
||||
q_rnd = q_min + (q_max - q_min).*rand(2,1);
|
||||
qd_rnd = -qd_max + 2*qd_max.*rand(2,1);
|
||||
q2d_rnd = -q2d_max + 2*q2d_max.*rand(2,1);
|
||||
|
||||
if includeMotorDynamics
|
||||
Yi = regressorWithMotorDynamicsPndbt(q_rnd,qd_rnd,q2d_rnd);
|
||||
else
|
||||
Yi = full_regressor_plnr(q_rnd,qd_rnd,q2d_rnd);
|
||||
end
|
||||
tau_full = Yi*plnr.pi;
|
||||
|
||||
pi_lgr_base = [eye(bb) beta]*E'*plnr.pi;
|
||||
Y_base = Yi*E(:,1:bb);
|
||||
tau_base = Y_base*pi_lgr_base;
|
||||
nrm_err1(i) = norm(tau_full - tau_base);
|
||||
end
|
||||
figure
|
||||
plot(nrm_err1)
|
||||
ylabel('||\tau - \tau_b||')
|
||||
grid on
|
||||
|
||||
if ~all(nrm_err1<1e-6)
|
||||
fprintf('Validation failed')
|
||||
return
|
||||
end
|
||||
|
||||
% ---------------------------------------------------------------------
|
||||
% Create structure with the result of QR decompositon and save it
|
||||
% for further use.
|
||||
% ---------------------------------------------------------------------
|
||||
plnrBaseQR = struct;
|
||||
plnrBaseQR.numberOfBaseParameters = bb;
|
||||
plnrBaseQR.permutationMatrix = E;
|
||||
plnrBaseQR.beta = beta;
|
||||
plnrBaseQR.motorDynamicsIncluded = includeMotorDynamics;
|
||||
|
||||
filename = 'planar2DOF/plnrBaseQR.mat';
|
||||
save(filename,'plnrBaseQR')
|
||||
|
|
@ -1,14 +1,9 @@
|
|||
clc; clear all; close all;
|
||||
|
||||
%-------------------------------------------------------------------------
|
||||
% Loading file from urdf
|
||||
% ------------------------------------------------------------------------
|
||||
|
||||
plnr = xml2struct('planar_manip.urdf');
|
||||
|
||||
% ------------------------------------------------------------------------
|
||||
% Extracting parameters of the robot
|
||||
% ------------------------------------------------------------------------
|
||||
for i = 1:2
|
||||
% axis of rotation of a joint i in coordinate system of joint i
|
||||
axis_of_rot = str2num(plnr.robot.joint{i}.axis.Attributes.xyz)';
|
||||
|
|
@ -40,6 +35,7 @@ for i = 1:2
|
|||
plnr.pi(:,i) = [plnr.I_vec(:,i);plnr.h(:,i);plnr.m(i)];
|
||||
end
|
||||
|
||||
return
|
||||
% ------------------------------------------------------------------------
|
||||
% Defining parameters symbolically
|
||||
% ------------------------------------------------------------------------
|
||||
|
|
@ -180,6 +176,7 @@ for i = 1:2
|
|||
pi_base_vctr = vertcat(pi_base_vctr,pi_base_num{i});
|
||||
end
|
||||
|
||||
return
|
||||
% ------------------------------------------------------------------------
|
||||
% Using lagrange equations of the second kind, find dynamics
|
||||
% ------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -0,0 +1,87 @@
|
|||
% get robot description
|
||||
run('plnr_idntfcn.m');
|
||||
|
||||
% Symbolic generilized coordiates, their first and second deriatives
|
||||
q_sym = sym('q%d',[2,1],'real');
|
||||
qd_sym = sym('qd%d',[2,1],'real');
|
||||
q2d_sym = sym('q2d%d',[2,1],'real');
|
||||
|
||||
% Getting energy functions, to derive dynamics
|
||||
T_pk = sym(zeros(4,4,2)); % transformation between links
|
||||
w_kk(:,1) = sym(zeros(3,1)); % angular velocity k in frame k
|
||||
v_kk(:,1) = sym(zeros(3,1)); % linear velocity of the origin of frame k in frame k
|
||||
g_kk(:,1) = sym([0,0,9.81])'; % vector of graviatational accelerations in frame k
|
||||
p_kk(:,1) = sym(zeros(3,1)); % origin of frame k in frame k
|
||||
|
||||
for i = 1:2
|
||||
jnt_axs_k = str2num(plnr.robot.joint{i}.axis.Attributes.xyz)';
|
||||
% Transformation from parent link frame p to current joint frame
|
||||
rpy_k = sym(str2num(plnr.robot.joint{i}.origin.Attributes.rpy));
|
||||
R_pj = RPY(rpy_k);
|
||||
% R_pj(abs(R_pj)<sqrt(eps)) = sym(0); % to avoid numerical errors
|
||||
R_pj(abs(R_pj)<1e-5) = sym(0); % to avoid numerical errors
|
||||
p_pj = str2num(plnr.robot.joint{i}.origin.Attributes.xyz)';
|
||||
T_pj = sym([R_pj, p_pj; zeros(1,3), 1]); % to avoid numerical errors
|
||||
% Tranformation from joint frame of the joint that rotaties body k to
|
||||
% link frame. The transformation is pure rotation
|
||||
R_jk = Rot(q_sym(i),sym(jnt_axs_k));
|
||||
p_jk = sym(zeros(3,1));
|
||||
T_jk = [R_jk, p_jk; sym(zeros(1,3)),sym(1)];
|
||||
% Transformation from parent link frame p to current link frame k
|
||||
T_pk(:,:,i) = T_pj*T_jk;
|
||||
z_kk(:,i) = sym(jnt_axs_k);
|
||||
|
||||
w_kk(:,i+1) = T_pk(1:3,1:3,i)'*w_kk(:,i) + sym(jnt_axs_k)*qd_sym(i);
|
||||
v_kk(:,i+1) = T_pk(1:3,1:3,i)'*(v_kk(:,i) + cross(w_kk(:,i),sym(p_pj)));
|
||||
g_kk(:,i+1) = T_pk(1:3,1:3,i)'*g_kk(:,i);
|
||||
p_kk(:,i+1) = T_pk(1:3,1:3,i)'*(p_kk(:,i) + sym(p_pj));
|
||||
|
||||
K_reg(i,:) = [sym(0.5)*w2wtlda(w_kk(:,i+1)),...
|
||||
v_kk(:,i+1)'*vec2skewSymMat(w_kk(:,i+1)),...
|
||||
sym(0.5)*v_kk(:,i+1)'*v_kk(:,i+1)];
|
||||
P_reg(i,:) = [sym(zeros(1,6)), g_kk(:,i+1)',...
|
||||
g_kk(:,i+1)'*p_kk(:,i+1)];
|
||||
end
|
||||
% Compose lagrangian
|
||||
Lagr = [K_reg(1,:) - P_reg(1,:), K_reg(2,:) - P_reg(2,:)];
|
||||
% Use Lagrange equations to derive regressor matrix
|
||||
dLagr_dq = jacobian(Lagr,q_sym)';
|
||||
dLagr_dqd = jacobian(Lagr,qd_sym)';
|
||||
t1 = sym(zeros(2,20));
|
||||
for i = 1:2
|
||||
t1 = t1 + diff(dLagr_dqd,q_sym(i))*qd_sym(i)+...
|
||||
diff(dLagr_dqd,qd_sym(i))*q2d_sym(i);
|
||||
end
|
||||
Y = simplify(t1 - dLagr_dq);
|
||||
Y_fcn = matlabFunction(Y,'Vars',{q_sym, qd_sym, q2d_sym});
|
||||
|
||||
|
||||
% Testing if regressor computed correctly using matlab Robotics Robotics
|
||||
rbt = importrobot('planar_manip.urdf');
|
||||
rbt.DataFormat = 'column';
|
||||
rbt.Gravity = [0 0 -9.81];
|
||||
|
||||
noIter = 100;
|
||||
err1 = zeros(noIter,1);
|
||||
for i = 1:noIter
|
||||
q = rand(2,1);
|
||||
qd = rand(2,1);
|
||||
q2d = rand(2,1);
|
||||
|
||||
Ylgr = Y_fcn(q,qd,q2d);
|
||||
|
||||
tau1 = inverseDynamics(rbt,q,qd,q2d);
|
||||
tau2 = Ylgr*reshape(plnr.pi,[20,1]);
|
||||
|
||||
% verifying if regressor is computed correctly
|
||||
err1(i) = norm(tau1 - tau2);
|
||||
end
|
||||
|
||||
if all(err1<1e-6)
|
||||
disp('Regressor matrix is computed correctly!')
|
||||
disp('Generating regressor matrix function...')
|
||||
matlabFunction(Y,'File','planar2DOF/full_regressor_plnr',...
|
||||
'Vars',{q_sym, qd_sym, q2d_sym});
|
||||
else
|
||||
disp('Regressor matrix is not computed correctly. Check derivation')
|
||||
end
|
||||
|
|
@ -0,0 +1,39 @@
|
|||
function plnr_visualize(q, plnr)
|
||||
|
||||
figure
|
||||
for j = 1:10:length(q)
|
||||
clf
|
||||
qj = q(j,:);
|
||||
|
||||
T_0k(:,:,1) = eye(4);
|
||||
for i = 1:2
|
||||
R_pj = RPY(str2num(plnr.robot.joint{i}.origin.Attributes.rpy));
|
||||
p_pj = str2num(plnr.robot.joint{i}.origin.Attributes.xyz)';
|
||||
T_pj = [R_pj, p_pj; zeros(1,3), 1];
|
||||
|
||||
R_jk = Rot(qj(i),plnr.k(:,i));
|
||||
p_jk = zeros(3,1);
|
||||
T_jk = [R_jk, p_jk; zeros(1,3),1];
|
||||
T_pk(:,:,i) = T_pj*T_jk;
|
||||
|
||||
T_0k(:,:,i+1) = T_0k(:,:,i)*T_pk(:,:,i);
|
||||
z_0k(:,i) = T_0k(1:3,1:3,i+1)*plnr.k(:,i);
|
||||
r_0k(:,i) = [eye(3),zeros(3,1)]*T_0k(:,:,i+1)*[plnr.r_com(:,i);1];
|
||||
end
|
||||
R_pee = RPY(str2num(plnr.robot.joint{3}.origin.Attributes.rpy));
|
||||
p_pee = str2num(plnr.robot.joint{3}.origin.Attributes.xyz)';
|
||||
T_pee = [R_pee, p_pee; zeros(1,3), 1];
|
||||
T_0ee = T_0k(:,:,3)*T_pee;
|
||||
|
||||
MARKER_SIZE = 100; %marker size of the scatter function
|
||||
scatter(T_0k(1,4,2), T_0k(3,4,2),MARKER_SIZE,'k','filled')
|
||||
hold on
|
||||
scatter(T_0k(1,4,3), T_0k(3,4,3),MARKER_SIZE,'k','filled')
|
||||
% scatter(Tee0(1,3), Tee0(2,3))
|
||||
line([T_0k(1,4,2) T_0k(1,4,3)], [T_0k(3,4,2), T_0k(3,4,3)],'Color','k','LineWidth',1.5)
|
||||
line([T_0k(1,4,3) T_0ee(1,4)], [T_0k(3,4,3), T_0ee(3,4)],'Color','k','LineWidth',1.5)
|
||||
xlim([-0.5 0.5])
|
||||
ylim([-0.6 0.2])
|
||||
|
||||
pause(1e-1)
|
||||
end
|
||||
|
|
@ -0,0 +1,121 @@
|
|||
% clc; clear all; close all;
|
||||
% get robot description
|
||||
run('plnr_idntfcn.m')
|
||||
|
||||
% load pendubot data
|
||||
% rawData = load('current_A_5_v_4.mat');
|
||||
rawData = load('current_A_0.7_v_1.mat');
|
||||
|
||||
% parse pendubot data
|
||||
pendubot.time = rawData.data(:,1) - rawData.data(1,1);
|
||||
pendubot.current = rawData.data(:,2);
|
||||
pendubot.current_desired = rawData.data(:,4);
|
||||
pendubot.torque = rawData.data(:,3);
|
||||
|
||||
pendubot.shldr_position = rawData.data(:,7) - pi/2; % to fit model
|
||||
pendubot.shldr_velocity = rawData.data(:,9);
|
||||
pendubot.elbw_position = rawData.data(:,8);
|
||||
pendubot.elbw_velocity = rawData.data(:,10);
|
||||
|
||||
pendubot.position_desired = rawData.data(:,5);
|
||||
pendubot.velocity_desired = rawData.data(:,6);
|
||||
|
||||
% filter velocties with zero phas filter
|
||||
vlcty_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.25,'DesignMethod','butter');
|
||||
pendubot.shldr_velocity_filtered = filtfilt(vlcty_fltr, pendubot.shldr_velocity);
|
||||
pendubot.elbw_velocity_filtered = filtfilt(vlcty_fltr, pendubot.elbw_velocity);
|
||||
|
||||
% estimating accelerations based on filtered velocities
|
||||
q2d1 = zeros(size(pendubot.shldr_velocity));
|
||||
q2d2 = zeros(size(pendubot.elbw_velocity));
|
||||
for i = 2:size(pendubot.shldr_velocity,1)-1
|
||||
q2d1(i) = (pendubot.shldr_velocity_filtered(i+1) - pendubot.shldr_velocity_filtered(i-1))/...
|
||||
(pendubot.time(i+1) - pendubot.time(i-1));
|
||||
q2d2(i) = (pendubot.elbw_velocity_filtered(i+1) - pendubot.elbw_velocity_filtered(i-1))/...
|
||||
(pendubot.time(i+1) - pendubot.time(i-1));
|
||||
end
|
||||
pendubot.shldr_acceleration = q2d1;
|
||||
pendubot.elbow_acceleration = q2d2;
|
||||
|
||||
% filter estimated accelerations with zero phase filter
|
||||
aclrtn_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.25,'DesignMethod','butter');
|
||||
pendubot.shldr_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.shldr_acceleration);
|
||||
pendubot.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration);
|
||||
|
||||
% filter torque measurements using zero phase filter
|
||||
trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
||||
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
|
||||
|
||||
%%
|
||||
return
|
||||
plotJointPositions(pendubot)
|
||||
plotJointVelocities(pendubot)
|
||||
plotJointAccelerations(pendubot)
|
||||
|
||||
plnr_visualize([pendubot.shldr_position, pendubot.elbw_position], plnr)
|
||||
|
||||
|
||||
%% Functions
|
||||
|
||||
function plotJointPositions(pendubot)
|
||||
figure
|
||||
subplot(2,1,1)
|
||||
plot(pendubot.time, pendubot.shldr_position)
|
||||
xlim([0 1])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$q_1$, rad','interpreter', 'latex')
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
plot(pendubot.time, pendubot.elbw_position)
|
||||
xlim([0 1])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$q_2$, rad','interpreter', 'latex')
|
||||
grid on
|
||||
end
|
||||
|
||||
function plotJointVelocities(pendubot)
|
||||
figure
|
||||
subplot(2,1,1)
|
||||
plot(pendubot.time, pendubot.shldr_velocity)
|
||||
hold on
|
||||
plot(pendubot.time, pendubot.shldr_velocity_filtered)
|
||||
xlim([0 5])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$\dot{q}_1$, rad/s','interpreter', 'latex')
|
||||
legend('measured', 'filtered')
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
plot(pendubot.time, pendubot.elbw_velocity)
|
||||
hold on
|
||||
plot(pendubot.time, pendubot.elbw_velocity_filtered)
|
||||
xlim([0 5])
|
||||
xlabel('t, sec','interpreter', 'latex')
|
||||
ylabel('$\dot{q}_2$, rad/s','interpreter', 'latex')
|
||||
legend('measured', 'filtered')
|
||||
grid on
|
||||
end
|
||||
|
||||
function plotJointAccelerations(pendubot)
|
||||
figure
|
||||
subplot(2,1,1)
|
||||
plot(pendubot.time, pendubot.shldr_acceleration)
|
||||
hold on
|
||||
plot(pendubot.time, pendubot.shldr_acceleration_filtered)
|
||||
xlim([0 2])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$\ddot{q}_1$, rad/s$^2$','interpreter', 'latex')
|
||||
legend('estimate', 'filetered estimate')
|
||||
grid on
|
||||
subplot(2,1,2)
|
||||
plot(pendubot.time, pendubot.elbow_acceleration)
|
||||
hold on
|
||||
plot(pendubot.time, pendubot.elbow_acceleration_filtered)
|
||||
xlim([0 2])
|
||||
xlabel('$t$, sec','interpreter', 'latex')
|
||||
ylabel('$\ddot{q}_2$, rad/s$^2$','interpreter', 'latex')
|
||||
legend('estimate', 'filetered estimate')
|
||||
grid on
|
||||
end
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
% run data processing script
|
||||
run('pndbt_data_processing.m')
|
||||
|
||||
% load mapping from standard parameters to base parameters
|
||||
load('plnrBaseQR.mat')
|
||||
|
||||
% compose observation matrix and torque vector
|
||||
noObservations = length(pendubot.time);
|
||||
W = []; Tau = [];
|
||||
for i = 1:noObservations
|
||||
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
|
||||
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
|
||||
q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]';
|
||||
|
||||
if plnrBaseQR.motorDynamicsIncluded
|
||||
Yi = regressorWithMotorDynamicsPndbt(qi, qdi, q2di);
|
||||
else
|
||||
Yi = full_regressor_plnr(qi, qdi, q2di);
|
||||
end
|
||||
Ybi = Yi*plnrBaseQR.permutationMatrix(:,1:plnrBaseQR.numberOfBaseParameters);
|
||||
Yfrctni = frictionRegressor(qdi);
|
||||
W = vertcat(W, [Ybi, Yfrctni]);
|
||||
|
||||
taui = [pendubot.torque(i), 0]';
|
||||
Tau = vertcat(Tau, taui);
|
||||
end
|
||||
|
||||
|
||||
%% Usual Least Squares Approach
|
||||
pi_hat = (W'*W)\(W'*Tau)
|
||||
|
||||
|
||||
%% Set-up SDP optimization procedure
|
||||
physicalConsistency = 1;
|
||||
|
||||
pi_frctn = sdpvar(6,1);
|
||||
pi_b = sdpvar(plnrBaseQR.numberOfBaseParameters,1); % variables for base paramters
|
||||
pi_d = sdpvar(15,1); % variables for dependent paramters
|
||||
|
||||
% Bijective mapping from [pi_b; pi_d] to standard parameters pi
|
||||
pii = plnrBaseQR.permutationMatrix*[eye(plnrBaseQR.numberOfBaseParameters), ...
|
||||
-plnrBaseQR.beta; ...
|
||||
zeros(15,plnrBaseQR.numberOfBaseParameters), ...
|
||||
eye(15)]*[pi_b; pi_d];
|
||||
|
||||
cnstr = [];
|
||||
if physicalConsistency
|
||||
for i = 1:11:21
|
||||
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||
pii(i+1), pii(i+3), pii(i+4); ...
|
||||
pii(i+2), pii(i+4), pii(i+5)];
|
||||
frst_mmnt_i = pii(i+6:i+8);
|
||||
|
||||
Di = [0.5*trace(link_inertia_i)*eye(3) - link_inertia_i, ...
|
||||
frst_mmnt_i; frst_mmnt_i', pii(i+9)];
|
||||
|
||||
cnstr = [cnstr, Di>0];
|
||||
end
|
||||
else
|
||||
for i = 1:11:21
|
||||
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||
pii(i+1), pii(i+3), pii(i+4); ...
|
||||
pii(i+2), pii(i+4), pii(i+5)];
|
||||
|
||||
frst_mmnt_i = vec2skewSymMat(pii(i+6:i+8));
|
||||
|
||||
Di = [link_inertia_i, frst_mmnt_i'; frst_mmnt_i, pii(i+9)*eye(3)];
|
||||
cnstr = [cnstr, Di>0];
|
||||
end
|
||||
end
|
||||
cnstr = [cnstr, pii(11) > 0]; % first motor inertia constraint
|
||||
|
||||
% Feasibility constraints on the friction prameters
|
||||
for i = 1:2
|
||||
cnstr = [cnstr, pi_frctn(3*i-2)>0, pi_frctn(3*i-1)>0];
|
||||
end
|
||||
|
||||
% Defining pbjective function
|
||||
obj = norm(Tau - W*[pi_b; pi_frctn], 2);
|
||||
|
||||
% Solving sdp problem
|
||||
sol2 = optimize(cnstr, obj, sdpsettings('solver','sdpt3'));
|
||||
|
||||
pi_b = value(pi_b) % variables for base paramters
|
||||
pi_frctn = value(pi_frctn)
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
% load pendubot data
|
||||
% rawData = load('current_A_0.7_v_1.mat');
|
||||
rawData = load('current_A_5_v_4.mat');
|
||||
|
||||
% parse pendubot data
|
||||
pendubot.time = rawData.data(:,1) - rawData.data(1,1);
|
||||
pendubot.current = rawData.data(:,2);
|
||||
pendubot.current_desired = rawData.data(:,4);
|
||||
pendubot.torque = rawData.data(:,3);
|
||||
|
||||
pendubot.shldr_position = rawData.data(:,7) - pi/2; % to fit model
|
||||
pendubot.shldr_velocity = rawData.data(:,9);
|
||||
pendubot.elbw_position = rawData.data(:,8);
|
||||
pendubot.elbw_velocity = rawData.data(:,10);
|
||||
|
||||
pendubot.position_desired = rawData.data(:,5);
|
||||
pendubot.velocity_desired = rawData.data(:,6);
|
||||
|
||||
% filter velocties with zero phas filter
|
||||
vlcty_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.25,'DesignMethod','butter');
|
||||
pendubot.shldr_velocity_filtered = filtfilt(vlcty_fltr, pendubot.shldr_velocity);
|
||||
pendubot.elbw_velocity_filtered = filtfilt(vlcty_fltr, pendubot.elbw_velocity);
|
||||
|
||||
% estimating accelerations based on filtered velocities
|
||||
q2d1 = zeros(size(pendubot.shldr_velocity));
|
||||
q2d2 = zeros(size(pendubot.elbw_velocity));
|
||||
for i = 2:size(pendubot.shldr_velocity,1)-1
|
||||
q2d1(i) = (pendubot.shldr_velocity_filtered(i+1) - pendubot.shldr_velocity_filtered(i-1))/...
|
||||
(pendubot.time(i+1) - pendubot.time(i-1));
|
||||
q2d2(i) = (pendubot.elbw_velocity_filtered(i+1) - pendubot.elbw_velocity_filtered(i-1))/...
|
||||
(pendubot.time(i+1) - pendubot.time(i-1));
|
||||
end
|
||||
pendubot.shldr_acceleration = q2d1;
|
||||
pendubot.elbow_acceleration = q2d2;
|
||||
|
||||
% filter estimated accelerations with zero phase filter
|
||||
aclrtn_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.25,'DesignMethod','butter');
|
||||
pendubot.shldr_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.shldr_acceleration);
|
||||
pendubot.elbow_acceleration_filtered = filtfilt(aclrtn_fltr, pendubot.elbow_acceleration);
|
||||
|
||||
% filter torque measurements using zero phase filter
|
||||
trq_fltr = designfilt('lowpassiir','FilterOrder',5, ...
|
||||
'HalfPowerFrequency',0.2,'DesignMethod','butter');
|
||||
pendubot.torque_filtered = filtfilt(trq_fltr, pendubot.torque);
|
||||
|
||||
|
||||
% Validation
|
||||
vldtnRange = 1:200;
|
||||
tau_prdctd_OLS = []; tau_prdctd_SDP = [];
|
||||
for i = vldtnRange
|
||||
qi = [pendubot.shldr_position(i), pendubot.elbw_position(i)]';
|
||||
qdi = [pendubot.shldr_velocity_filtered(i), pendubot.elbw_velocity_filtered(i)]';
|
||||
q2di = [pendubot.shldr_acceleration_filtered(i), pendubot.elbow_acceleration_filtered(i)]';
|
||||
|
||||
if plnrBaseQR.motorDynamicsIncluded
|
||||
Yi = regressorWithMotorDynamicsPndbt(qi, qdi, q2di);
|
||||
else
|
||||
Yi = full_regressor_plnr(qi, qdi, q2di);
|
||||
end
|
||||
Ybi = Yi*plnrBaseQR.permutationMatrix(:,1:plnrBaseQR.numberOfBaseParameters);
|
||||
Yfrctni = frictionRegressor(qdi);
|
||||
tau_prdctd_OLS = horzcat(tau_prdctd_OLS, [Ybi, Yfrctni]*pi_hat);
|
||||
tau_prdctd_SDP = horzcat(tau_prdctd_SDP, [Ybi, Yfrctni]*[pi_b; pi_frctn]);
|
||||
end
|
||||
|
||||
%%
|
||||
figure
|
||||
plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange),'r')
|
||||
hold on
|
||||
plot(pendubot.time(vldtnRange), tau_prdctd_OLS(1,:),'b-')
|
||||
plot(pendubot.time(vldtnRange), tau_prdctd_SDP(1,:),'k-')
|
||||
% plot(pendubot.time(vldtnRange), pendubot.torque(vldtnRange) - tau_prdctd(1,:)', 'k--')
|
||||
legend('measured', 'predicted OLS', 'predicted SDP')
|
||||
grid on
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
function Y = regressorWithMotorDynamicsPndbt(q,qd,q2d)
|
||||
% ----------------------------------------------------------------------
|
||||
% This function adds motor dynamics to rigid body regressor.
|
||||
% It is simplified model of motor dynamics, it adds only reflected
|
||||
% inertia i.e. I_rflctd = Im*N^2 where N is reduction ratio - I_rflctd*q_2d
|
||||
% parameter is added to existing vector of each link [pi_i I_rflctd_i]
|
||||
% so that each link has 11 parameters
|
||||
% ----------------------------------------------------------------------
|
||||
if size(q,1)==2 && size(q,2)==1 && size(qd,1)==2 && size(qd,2)==1 ...
|
||||
&& size(q2d,1)==2 && size(q2d,2)==1
|
||||
Y_rgd_bdy = full_regressor_plnr(q,qd,q2d);
|
||||
Y = [Y_rgd_bdy(:,1:10), [q2d(1) 0]', Y_rgd_bdy(:,11:20)];
|
||||
else
|
||||
error('Input dimension error!')
|
||||
end
|
||||
22710
planar2DOF/traj1.eps
22710
planar2DOF/traj1.eps
File diff suppressed because it is too large
Load Diff
22638
planar2DOF/traj2.eps
22638
planar2DOF/traj2.eps
File diff suppressed because it is too large
Load Diff
22638
planar2DOF/traj3.eps
22638
planar2DOF/traj3.eps
File diff suppressed because it is too large
Load Diff
16940
planar2DOF/valid1.eps
16940
planar2DOF/valid1.eps
File diff suppressed because it is too large
Load Diff
16990
planar2DOF/valid2.eps
16990
planar2DOF/valid2.eps
File diff suppressed because it is too large
Load Diff
17024
planar2DOF/valid3.eps
17024
planar2DOF/valid3.eps
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,6 @@
|
|||
a1 = [63.14445447,3.8464372,0.4158832844,0.3695524202,0.08746106361,-0.1337522786,0.2961289419,0.1481741796,-0.2353883251,0.09399249647,-0.1538978199,0.233615134]
|
||||
a2 = [64.11741816,3.419368669,0.9569679605,0.3840862501,0.07814075303,-0.1229639591,0.02985321809,0.1491091833,0.02899625285,0.05196032274,0.2596098438,0.07364049564]
|
||||
a3 = [63.99585112,3.795840113,0.3484088895,0.4071788941,0.08260375437,0.2451950051,0.09884994438,-0.04640752829,-0.04826315454,0.2385501641,0.04460446056,0.1202201747]
|
||||
a4 = [16.02097434,1.253074147,-0.255199746,0.4398665024,-0.2171605318,-0.0671930056,0.04760909641,0.05378361497,0.1664478999,-0.1412713881,-0.107594707,-0.01160368875]
|
||||
a5 = [8.719760589,0.812418026,-0.1530323563,0.1242863329,-0.09048595915,-0.07278439425,0.09679122168,-0.04824579569,-0.1194642801,0.08013340566,-0.01283551447,0.005705712498]
|
||||
a6 = [64.61350353,3.897149875,0.8834439241,-0.007566380055,0.3211701597,0.1278674554,0.07810124808,0.1587321747,0.1003165104,0.03590044572,0.03013318791,-0.01484059122]
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
b1 = [-0.7092040412,0.5230610431,0.1976180106,-0.2377492599,0.09662808559,-0.0442446606,0.04546091583,-0.05527008338,0.07999041891,-0.1954301078,0.02276381324,-0.0003037708343]
|
||||
b2 = [16.44144409,1.96646412,0.4413777885,0.01535646862,0.1699440808,-0.04517986656,0.08204914487,0.09802980971,0.1107566817,0.164257973,0.1687008776,-0.1158379341]
|
||||
b3 = [15.91827703,1.738289041,0.6396138544,0.4400174545,0.06841133754,0.1540512842,-0.01213731937,0.1288104914,0.01749713258,0.2106228112,-0.1025715439,-0.03232628298]
|
||||
b4 = [3.07591063,0.4116519318,0.1342122544,-0.02000429905,0.1916582385,0.1154067377,0.03839347039,0.01232415683,-0.1919967103,-0.01691073997,0.01163710601,0.04178807472]
|
||||
b5 = [32.38341148,4.112282002,1.463403865,0.4409561611,0.3426812758,0.1595795442,0.1184208908,0.05993487595,-0.09285686628,0.01674492771,0.09477511091,0.168557236]
|
||||
b6 = [32.04972222,3.852785925,1.284364462,-0.1618577222,0.456724459,0.2217872019,0.2525677551,0.1450619486,0.09508517903,0.04484496664,0.1612198891,-0.003833787606]
|
||||
|
|
@ -0,0 +1,6 @@
|
|||
c1 = [-1.39519063326,-68.1126607632,0.14374629977,1.6884418891,-0.127351873182,0.00255422477862]
|
||||
c2 = [54.6442907022,-69.4261871541,-4.21007966412,2.15666264527,-0.140699300074,0.00260348201828]
|
||||
c3 = [54.6708156721,-69.2826318354,-4.08927819065,2.14099361495,-0.140128130168,0.00259809869383]
|
||||
c4 = [9.14854552465,-17.1817325285,-0.781047135654,0.507648026779,-0.0341683663301,0.00064431496982]
|
||||
c5 = [111.953260927,-9.34224698765,-8.34694011304,1.06825018599,-0.0383840633844,0.000350334262037]
|
||||
c6 = [110.057813716,-70.2239115388,-8.2522415789,2.58082194636,-0.152300438083,0.00263339668271]
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -1,4 +1,4 @@
|
|||
function [c,ceq] = traj_cnstr(opt_vars,traj_par)
|
||||
function [c,ceq] = traj_cnstr(opt_vars, traj_par)
|
||||
% --------------------------------------------------------------------
|
||||
% The function computes constraints on trajectory for trajectoty
|
||||
% optimization needed for dynamic parameter identification
|
||||
|
|
|
|||
|
|
@ -0,0 +1,53 @@
|
|||
function [c,ceq] = traj_cnstr_fsblty(opt_vars, traj_par, baseQR)
|
||||
% --------------------------------------------------------------------
|
||||
% The function computes constraints on trajectory for trajectoty
|
||||
% optimization needed for dynamic parameter identification more
|
||||
% precisely for searching feasibly solutions with condition number
|
||||
% less than 100 that guarantees good estimate
|
||||
% -------------------------------------------------------------------
|
||||
% Trajectory parameters
|
||||
N = traj_par.N;
|
||||
wf = traj_par.wf;
|
||||
T = traj_par.T;
|
||||
t = traj_par.t;
|
||||
|
||||
% As paramters of the trajectory are in a signle vector we reshape them as
|
||||
% to feed the function that computes the trajectory
|
||||
ab = reshape(opt_vars,[12,N]);
|
||||
a = ab(1:6,:); % sin coeffs
|
||||
b = ab(7:12,:); % cos coeffs
|
||||
|
||||
% To guarantee that positions, velocities and accelerations are zero in the
|
||||
% beginning and at time T, we add fifth order polynomial to fourier
|
||||
% series. The parameters of the polynomial depends on the parameters of
|
||||
% fourier series. Here we compute them.
|
||||
c_pol = getPolCoeffs(T, a, b, wf, N, traj_par.q0);
|
||||
|
||||
% Compute trajectory (Fouruer series + fifth order polynomail)
|
||||
[q,qd,q2d] = mixed_traj(t, c_pol, a, b, wf, N);
|
||||
|
||||
% Obtain observation matrix by computing regressor for each sampling time
|
||||
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||
W = [];
|
||||
for i = 1:length(t)
|
||||
% Y = base_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i));
|
||||
if baseQR.motorDynamicsIncluded
|
||||
Y = [regressorWithMotorDynamics(q(:,i),qd(:,i),q2d(:,i))*E1, ...
|
||||
frictionRegressor(qd(:,i))];
|
||||
else
|
||||
Y = [full_regressor_UR10E(q(:,i),qd(:,i),q2d(:,i))*E1, ...
|
||||
frictionRegressor(qd(:,i))];
|
||||
end
|
||||
W = vertcat(W,Y);
|
||||
end
|
||||
|
||||
|
||||
% Inequality constraints
|
||||
c(1:6) = traj_par.q_min - min(q,[],2); % upper joint limit constraint
|
||||
c(7:12) = max(q,[],2) - traj_par.q_max; % lower joint limit constraint
|
||||
c(13:18) = max(abs(qd),[],2) - traj_par.qd_max; % max joint velocity const
|
||||
c(19:24) = max(abs(q2d),[],2) - traj_par.q2d_max; % max joint acceleration constr
|
||||
c(25) = cond(W) - 100;
|
||||
|
||||
% Equality contrsints
|
||||
ceq = [];
|
||||
|
|
@ -86,7 +86,7 @@ bb = rank(W);
|
|||
% R = [R1 R2;
|
||||
% 0 0]
|
||||
% R1 is bbxbb upper triangular and reguar matrix
|
||||
% R2 is bbx(c-bb) matrix where c is number of identifiable parameters
|
||||
% R2 is bbx(c-bb) matrix where c is number of standard parameters
|
||||
R1 = R(1:bb,1:bb);
|
||||
R2 = R(1:bb,bb+1:end);
|
||||
beta = R1\R2; % the zero rows of K correspond to independent columns of WP
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ load('baseQR.mat'); % load mapping from full parameters to base parameters
|
|||
|
||||
% Choose optimization algorithm: 'patternsearch', 'ga', 'fmincon'
|
||||
optmznAlgorithm = 'patternsearch';
|
||||
searchOnlyForFeasibleSolution = 1;
|
||||
|
||||
% Getting limits on posistion and velocities. Moreover we get some
|
||||
% constant parmeters of the robot that allow us to accelerate computation
|
||||
|
|
@ -30,17 +31,17 @@ ur10.r_com = r_com;
|
|||
|
||||
|
||||
% Trajectory parameters
|
||||
traj_par.T = 20; % period of signal
|
||||
traj_par.T = 25; % period of signal
|
||||
traj_par.wf = 2*pi/traj_par.T; % fundamental frequency
|
||||
traj_par.t_smp = 1e-1; % sampling time
|
||||
traj_par.t_smp = 2e-1; % sampling time
|
||||
traj_par.t = 0:traj_par.t_smp:traj_par.T; % time
|
||||
traj_par.N = 5; % number of harmonics
|
||||
traj_par.N = 7; % number of harmonics
|
||||
traj_par.q0 = deg2rad([0 -90 0 -90 0 0 ]');
|
||||
% Use different limit for positions for safety
|
||||
traj_par.q_min = -deg2rad([180 180 160 180 90 90]');
|
||||
traj_par.q_max = deg2rad([180 0 160 0 90 90]');
|
||||
traj_par.q_min = -deg2rad([180 180 100 180 90 90]');
|
||||
traj_par.q_max = deg2rad([180 0 100 0 90 90]');
|
||||
traj_par.qd_max = qd_max;
|
||||
traj_par.q2d_max = 2*ones(6,1);
|
||||
traj_par.q2d_max = [2 1 1 1 1 2.5]';
|
||||
|
||||
% ----------------------------------------------------------------------
|
||||
% Otimization
|
||||
|
|
@ -50,19 +51,26 @@ Aeq = []; beq = [];
|
|||
lb = []; ub = [];
|
||||
|
||||
if strcmp(optmznAlgorithm, 'patternsearch')
|
||||
x0 = rand(6*2*traj_par.N,1);
|
||||
load('ptrnSrch_N7T25QR.mat')
|
||||
% x0 = rand(6*2*traj_par.N,1);
|
||||
x0 = reshape([a b], [6*2*traj_par.N, 1]);
|
||||
optns_pttrnSrch = optimoptions('patternsearch');
|
||||
optns_pttrnSrch.Display = 'iter';
|
||||
optns_pttrnSrch.StepTolerance = 1e-3;
|
||||
optns_pttrnSrch.FunctionTolerance = 1;
|
||||
optns_pttrnSrch.StepTolerance = 1e-1;
|
||||
optns_pttrnSrch.FunctionTolerance = 10;
|
||||
optns_pttrnSrch.ConstraintTolerance = 1e-6;
|
||||
optns_pttrnSrch.MaxTime = inf;
|
||||
optns_pttrnSrch.MaxFunctionEvaluations = 1e+6;
|
||||
|
||||
|
||||
if searchOnlyForFeasibleSolution
|
||||
[x,fval] = patternsearch(@(x)0, x0, A, [], Aeq, beq, lb, ub, ...
|
||||
@(x)traj_cnstr_fsblty(x, traj_par, baseQR), ...
|
||||
optns_pttrnSrch);
|
||||
else
|
||||
[x,fval] = patternsearch(@(x)traj_cost_lgr(x,traj_par,baseQR), x0, ...
|
||||
A, b, Aeq, beq, lb, ub, ...
|
||||
@(x)traj_cnstr(x,traj_par), optns_pttrnSrch);
|
||||
|
||||
end
|
||||
elseif strcmp(optmznAlgorithm, 'ga')
|
||||
optns_ga = optimoptions('ga');
|
||||
optns_ga.Display = 'iter';
|
||||
|
|
@ -120,14 +128,16 @@ subplot(3,1,3)
|
|||
grid on
|
||||
legend('q2d1','q2d2','q2d3','q2d4','q2d5','q2d6')
|
||||
|
||||
|
||||
%%
|
||||
% %{
|
||||
pathToFolder = 'trajectory_optmzn/';
|
||||
t1 = strcat('N',num2str(traj_par.N),'T',num2str(traj_par.T));
|
||||
if strcmp(optmznAlgorithm, 'patternsearch')
|
||||
filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR2.mat');
|
||||
filename = strcat(pathToFolder,'ptrnSrch_',t1,'QR.mat');
|
||||
elseif strcmp(optmznAlgorithm, 'ga')
|
||||
filename = strcat(pathToFolder,'ga_',t1,'.mat');
|
||||
elseif strcmp(optmznAlgorithm, 'fmincon')
|
||||
filename = strcat(pathToFolder,'fmncn_',t1,'.mat');
|
||||
end
|
||||
save(filename,'a','b','c_pol','traj_par')
|
||||
%}
|
||||
|
|
@ -3,10 +3,12 @@ clc; clear all; close all;
|
|||
% ------------------------------------------------------------------------
|
||||
% Load data and procces it (filter and estimate accelerations)
|
||||
% ------------------------------------------------------------------------
|
||||
unloadedTrajectory = parseURData('ur-19_12_23_free.csv', 1, 2036);
|
||||
% unloadedTrajectory = parseURData('ur-19_12_23_free.csv', 1, 2036);
|
||||
unloadedTrajectory = parseURData('ur-20_01_31-unload.csv', 300, 2623);
|
||||
unloadedTrajectory = filterData(unloadedTrajectory);
|
||||
|
||||
loadedTrajectory = parseURData('ur-20_01_13-load_2600.csv', 250, 2274);
|
||||
% loadedTrajectory = parseURData('ur-20_01_13-load_2600.csv', 250, 2274);
|
||||
loadedTrajectory = parseURData('ur-20_01_31-load.csv', 370, 2881);
|
||||
loadedTrajectory = filterData(loadedTrajectory);
|
||||
|
||||
% ------------------------------------------------------------------------
|
||||
|
|
@ -16,7 +18,8 @@ loadedTrajectory = filterData(loadedTrajectory);
|
|||
% load('full2base_mapping.mat');
|
||||
load('baseQR.mat'); % load mapping from full parameters to base parameters
|
||||
E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
||||
m_load = 2.602;
|
||||
m_load = 2.805;
|
||||
% m_load = 2.602;
|
||||
|
||||
% Constracting regressor matrix for unloaded case
|
||||
Wb_uldd = []; I_uldd = [];
|
||||
|
|
@ -165,7 +168,6 @@ sol = optimize(cnstr,obj,sdpsettings('solver','sdpt3'));
|
|||
% Getting values of the estimated patamters
|
||||
drvGainsSDP = value(drv_gns)
|
||||
|
||||
return
|
||||
%% Saving obtained drive gains
|
||||
drvGains = drvGainsSDP;
|
||||
filename = 'driveGains.mat';
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ x1 = fminsearch(@(x)costFcn(x, link1.qd_fltrd(:,1), link1.deltaTau), x10, fminsr
|
|||
x2 = fminsearch(@(x)costFcn(x, link2.qd_fltrd(:,2), link2.deltaTau), x20, fminsrchOptns);
|
||||
x3 = fminsearch(@(x)costFcn(x, link3.qd_fltrd(:,3), link3.deltaTau), x30, fminsrchOptns);
|
||||
x4 = fminsearch(@(x)costFcn(x, link4.qd_fltrd(:,4), link4.deltaTau), x40, fminsrchOptns);
|
||||
x5 = fminsearch(@(x)costFcn(x, link5.qd_fltrd(:,5), link5.deltaTau), x50, fminsrchOptns);
|
||||
x5 = fminsearch(@(x)costFcn(x, link5.qd_fltrd(:,5), link5.deltaTau), x4, fminsrchOptns);
|
||||
x6 = fminsearch(@(x)costFcn(x, link6.qd_fltrd(:,6), link6.deltaTau), x5, fminsrchOptns);
|
||||
|
||||
pi_nonlnr_frcn = [x1; x2; x3; x4; x5; x6];
|
||||
|
|
@ -77,7 +77,7 @@ plotFrictionModels(link4, 4, pi_frctn(10:12), x4)
|
|||
plotFrictionModels(link5, 5, pi_frctn(13:15), x5)
|
||||
plotFrictionModels(link6, 6, pi_frctn(16:18), x6)
|
||||
|
||||
|
||||
return
|
||||
%% Plot torque prediction with different friction models
|
||||
plotTroqueEstimation(link1, 1, drvGains(1), pi_frctn(1:3), x1)
|
||||
plotTroqueEstimation(link2, 2, drvGains(2), pi_frctn(4:6), x2)
|
||||
|
|
@ -87,8 +87,6 @@ plotTroqueEstimation(link5, 5, drvGains(5), pi_frctn(13:15), x5)
|
|||
plotTroqueEstimation(link6, 6, drvGains(6), pi_frctn(16:18), x6)
|
||||
|
||||
|
||||
|
||||
|
||||
%% Utilized Functions
|
||||
|
||||
function plotTroqueEstimation(link, noLink, drvGain, linear, nonlinear)
|
||||
|
|
@ -104,20 +102,12 @@ end
|
|||
|
||||
function plotFrictionModels(link, noLink, linear, nonlinear)
|
||||
figure
|
||||
subplot(1,2,1)
|
||||
hold on
|
||||
scatter(link.qd(:,noLink), link.deltaTau, '.')
|
||||
plot(link.qd(:,noLink), nonlinearFrictionModel(nonlinear, link.qd(:,noLink)))
|
||||
plot(link.qd(:,noLink), linearFrictionModel(linear, link.qd(:,noLink)))
|
||||
legend('real', 'nonlinear model', 'linear model','Location','northwest')
|
||||
title(strcat('Link ',num2str(noLink),' friction model'))
|
||||
subplot(1,2,2)
|
||||
hold on
|
||||
scatter(link.q(:,noLink), link.deltaTau, '.')
|
||||
plot(link.q(:,noLink), nonlinearFrictionModel(nonlinear, link.qd(:,noLink)))
|
||||
plot(link.q(:,noLink), linearFrictionModel(linear, link.qd(:,noLink)))
|
||||
legend('real', 'nonlinear model', 'linear model','Location','northwest')
|
||||
title(strcat('Link ',num2str(noLink),' friction model'))
|
||||
end
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@ clc; clear all; close all;
|
|||
% Load data and procces it (filter and estimate accelerations)
|
||||
% ------------------------------------------------------------------------
|
||||
unloadedTrajectory = parseURData('ur-19_12_23_free.csv', 1, 2005);
|
||||
% unloadedTrajectory = parseURData('ur-20_01_31-unload.csv', 300, 2623);
|
||||
unloadedTrajectory = filterData(unloadedTrajectory);
|
||||
|
||||
% ------------------------------------------------------------------------
|
||||
|
|
@ -18,11 +19,11 @@ E1 = baseQR.permutationMatrix(:,1:baseQR.numberOfBaseParameters);
|
|||
load('driveGains.mat')
|
||||
% drvGains = [14.87; 13.26; 11.13; 10.62; 11.03; 11.47]; % deLuca gains
|
||||
% drvGains = [11.1272; 11.83; 9.53; 12.64; 10.24; 5.53];
|
||||
|
||||
% drvGains = [15.87; 11.83; 9.53; 11.92; 15.24; 18.47];
|
||||
|
||||
%Constracting regressor matrix
|
||||
Wb_uldd = []; Tau_uldd = [];
|
||||
for i = 1:2:length(unloadedTrajectory.t)
|
||||
for i = 1:1:length(unloadedTrajectory.t)
|
||||
Y_ulddi = regressorWithMotorDynamics(unloadedTrajectory.q(i,:)',...
|
||||
unloadedTrajectory.qd_fltrd(i,:)',...
|
||||
unloadedTrajectory.q2d_est(i,:)');
|
||||
|
|
@ -31,9 +32,16 @@ for i = 1:2:length(unloadedTrajectory.t)
|
|||
|
||||
Wb_uldd = vertcat(Wb_uldd, Ybi_uldd);
|
||||
Tau_uldd = vertcat(Tau_uldd, diag(drvGains)*unloadedTrajectory.i_fltrd(i,:)');
|
||||
% Tau_uldd = vertcat(Tau_uldd, unloadedTrajectory.tau_des(i,:)');
|
||||
end
|
||||
|
||||
%% Usual least squares
|
||||
pib_hat = (Wb_uldd'*Wb_uldd)\(Wb_uldd'*Tau_uldd);
|
||||
|
||||
pi_b = pib_hat(1:40); % variables for base paramters
|
||||
pi_frctn = pib_hat(41:end);
|
||||
|
||||
|
||||
|
||||
|
||||
%% Set-up SDP optimization procedure
|
||||
physicalConsistency = 1;
|
||||
|
|
@ -62,6 +70,8 @@ for i = 1:6
|
|||
|
||||
end
|
||||
|
||||
|
||||
|
||||
if physicalConsistency
|
||||
for i = 1:11:66
|
||||
link_inertia_i = [pii(i), pii(i+1), pii(i+2); ...
|
||||
|
|
|
|||
|
|
@ -0,0 +1,130 @@
|
|||
% Get robot description
|
||||
run('main_ur.m')
|
||||
|
||||
%{
|
||||
q_sym = sym('q%d',[6,1], 'real');
|
||||
|
||||
T_pk = sym(zeros(4,4,6)); % transformation between links
|
||||
T_0k = sym(zeros(4,4,7)); T_0k(:,:,1) = sym(eye(4));
|
||||
|
||||
for i = 1:6
|
||||
jnt_axs_k = str2num(ur10.robot.joint{i}.axis.Attributes.xyz)';
|
||||
% Transformation from parent link frame p to current joint frame
|
||||
rpy_k = sym(str2num(ur10.robot.joint{i}.origin.Attributes.rpy));
|
||||
R_pj = RPY(rpy_k);
|
||||
R_pj(abs(R_pj)<sqrt(eps)) = sym(0); % to avoid numerical errors
|
||||
p_pj = str2num(ur10.robot.joint{i}.origin.Attributes.xyz)';
|
||||
T_pj = sym([R_pj, p_pj; zeros(1,3), 1]); % to avoid numerical errors
|
||||
% Tranformation from joint frame of the joint that rotaties body k to
|
||||
% link frame. The transformation is pure rotation
|
||||
R_jk = Rot(q_sym(i),sym(jnt_axs_k));
|
||||
p_jk = sym(zeros(3,1));
|
||||
T_jk = [R_jk, p_jk; sym(zeros(1,3)),sym(1)];
|
||||
% Transformation from parent link frame p to current link frame k
|
||||
T_pk(:,:,i) = T_pj*T_jk;
|
||||
T_0k(:,:,i+1) = T_0k(:,:,i)*T_pk(:,:,i);
|
||||
end
|
||||
pos = T_0k(1:3,4,7);
|
||||
|
||||
matlabFunction(pos,'File','autogen/position_fk_UR10E','Vars',{q_sym})
|
||||
%}
|
||||
|
||||
% load('ptrnSrch_N6T20QR.mat');
|
||||
% load('ptrnSrch_N8T20QR.mat');
|
||||
% load('ptrnSrch_N10T20QR.mat');
|
||||
load('ptrnSrch_N12T20QR.mat');
|
||||
traj_par.t_smp = 5e-2;
|
||||
traj_par.t = 0:traj_par.t_smp:traj_par.T;
|
||||
[q,qd,q2d] = mixed_traj(traj_par.t, c_pol, a, b, traj_par.wf, traj_par.N);
|
||||
|
||||
|
||||
%%
|
||||
for i = 1:size(q,2)
|
||||
% pos(:,i) = ur_fk(q(:,i), ur10);
|
||||
pos(:,i) = position_fk_UR10E(q(:,i));
|
||||
end
|
||||
|
||||
% Needed for surface
|
||||
[x, y] = meshgrid(-0.7:0.05:0.7); % Generate x and y data
|
||||
z = zeros(size(x, 1)); % Generate z data
|
||||
|
||||
figure
|
||||
hold on
|
||||
surf(x, y, z) % Plot the surface
|
||||
for i = 1:size(q,2)
|
||||
scatter3(pos(1,i), pos(2,i), pos(3,i))
|
||||
end
|
||||
xlabel('X')
|
||||
ylabel('Y')
|
||||
zlabel('Z')
|
||||
axis equal
|
||||
view(3)
|
||||
|
||||
%% write into file
|
||||
writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
|
||||
|
||||
%%
|
||||
% figure1 = figure;
|
||||
% axes1 = axes('Parent',figure1);
|
||||
% hold(axes1,'on');
|
||||
% for i = 1:6
|
||||
% scatter3(T_0k(1,4,i), T_0k(2,4,i), T_0k(3,4,i))
|
||||
% line([T_0k(1,4,i) T_0k(1,4,i+1)], [T_0k(2,4,i) T_0k(2,4,i+1)], ...
|
||||
% [T_0k(3,4,i) T_0k(3,4,i+1)])
|
||||
% end
|
||||
% xlabel('X')
|
||||
% ylabel('Y')
|
||||
% zlabel('Z')
|
||||
% axis equal
|
||||
|
||||
|
||||
function writeTrajectoryCoefficientsIntoFile(a, b, c_pol)
|
||||
prcsn = 10;
|
||||
a_coefs = {};
|
||||
b_coefs = {};
|
||||
a_coefs{1} = 'a1 = ['; a_coefs{2} = 'a2 = ['; a_coefs{3} = 'a3 = [';
|
||||
a_coefs{4} = 'a4 = ['; a_coefs{5} = 'a5 = ['; a_coefs{6} = 'a6 = [';
|
||||
b_coefs{1} = 'b1 = ['; b_coefs{2} = 'b2 = ['; b_coefs{3} = 'b3 = [';
|
||||
b_coefs{4} = 'b4 = ['; b_coefs{5} = 'b5 = ['; b_coefs{6} = 'b6 = [';
|
||||
for i = 1:size(a,2)
|
||||
if i < size(a,2)
|
||||
for j = 1:6
|
||||
a_coefs{j} = strcat(a_coefs{j}, num2str(a(j,i), prcsn), ',');
|
||||
b_coefs{j} = strcat(b_coefs{j}, num2str(b(j,i), prcsn), ',');
|
||||
end
|
||||
elseif i == size(a,2)
|
||||
for j = 1:6
|
||||
a_coefs{j} = strcat(a_coefs{j}, num2str(a(j,i), prcsn), ']\n');
|
||||
b_coefs{j} = strcat(b_coefs{j}, num2str(b(j,i), prcsn), ']\n');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
c_coefs = {};
|
||||
c_coefs{1} = 'c1 = ['; c_coefs{2} = 'c2 = ['; c_coefs{3} = 'c3 = [';
|
||||
c_coefs{4} = 'c4 = ['; c_coefs{5} = 'c5 = ['; c_coefs{6} = 'c6 = [';
|
||||
for i = 1:size(c_pol,2)
|
||||
if i < size(c_pol,2)
|
||||
for j = 1:6
|
||||
c_coefs{j} = strcat(c_coefs{j}, num2str(c_pol(j,i), prcsn+2), ',');
|
||||
end
|
||||
elseif i == size(c_pol,2)
|
||||
for j = 1:6
|
||||
c_coefs{j} = strcat(c_coefs{j}, num2str(c_pol(j,i), prcsn+2), ']\n');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
fileID_a = fopen('trajectory_optmzn/coeffs4_UR/a_coeffs.script','w');
|
||||
fileID_b = fopen('trajectory_optmzn/coeffs4_UR/b_coeffs.script','w');
|
||||
fileID_c = fopen('trajectory_optmzn/coeffs4_UR/c_coeffs.script','w');
|
||||
for i = 1:6
|
||||
fprintf(fileID_a, a_coefs{i});
|
||||
fprintf(fileID_b, b_coefs{i});
|
||||
fprintf(fileID_c, c_coefs{i});
|
||||
end
|
||||
fclose(fileID_a);
|
||||
fclose(fileID_b);
|
||||
fclose(fileID_c);
|
||||
|
||||
end
|
||||
49
ur_vldtn.m
49
ur_vldtn.m
|
|
@ -5,19 +5,14 @@
|
|||
% vldtnTrjctry = parseURData('ur-19_10_01-14_04_13.csv', 1, 759);
|
||||
% vldtnTrjctry = parseURData('ur-19_10_01-13_51_41.csv', 1, 660);
|
||||
% vldtnTrjctry = parseURData('ur-19_09_27-11_28_22.csv', 1, 594);
|
||||
% vldtnTrjctry = parseURData('ur-20_01_17-p1.csv', 1, 250);
|
||||
vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
|
||||
|
||||
vldtnTrjctry = parseURData('ur-20_01_17-p2.csv', 1, 250);
|
||||
% vldtnTrjctry = parseURData('ur-20_01_17-ptp_10_points.csv', 1, 5346);
|
||||
vldtnTrjctry = filterData(vldtnTrjctry);
|
||||
|
||||
|
||||
% -----------------------------------------------------------------------
|
||||
% Predicting torques
|
||||
% -----------------------------------------------------------------------
|
||||
pi_rigidBody = reshape(identifiedUR10E.standardParameters,[11,6]);
|
||||
pi_rigidBody = pi_rigidBody(1:10,:);
|
||||
pi_rigidBody = reshape(pi_rigidBody,[60 1]);
|
||||
|
||||
%Constracting regressor matrix
|
||||
tau_msrd = []; tau_prdctd1 = []; tau_prdctd2 = [];
|
||||
i_prdct1 = []; i_prdct2 = [];
|
||||
|
|
@ -26,7 +21,7 @@ for i = 1:length(vldtnTrjctry.t)
|
|||
vldtnTrjctry.qd_fltrd(i,:)',...
|
||||
vldtnTrjctry.q2d_est(i,:)');
|
||||
|
||||
tau_withoutFriction = Yi*E1*identifiedUR10E.baseParameters;
|
||||
tau_withoutFriction = Yi*E1*pi_b;
|
||||
|
||||
tau_lnr_frcn = zeros(6,1);
|
||||
% tau_nonlnr_frcn = zeros(6,1);
|
||||
|
|
@ -36,7 +31,7 @@ for i = 1:length(vldtnTrjctry.t)
|
|||
% tau_nonlnr_frcn(j) = nonlinearFrictionModel(pi_nonlnr_frcn(5*(j-1)+1:5*(j-1)+5),...
|
||||
% vldtnTrjctry.qd_fltrd(i,j)');
|
||||
end
|
||||
tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i_fltrd(i,:)');
|
||||
tau_msrd = horzcat(tau_msrd, diag(drvGains)*vldtnTrjctry.i(i,:)');
|
||||
tau_prdctd1 = horzcat(tau_prdctd1, tau_withoutFriction + tau_lnr_frcn);
|
||||
% tau_prdctd2 = horzcat(tau_prdctd2, tau_withoutFriction + tau_nonlnr_frcn);
|
||||
|
||||
|
|
@ -45,16 +40,44 @@ for i = 1:length(vldtnTrjctry.t)
|
|||
end
|
||||
|
||||
%%
|
||||
plotTorquesWithLinearFriction = 0;
|
||||
plotTorquesWithNonlinearFriction = 0;
|
||||
|
||||
for i = 1:6
|
||||
figure
|
||||
hold on
|
||||
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
|
||||
% plot(vldtnTrjctry.t, vldtnTrjctry.tau_des(:,i))
|
||||
plot(vldtnTrjctry.t, tau_prdctd1(i,:),'k-')
|
||||
legend('measured', 'desired', 'predicted')
|
||||
plot(vldtnTrjctry.t, vldtnTrjctry.i(:,i), 'r-')
|
||||
plot(vldtnTrjctry.t, i_prdct1(i,:), 'k-')
|
||||
legend('measured', 'prdctd1')
|
||||
grid on
|
||||
end
|
||||
|
||||
|
||||
|
||||
if plotTorquesWithLinearFriction
|
||||
for i = 1:6
|
||||
figure
|
||||
hold on
|
||||
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
|
||||
plot(vldtnTrjctry.t, tau_prdctd1(i,:), 'k-')
|
||||
plot(vldtnTrjctry.t, tau_prdctd1(i,:) - tau_msrd(i,:), '--')
|
||||
legend('measured', 'predicted', 'error')
|
||||
grid on
|
||||
end
|
||||
end
|
||||
|
||||
if plotTorquesWithNonlinearFriction
|
||||
for i = 1:6
|
||||
figure
|
||||
hold on
|
||||
plot(vldtnTrjctry.t, tau_msrd(i,:), 'r-')
|
||||
plot(vldtnTrjctry.t, tau_prdctd2(i,:), 'k-')
|
||||
plot(vldtnTrjctry.t, tau_prdctd2(i,:) - tau_msrd(i,:), '--')
|
||||
legend('measured', 'predicted', 'error')
|
||||
grid on
|
||||
end
|
||||
end
|
||||
|
||||
return
|
||||
for i = 1:6
|
||||
figure
|
||||
|
|
|
|||
Loading…
Reference in New Issue