HW_10_Bonus_Solution
pdf
keyboard_arrow_up
School
Purdue University *
*We aren’t endorsed by this school
Course
56900
Subject
Mathematics
Date
Jan 9, 2024
Type
Pages
14
Uploaded by DeaconWallaby811
HW 10 Solution Code: Problem 1
clear all
;
close all
;
clc;
(a) Calculate arc length of Lissajous Curve.
T = 2*pi;
t = linspace(0,T,1000);
xd = 0.16*sin(1*t);
yd = 0.08*sin(2*t);
d = 0;
for i=1:length(t)-1
d = d + sqrt((xd(i+1)-xd(i))^2 + (yd(i+1)-yd(i))^2);
end
Determine the average speed, c
, of end effector over tfinal
seconds.
tfinal = 5;
c = d/tfinal;
(b) Use forward-euler method to numerically approximate dt = 0.01;
t = 0:dt:tfinal;
alpha = zeros(size(t));
for i=1:length(t)-1
a = alpha(i);
alpha(i+1) = a + c/sqrt((0.16*cos(a))^2 + (0.16*cos(2*a))^2)*dt;
end
plot(t,alpha,
'LineWidth'
,2); title(
'\alpha(t)'
);
grid on
; xlabel(
'time (s)'
); ylabel(
'\alpha(t)'
);
yline(T,
'k--'
,
'LineWidth'
,2); legend(
'\alpha'
,
'\alpha(end)'
,
'Location'
,
'southeast'
)
1
Combine all trajectories together.
x = 0.16*sin(alpha);
y = 0.08*sin(2*alpha);
Plot the speed of the trajectory as function of time.
v = sqrt( (diff(x)/dt).^2 + (diff(y)/dt).^2 );
plot(dt:dt:tfinal,v,
'LineWidth'
,3); hold on
;
yline(c,
'--'
,
'Color'
,
'k'
); ylim([0 0.3])
hold off
;
title(
'Speed of End Effector'
)
grid on
;
xlabel(
'time (s)'
)
ylabel(
'speed (m/s)'
)
2
(d) Create an animation of end effector position. This may take a while to run.
% first, make parametric plot of trajectory
plot(x,y,
'LineWidth'
,2); grid on
; hold on
;
m = line(
'xdata'
,x(1), 'ydata'
, y(1), 'marker'
, 'o'
, ...
'markersize'
, 10, 'MarkerFaceColor'
, 'r'
);
hold off
;
title(
'Lissajous Trajectory'
);
xlabel(
'x position (m)'
); ylabel(
'y position (m)'
); xlim([min(x)-0.1 max(x)+0.1]);
ylim([min(y)-0.1 max(y)+0.1]);
set(gca,
'dataaspectratio'
, [1 1 1]);
% then create the video
vidObj = VideoWriter(
'MyTrajectoryName'
);
open(vidObj);
frameRate = 30;
nframes = floor(tfinal*frameRate);
Frames = moviein(nframes);
for i=1:nframes
3
Your preview ends here
Eager to read complete document? Join bartleby learn and gain access to the full version
- Access to all documents
- Unlimited textbook solutions
- 24/7 expert homework help
% use interpolation to get end effector
% position at frame i
x_i = interp1(t, x, (i-1)/frameRate);
y_i = interp1(t, y, (i-1)/frameRate);
% update the graphics
set(m,
'xdata'
,x_i, 'ydata'
, y_i);
drawnow;
% save the graphics
Frames(:,i) = getframe;
writeVideo(vidObj,Frames(:,i))
end
close(vidObj); % very important to close the video object!
(e) Forward Kinematics
% Calculate starting Tsd(0) := T0
theta_0 = [-1.6800 -1.4018 -1.8127 -2.9937 -0.8857 3.0720];
T0 = ECE569_FK(theta_0);
% ur3e = loadrobot("universalUR3e","DataFormat","row");
% show(ur3e,theta_0);
(f) Calculate Tsd at every time step.
4
% Calculate Tsd(t) for t=0 to t=tfinal
N = length(t);
Tsd = zeros(4,4,N);
for i=1:N
Tsd(:,:,i) = T0*[eye(3) [-x(i);-y(i);0]; 0 0 0 1];
end
Problem 2
(a) Implement your own inverse kinematics solver. Before running the inverse kinematics for every time step, try testing your algorithm on (we know the answer should be very close to .)
% Test Inverse Kinematics algorithm by
% making initial guess close to theta_0
initialguess = (theta_0 + 0.01)';
Td = T0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Complete the function to compute inverse kinematics
% given a desired Td, and an initial guess.
% Should return thetaSol as a column vector.
thetaSol = ECE569_IK(Td,initialguess);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% end effector should be very close to desired location
% so this difference should be small
norm(T0 - ECE569_FK(thetaSol))
ans = 1.0961e-06
Now run the inverse kinematics solver for every time step. If inverse kinematics is working properly, this code should not need to be modified.
close all
;
% each row of theta_total is the joint angles for a time step
% that is, at time step k, theta_k = theta_total(k,:)
theta_total = zeros(N,6);
theta_total(1,:) = theta_0;
ur3e = loadrobot(
"universalUR3e"
,
"DataFormat"
,
"row"
);
show(ur3e,theta_0);
5
f = waitbar(0,[
'Inverse Kinematics (1/'
,num2str(N),
') complete.'
]);
for i=2:N
% use previous solution as current guess
initialguess = theta_total(i-1,:)';
Td = Tsd(:,:,i);
thetaSol = ECE569_IK(Td,initialguess);
theta_total(i,:) = thetaSol';
waitbar(i/N,f,[
'Inverse Kinematics ('
,num2str(i),
'/'
,num2str(N),
') complete.'
]);
% show every 10th pose
if mod(i,10) == 0
show(ur3e,thetaSol');
end
end
6
Your preview ends here
Eager to read complete document? Join bartleby learn and gain access to the full version
- Access to all documents
- Unlimited textbook solutions
- 24/7 expert homework help
close(f);
(b) Write to CSV
% write to CSV
A = [t' theta_total];
writematrix(A,
'pete7.csv'
)
(c) Example implementation using MATLAB's toolbox (not allowed for HW, but is helpful for learning!)
% where the joint values will live
theta_total = zeros(N,6);
% inverse kinematics solver
ur3e = loadrobot(
"universalUR3e"
,
"DataFormat"
,
"row"
);
ik = inverseKinematics(
"RigidBodyTree"
,ur3e);
weights = [1 1 1 1 1 1];
theta_total(1,:) = theta_0;
show(ur3e,theta_0);
7
f = waitbar(0,[
'Inverse Kinematics (1/'
,num2str(N),
') complete.'
]);
for i=2:N
initialguess = theta_total(i-1,:);
[configSol,solInfo] = ik(
"tool0"
,Tsd(:,:,i),weights,initialguess);
theta_total(i,:) = configSol;
waitbar(i/N,f,[
'Inverse Kinematics ('
,num2str(i),
'/'
,num2str(N),
') complete.'
]);
% show every 10th pose
if mod(i,10) == 0
show(ur3e,configSol);
end
end
8
close(f);
Inverse Kinematics Function
function thetaSol = ECE569_IK(Td,initialguess)
% initialguess and thetaSol are col vectors
% Td is the desired pose in base frame
% current T(theta)
theta = initialguess;
T_theta = ECE569_FK(theta);
% initial error
e = Vee4(Log4(T_theta\Td));
ew = e(1:3);
ev = e(4:6);
ew_max = 1e-6;
ev_max = 1e-6;
% step size
ds = 1;
% max iterations
i = 0;
imax = 500;
9
Your preview ends here
Eager to read complete document? Join bartleby learn and gain access to the full version
- Access to all documents
- Unlimited textbook solutions
- 24/7 expert homework help
while (norm(ew) > ew_max) || (norm(ev) > ev_max)
Jb = CalculateJb(theta);
theta = theta + (Jb\e) * ds;
T_theta = ECE569_FK(theta);
e = Vee4(Log4(T_theta\Td));
ew = e(1:3);
ev = e(4:6);
% maximum iterations
i = i + 1;
if i >= imax
break
;
end
end
thetaSol = theta;
end
Helper functions for IK
function w_hat = Hat3(w)
w_hat = [0 -w(3) w(2); w(3) 0 -w(1); -w(2) w(1) 0];
end
function S_hat = Hat6(s)
% s = [w; v] where w and v are 3x1 column vectors
w = s(1:3);
v = s(4:6);
w_hat = Hat3(w);
S_hat = [w_hat v; zeros(1,4)];
end
function w = Vee3(w_hat)
w = zeros(3,1);
w(1) = w_hat(3,2);
w(2) = w_hat(1,3);
w(3) = w_hat(2,1);
end
function Vb = Vee4(Vb_hat)
wb_hat = Vb_hat(1:3,1:3);
wb = Vee3(wb_hat);
vb = Vb_hat(1:3,4);
Vb = [wb; vb];
end
function A = Adjoint(T)
% T = [R p; 0 0 0 1]
R = T(1:3,1:3);
10
p = T(1:3,4);
A = [R zeros(3,3); Hat3(p)*R R];
end
function A = Adjoint_Inv(T)
% T = [R p; 0 0 0 1]
R = T(1:3,1:3);
p = T(1:3,4);
A = [R' -R'*Hat3(p); zeros(3,3) R'];
end
function so3mat = Log3(R)
epsilon=1e-6;
cos_theta = (trace(R) - 1) / 2;
if cos_theta >= 1
so3mat = zeros(3);
elseif cos_theta <= -1
if norm(1 + R(3, 3))>epsilon
omg = (1 / sqrt(2 * (1 + R(3, 3)))) ...
* [R(1, 3); R(2, 3); 1 + R(3, 3)];
elseif norm(1 + R(2, 2))>epsilon
omg = (1 / sqrt(2 * (1 + R(2, 2)))) ...
* [R(1, 2); 1 + R(2, 2); R(3, 2)];
else
omg = (1 / sqrt(2 * (1 + R(1, 1)))) ...
* [1 + R(1, 1); R(2, 1); R(3, 1)];
end
theta=pi;
so3mat = pi*Hat3(omg); else
theta = acos(cos_theta);
so3mat = theta * (1 / (2 * sin(theta))) * (R - R');
end
end
function exp_screwhat_theta = Expm4(screw, theta)
% decompose the Screw
omega = screw(1:3);
v = screw(4:6);
omegahat = Hat3(omega);
% define the R=e^{omegahat * theta} using Rodrigues formula.
exp_omegahat_theta = eye(3) + sin(theta)*omegahat+(1-cos(theta))*omegahat^2;
% Compute the e^{screwhat*theta}
exp_screwhat_theta = zeros(4,4); % initialize
exp_screwhat_theta(1:3,1:3) = exp_omegahat_theta; % define the rotational matrix
11
% Chcek if ||w||=0, to compute the position
if norm(omega)==0
exp_screwhat_theta(1:3,4) = v*theta;
else
exp_screwhat_theta(1:3,4) = (eye(3)-exp_omegahat_theta)*omegahat*v + omega*(omega'*v)*theta;
end
exp_screwhat_theta(4,4) = 1; % Homogeneous coordinate.
end
function logT = Log4(T)
epsilon = 1e-7;
logT = zeros(4,4); % Initialize
% Get omega
R = T(1:3,1:3);
omegahat = Log3(R);
omega = Vee3(omegahat); % Get theta
theta = norm(omega);
% Find v
p = T(1:3,4);
if theta<epsilon
logT(1:3,4) = p; % w=0 and v=p twist hat
else
A_omega_theta = (eye(3)-R)*omegahat/((theta)^2) +omega*omega'/((theta)^2);
v= A_omega_theta\p;
logT(1:3,1:3) = omegahat;
logT(1:3,4) = v;
end
end
function T = ECE569_FK(theta)
L1 = 0.2435;
L2 = 0.2132;
W1 = 0.1311;
W2 = 0.0921;
H1 = 0.1519;
H2 = 0.0854;
M = [-1 0 0 L1+L2;
0 0 1 W1+W2;
0 1 0 H1-H2;
0 0 0 1];
S1 = [0 0 1 0 0 0]';
12
Your preview ends here
Eager to read complete document? Join bartleby learn and gain access to the full version
- Access to all documents
- Unlimited textbook solutions
- 24/7 expert homework help
S2 = [0 1 0 -H1 0 0]';
S3 = [0 1 0 -H1 0 L1]';
S4 = [0 1 0 -H1 0 L1+L2]';
S5 = [0 0 -1 -W1 L1+L2 0]';
S6 = [0 1 0 H2-H1 0 L1+L2]';
E1 = Expm4(S1, theta(1));
E2 = Expm4(S2, theta(2));
E3 = Expm4(S3, theta(3));
E4 = Expm4(S4, theta(4));
E5 = Expm4(S5, theta(5));
E6 = Expm4(S6, theta(6));
% print out Tsb_1
T = E1*E2*E3*E4*E5*E6*M;
end
function Jb = CalculateJb(theta)
% Length information of the UR3e
L1 = 0.2435;
L2 = 0.2132;
W1 = 0.1311;
W2 = 0.0921;
H1 = 0.1519;
H2 = 0.0854;
M = [-1 0 0 L1+L2;
0 0 1 W1+W2;
0 1 0 H1-H2;
0 0 0 1];
S1 = [0 0 1 0 0 0]';
S2 = [0 1 0 -H1 0 0]';
S3 = [0 1 0 -H1 0 L1]';
S4 = [0 1 0 -H1 0 L1+L2]';
S5 = [0 0 -1 -W1 L1+L2 0]';
S6 = [0 1 0 H2-H1 0 L1+L2]';
B1 = Adjoint_Inv(M)*S1;
B2 = Adjoint_Inv(M)*S2;
B3 = Adjoint_Inv(M)*S3;
B4 = Adjoint_Inv(M)*S4;
B5 = Adjoint_Inv(M)*S5;
B6 = Adjoint_Inv(M)*S6;
EB2 = Expm4(B2,-theta(2));
EB3 = Expm4(B3,-theta(3));
EB4 = Expm4(B4,-theta(4));
EB5 = Expm4(B5,-theta(5));
EB6 = Expm4(B6,-theta(6));
13
Jb1 = Adjoint(EB6*EB5*EB4*EB3*EB2)*B1;
Jb2 = Adjoint(EB6*EB5*EB4*EB3)*B2;
Jb3 = Adjoint(EB6*EB5*EB4)*B3;
Jb4 = Adjoint(EB6*EB5)*B4;
Jb5 = Adjoint(EB6)*B5;
Jb6 = B6;
Jb = [Jb1 Jb2 Jb3 Jb4 Jb5 Jb6];
end
14