79 lines
3.1 KiB
Mathematica
79 lines
3.1 KiB
Mathematica
|
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
|
||
|
|
|
||
|
|
function taumat ...
|
||
|
|
= InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat,g, ...
|
||
|
|
Ftipmat,Mlist,Glist,Slist)
|
||
|
|
% Takes thetamat: An N x n matrix of robot joint variables,
|
||
|
|
% dthetamat: An N x n matrix of robot joint velocities,
|
||
|
|
% ddthetamat: An N x n matrix of robot joint accelerations,
|
||
|
|
% g: Gravity vector g,
|
||
|
|
% Ftipmat: An N x 6 matrix of spatial forces applied by the
|
||
|
|
% end-effector (If there are no tip forces, the user should
|
||
|
|
% input a zero and a zero matrix will be used),
|
||
|
|
% Mlist: List of link frames i relative to i-1 at the home position,
|
||
|
|
% Glist: Spatial inertia matrices Gi of the links,
|
||
|
|
% Slist: Screw axes Si of the joints in a space frame.
|
||
|
|
% Returns taumat: The N x n matrix of joint forces/torques for the
|
||
|
|
% specified trajectory, where each of the N rows is the
|
||
|
|
% vector of joint forces/torques at each time step.
|
||
|
|
% This function uses InverseDynamics to calculate the joint forces/torques
|
||
|
|
% required to move the serial chain along the given trajectory.
|
||
|
|
% Example Inputs (3 Link Robot)
|
||
|
|
%{
|
||
|
|
clc;clear;
|
||
|
|
%Create a trajectory to follow using functions from Chapter 9
|
||
|
|
thetastart = [0; 0; 0];
|
||
|
|
thetaend = [pi/2; pi/2; pi/2];
|
||
|
|
Tf = 3;
|
||
|
|
N= 1000;
|
||
|
|
method = 5 ;
|
||
|
|
traj = JointTrajectory(thetastart,thetaend,Tf,N,method);
|
||
|
|
thetamat = traj;
|
||
|
|
dthetamat = zeros(1000,3);
|
||
|
|
ddthetamat = zeros(1000,3);
|
||
|
|
dt = Tf / (N-1);
|
||
|
|
for i = 1:N - 1
|
||
|
|
dthetamat(i + 1,:) = (thetamat(i + 1,:)-thetamat(i,:)) / dt;
|
||
|
|
ddthetamat(i + 1,:) = (dthetamat(i + 1,:)-dthetamat(i,:)) / dt;
|
||
|
|
end
|
||
|
|
%Initialise robot descripstion (Example with 3 links)
|
||
|
|
g = [0; 0; -9.8];
|
||
|
|
Ftipmat = ones(N,6);
|
||
|
|
M01 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.089159]; [0, 0, 0, 1]];
|
||
|
|
M12 = [[0, 0, 1, 0.28]; [0, 1, 0, 0.13585]; [-1, 0 ,0, 0]; [0, 0, 0, 1]];
|
||
|
|
M23 = [[1, 0, 0, 0]; [0, 1, 0, -0.1197]; [0, 0, 1, 0.395]; [0, 0, 0, 1]];
|
||
|
|
M34 = [[1, 0, 0, 0]; [0, 1, 0, 0]; [0, 0, 1, 0.14225]; [0, 0, 0, 1]];
|
||
|
|
G1 = diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]);
|
||
|
|
G2 = diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]);
|
||
|
|
G3 = diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]);
|
||
|
|
Glist = cat(3,G1,G2,G3);
|
||
|
|
Mlist = cat(4,M01,M12,M23,M34);
|
||
|
|
Slist = [[1; 0; 1; 0; 1; 0], ...
|
||
|
|
[0; 1; 0; -0.089; 0; 0], ...
|
||
|
|
[0; 1; 0; -0.089; 0; 0.425]];
|
||
|
|
taumat = InverseDynamicsTrajectory(thetamat,dthetamat,ddthetamat,g, ...
|
||
|
|
Ftipmat,Mlist,Glist,Slist);
|
||
|
|
%Output using matplotlib to plot the joint forces/torques
|
||
|
|
time=0:dt:Tf;
|
||
|
|
plot(time,taumat(:,1),'b')
|
||
|
|
hold on
|
||
|
|
plot(time,taumat(:,2),'g')
|
||
|
|
plot(time,taumat(:,3),'r')
|
||
|
|
title('Plot for Torque Trajectories')
|
||
|
|
xlabel('Time')
|
||
|
|
ylabel('Torque')
|
||
|
|
legend('Tau1','Tau2','Tau3')
|
||
|
|
%}
|
||
|
|
|
||
|
|
thetamat = thetamat';
|
||
|
|
dthetamat = dthetamat';
|
||
|
|
ddthetamat = ddthetamat';
|
||
|
|
Ftipmat = Ftipmat';
|
||
|
|
taumat = thetamat;
|
||
|
|
for i = 1:size(thetamat,2)
|
||
|
|
taumat(:,i) ...
|
||
|
|
= InverseDynamics(thetamat(:,i),dthetamat(:,i),ddthetamat(:,i),g, ...
|
||
|
|
Ftipmat(:,i),Mlist,Glist,Slist);
|
||
|
|
end
|
||
|
|
taumat = taumat';
|
||
|
|
end
|