Consider a 4-Link robot manipulator shown below. Use the forward kinematic D-H table and
write an m file that plots the manipulator. The instructions are given in the module 6. Submit
your solutions by the due date, in a single MATLAB m file.
Solution
Please give the kinetic D-H table else it would be difficult to code as we need to know the
rotation spin axis and other momentum of manipulator
Stating a general example code for manipulator with data
function X = fwd_kin(q,x)
% given a position in the configuration space, calculate the position of
% the end effector in the workspace for a two-link manipulator.
% q: vector of joint positions
% x: design vector (link lengths)
% X: end effector position in cartesian coordinates
% configuration space coordinates:
q1 = q(1); % theta 1
q2 = q(2); % theta 2
% manipulator parameters:
l1 = x(1); % link 1 length
l2 = x(2); % link 2 length
% calculate end effector position:
X = [l1*cos(q1) + l2*cos(q1+q2)
l1*sin(q1) + l2*sin(q1+q2)];
% SimulateTwolink.m uses inverse dynamics to simulate the torque
% trajectories required for a two-link planar robotic manipulator to follow
% a prescribed trajectory. It also computes total energy consumption. This
% code is provided as supplementary material for the paper:
%
% \'Engineering System Co-Design with Limited Plant Redesign\'
% Presented at the 8th AIAA Multidisciplinary Design Optimization
% Specialist Conference, April 2012.
%
% The paper is available from:
%
% http://systemdesign.illinois.edu/publications/All12a.pdf
%
% Here both the physical system design and control system design are
% considered simultaneously. Manipulator link length and trajectory
% specification can be specified, and torque trajectory and energy
% consumption are computed based on this input. It was found that maximum
% torque and total energy consumption calculated using inverse dynamics
% agreed closely with results calculated using feedback linearization, so
% to simplify optimization problem solution an inverse dynamics approach
% was used, which reduces the control design vector to just the trajectory
% design.
%
% In the conference paper several cases are considered, each with its own
% manipulator task, manipulator design, and trajectory design. The
% specifications for each of these five cases are provided here, and can be
% explored by changing the case number variable (cn).
%
% This code was incorporated into a larger optimization project. The code
% presented here includes only the analysis portion of the code, no
% optimization.
%
% A video illustrating the motion of each of these five cases is available
% on YouTube:
%
% http://www.youtube.com/watch?v=OR7Y9-n5SjA
%
% Author: James T. Allison, Assistant Professor, University of Illinois at
% Urbana-Champaign
% Date: 4/10/12
clear;clc
% simulation parameters:
p.dt = 0.0005; % simulation step size
tf = 2; p.tf = tf; % final time
p.ploton = 0; % turn off additional plotting capabilities
p.ploton2 = 0;
p.Tallow = 210; % maximum .
Consider a 4-Link robot manipulator shown below. Use the forward kine.pdf
1. Consider a 4-Link robot manipulator shown below. Use the forward kinematic D-H table and
write an m file that plots the manipulator. The instructions are given in the module 6. Submit
your solutions by the due date, in a single MATLAB m file.
Solution
Please give the kinetic D-H table else it would be difficult to code as we need to know the
rotation spin axis and other momentum of manipulator
Stating a general example code for manipulator with data
function X = fwd_kin(q,x)
% given a position in the configuration space, calculate the position of
% the end effector in the workspace for a two-link manipulator.
% q: vector of joint positions
% x: design vector (link lengths)
% X: end effector position in cartesian coordinates
% configuration space coordinates:
q1 = q(1); % theta 1
q2 = q(2); % theta 2
% manipulator parameters:
l1 = x(1); % link 1 length
l2 = x(2); % link 2 length
% calculate end effector position:
X = [l1*cos(q1) + l2*cos(q1+q2)
l1*sin(q1) + l2*sin(q1+q2)];
% SimulateTwolink.m uses inverse dynamics to simulate the torque
% trajectories required for a two-link planar robotic manipulator to follow
% a prescribed trajectory. It also computes total energy consumption. This
% code is provided as supplementary material for the paper:
%
% 'Engineering System Co-Design with Limited Plant Redesign'
% Presented at the 8th AIAA Multidisciplinary Design Optimization
% Specialist Conference, April 2012.
%
% The paper is available from:
%
% http://systemdesign.illinois.edu/publications/All12a.pdf
2. %
% Here both the physical system design and control system design are
% considered simultaneously. Manipulator link length and trajectory
% specification can be specified, and torque trajectory and energy
% consumption are computed based on this input. It was found that maximum
% torque and total energy consumption calculated using inverse dynamics
% agreed closely with results calculated using feedback linearization, so
% to simplify optimization problem solution an inverse dynamics approach
% was used, which reduces the control design vector to just the trajectory
% design.
%
% In the conference paper several cases are considered, each with its own
% manipulator task, manipulator design, and trajectory design. The
% specifications for each of these five cases are provided here, and can be
% explored by changing the case number variable (cn).
%
% This code was incorporated into a larger optimization project. The code
% presented here includes only the analysis portion of the code, no
% optimization.
%
% A video illustrating the motion of each of these five cases is available
% on YouTube:
%
% http://www.youtube.com/watch?v=OR7Y9-n5SjA
%
% Author: James T. Allison, Assistant Professor, University of Illinois at
% Urbana-Champaign
% Date: 4/10/12
clear;clc
% simulation parameters:
p.dt = 0.0005; % simulation step size
tf = 2; p.tf = tf; % final time
p.ploton = 0; % turn off additional plotting capabilities
p.ploton2 = 0;
p.Tallow = 210; % maximum torque requirement
% Case number:
3. cn = 4;
switch cn
case 1 % Nominal system design (sequential design, optimal control)
% Task A:
p.task = 1;
% plant design:
L = [0.6 0.6];
% trajectory design:
Xtraj = [0.18333 -0.083583 0.014569 0.14199]';
case 2 % Task A optimal co-design
% Task A:
p.task = 1;
% plant design:
L = [1.7715 1.63455];
% trajectory design:
Xtraj = [0.11281 0.120718 0.0502807 -0.43749]';
case 3 % Task B with Task A optimal design
% Task A:
p.task = 2;
% plant design:
L = [1.7715 1.63455];
% trajectory design:
Xtraj = [0.81030 0.99999 -1.09068 -0.0056275]';
case 4 % Task B co-design
% Task A:
p.task = 2;
% plant design:
L = [2.2797 1.13688];
% trajectory design:
Xtraj = [0.69136 1.5248 -0.75799 -0.061826]';
case 5 % Task B PLCD
% Task B:
p.task = 2;
% plant design:
L = [1.7715 1.100];
% trajectory design:
10. traj_qdd2 = @(t) 2*a2(3)+6*a2(4)*t+12*a2(5)*t.^2+20*a2(6)*t.^3;
% form output argument
traj.traj_q1 = traj_q1;
traj.traj_q2 = traj_q2;
traj.traj_qd1 = traj_qd1;
traj.traj_qd2 = traj_qd2;
traj.traj_qdd1 = traj_qdd1;
traj.traj_qdd2 = traj_qdd2;
% plot
if ploton
figure(1);clf
fplot(traj_q1,[0 tf]); hold on
set(findobj(gca, 'Type', 'Line'), 'LineWidth', 2,'Color', 'k');
fplot(traj_q2,[0 tf],'r-'); hold on
set(findobj(gca, 'Type', 'Line'), 'LineWidth', 2);
legend('q_1','q_2')
title('Position Trajectories')
% plot
figure(2);clf
fplot(traj_qd1,[0 tf]); hold on
set(findobj(gca, 'Type', 'Line'), 'LineWidth', 2,'Color', 'k');
fplot(traj_qd2,[0 tf],'r-'); hold on
set(findobj(gca, 'Type', 'Line'), 'LineWidth', 2);
legend('qd_1','qd_2')
title('Velocity Trajectories')
end
function u = inv_control(q,qd,qdd,X)
% Compute required input torque as a function of desired trajectory and the
% extended plant design vector.
% check vector orientation:
if isrow(q)
q = q';
end
if isrow(qd)
11. qd = qd';
end
if isrow(qdd)
qdd = qdd';
end
q1 = q(1);
q2 = q(2);
q1d = qd(1);
q2d = qd(2);
% physical constants:
g = 9.81; % gravitational constant
% manipulator parameters:
l1 = X(1); % link 1 length
l2 = X(2); % link 2 length
m1 = X(3); % link 1 mass
m2 = X(4); % link 2 mass
mp = X(5); % payload mass
% dependent parameters:
lc1 = l1/2; % link 1 cm location
lc2 = l2*(m2/2+mp)/(m2+mp);
% link 2 cm location
I1 = m1*l1^2/12; % link 1 mass moment of inertia
Ilink = m2*l2^2/12 + m2*(lc2-l2/2)^2;
Ipayload = mp*(l2-lc2)^2;
I2 = Ilink+Ipayload;% link 2 mass moment of inertia
% update total link masses for D(q) calculation:
m2 = m2 + mp;
% form inertia matrix:
D(1,1) = m1*lc1^2 + m2*(l1^2+lc2^2+2*l1*lc2*cos(q2)) + I1 + I2;
D(1,2) = m2*(lc2^2+l1*lc2*cos(q2)) + I2;
D(2,1) = D(1,2);
D(2,2) = m2*lc2^2 + I2;
% form damping matrix:
h = -m2*l1*lc2*sin(q2);
C(1,1) = h*q2d;
C(1,2) = h*q2d + h*q1d;
12. C(2,1) = -h*q1d;
% form gravity vector:
G(1,1) = (m1*lc1+m2*l1)*g*cos(q1) + m2*lc2*g*cos(q1+q2);
G(2,1) = m2*lc2*g*cos(q1+q2);
% compute required torque (outputs column vector)
u = D*qdd + C*qd + G;
function qd = inv_vel(xi,q,x)
% Given a velocity of the end effector in the workspace, calculate the
% velocity in the configuration space.
% configuration space coordinates:
q1 = q(1); % theta 1
q2 = q(2); % theta 2
% manipulator parameters:
l1 = x(1); % link 1 length
l2 = x(2); % link 2 length
% Jacobian:
J = [(-l1*sin(q1) - l2*sin(q1+q2)) (-l2*sin(q1+q2))
(l1*cos(q1) + l2*cos(q1+q2)) (l2*cos(q1+q2))];
% Configuration space velocity:
if cond(J) > 1000
qd = 0;
return
end
qd = Jxi;
function q = inv_kin(X,x)
% Given a position of the end effector in the workspace, calculate the
% position in the configuration space.
% workspace coordinates:
xc = X(1); % horizontal
yc = X(2); % vertical
% manipulator parameters:
l1 = x(1); % link 1 length
l2 = x(2); % link 2 length
D = (xc^2+yc^2-l1^2-l2^2)/(2*l1*l2);
% this assumes elbow down position
q2 = atan2(-sqrt(1-D^2),D);