Skip to content

Commit 4b1be28

Browse files
petercorkepetercorke
committed
Add logic to find axis bounds automatically Fix bug in pose of the frame that is animated, should be I not the first in sequence
0 parents  commit 4b1be28

File tree

237 files changed

+55700
-0
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

237 files changed

+55700
-0
lines changed

‎@SerialLink/SerialLink.m

Lines changed: 544 additions & 0 deletions
Large diffs are not rendered by default.

‎@SerialLink/accel.m

Lines changed: 91 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,91 @@
1+
%SerialLink.accel Manipulator forward dynamics
2+
%
3+
% QDD = R.accel(Q, QD, TORQUE) is a vector (Nx1) of joint accelerations that result
4+
% from applying the actuator force/torque to the manipulator robot in state Q and QD.
5+
% If Q, QD, TORQUE are matrices with M rows, then QDD is a matrix with M rows
6+
% of acceleration corresponding to the equivalent rows of Q, QD, TORQUE.
7+
%
8+
% QDD = R.ACCEL(X) as above but X=[Q,QD,TORQUE].
9+
%
10+
% Note::
11+
% - Uses the method 1 of Walker and Orin to compute the forward dynamics.
12+
% - This form is useful for simulation of manipulator dynamics, in
13+
% conjunction with a numerical integration function.
14+
%
15+
% See also SerialLink.rne, SerialLink, ode45.
16+
17+
18+
% Copyright (C) 1993-2011, by Peter I. Corke
19+
%
20+
% This file is part of The Robotics Toolbox for Matlab (RTB).
21+
%
22+
% RTB is free software: you can redistribute it and/or modify
23+
% it under the terms of the GNU Lesser General Public License as published by
24+
% the Free Software Foundation, either version 3 of the License, or
25+
% (at your option) any later version.
26+
%
27+
% RTB is distributed in the hope that it will be useful,
28+
% but WITHOUT ANY WARRANTY; without even the implied warranty of
29+
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
30+
% GNU Lesser General Public License for more details.
31+
%
32+
% You should have received a copy of the GNU Leser General Public License
33+
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
34+
%
35+
% http://www.petercorke.com
36+
37+
38+
function qdd = accel(robot, Q, qd, torque)
39+
40+
if numcols(Q) ~= robot.n
41+
error('q must have %d columns', robot.n);
42+
end
43+
if numcols(qd) ~= robot.n
44+
error('qd must have %d columns', robot.n);
45+
end
46+
if numcols(torque) ~= robot.n
47+
error('torque must have %d columns', robot.n);
48+
end
49+
50+
if numrows(Q) > 1
51+
if numrows(Q) ~= numrows(qd)
52+
error('for trajectory q and qd must have same number of rows');
53+
end
54+
if numrows(Q) ~= numrows(torque)
55+
error('for trajectory q and torque must have same number of rows');
56+
end
57+
qdd = [];
58+
for i=1:numrows(Q)
59+
qdd = cat(1, qdd, robot.accel(Q(i,:), qd(i,:), torque(i,:))');
60+
end
61+
return
62+
end
63+
64+
n = robot.n;
65+
66+
if nargin == 2
67+
% accel(X)
68+
q = Q(1:n);
69+
qd = Q(n+1:2*n);
70+
torque = Q(2*n+1:3*n);
71+
else
72+
% accel(Q, qd, torque)
73+
q = Q;
74+
if length(q) == robot.n,
75+
q = q(:);
76+
qd = qd(:);
77+
end
78+
end
79+
80+
% compute current manipulator inertia
81+
% torques resulting from unit acceleration of each joint with
82+
% no gravity.
83+
M = rne(robot, ones(n,1)*q', zeros(n,n), eye(n), [0;0;0]);
84+
85+
% compute gravity and coriolis torque
86+
% torques resulting from zero acceleration at given velocity &
87+
% with gravity acting.
88+
tau = rne(robot, q', qd', zeros(1,n));
89+
90+
qdd = inv(M) * (torque(:) - tau');
91+

‎@SerialLink/cinertia.m

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,39 @@
1+
%SerialLink.cinertia Cartesian inertia matrix
2+
%
3+
% M = R.cinertia(Q) is the NxN Cartesian (operational space) inertia matrix which relates
4+
% Cartesian force/torque to Cartesian acceleration at the joint configuration Q, and N
5+
% is the number of robot joints.
6+
%
7+
% See also SerialLink.inertia, SerialLink.rne.
8+
9+
% MOD HISTORY
10+
% 4/99 add object support
11+
% $Log: not supported by cvs2svn $
12+
% $Revision: 1.2 $
13+
14+
15+
16+
% Copyright (C) 1993-2011, by Peter I. Corke
17+
%
18+
% This file is part of The Robotics Toolbox for Matlab (RTB).
19+
%
20+
% RTB is free software: you can redistribute it and/or modify
21+
% it under the terms of the GNU Lesser General Public License as published by
22+
% the Free Software Foundation, either version 3 of the License, or
23+
% (at your option) any later version.
24+
%
25+
% RTB is distributed in the hope that it will be useful,
26+
% but WITHOUT ANY WARRANTY; without even the implied warranty of
27+
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
28+
% GNU Lesser General Public License for more details.
29+
%
30+
% You should have received a copy of the GNU Leser General Public License
31+
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
32+
%
33+
% http://www.petercorke.com
34+
35+
function Mx = cinertia(robot, q)
36+
J = jacob0(robot, q);
37+
Ji = inv(J);
38+
M = inertia(robot, q);
39+
Mx = Ji' * M * Ji;

‎@SerialLink/coriolis.m

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
%SerialLink.coriolis Coriolis matrix
2+
%
3+
% C = R.CORIOLIS(Q, QD) is the NxN Coriolis/centripetal matrix for
4+
% the robot in configuration Q and velocity QD, where N is the number of
5+
% joints. The product C*QD is the vector of joint force/torque due to velocity
6+
% coupling. The diagonal elements are due to centripetal effects and the
7+
% off-diagonal elements are due to Coriolis effects. This matrix is also
8+
% known as the velocity coupling matrix, since gives the disturbance forces
9+
% on all joints due to velocity of any joint.
10+
%
11+
% If Q and QD are matrices (DxN), each row is interpretted as a joint state
12+
% vector, and the result (NxNxD) is a 3d-matrix where each plane corresponds
13+
% to a row of Q and QD.
14+
%
15+
% Notes::
16+
% - joint friction is also a joint force proportional to velocity but it is
17+
% eliminated in the computation of this value.
18+
% - computationally slow, involves N^2/2 invocations of RNE.
19+
%
20+
% See also SerialLink.rne.
21+
22+
23+
24+
25+
% Copyright (C) 1993-2011, by Peter I. Corke
26+
%
27+
% This file is part of The Robotics Toolbox for Matlab (RTB).
28+
%
29+
% RTB is free software: you can redistribute it and/or modify
30+
% it under the terms of the GNU Lesser General Public License as published by
31+
% the Free Software Foundation, either version 3 of the License, or
32+
% (at your option) any later version.
33+
%
34+
% RTB is distributed in the hope that it will be useful,
35+
% but WITHOUT ANY WARRANTY; without even the implied warranty of
36+
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
37+
% GNU Lesser General Public License for more details.
38+
%
39+
% You should have received a copy of the GNU Leser General Public License
40+
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
41+
%
42+
% http://www.petercorke.com
43+
44+
function C = coriolis(robot, q, qd)
45+
46+
if numcols(q) ~= robot.n
47+
error('q must have %d columns', robot.n);
48+
end
49+
if numcols(qd) ~= robot.n
50+
error('qd must have %d columns', robot.n);
51+
end
52+
53+
% we need to create a clone robot with no friciton, since friction
54+
% is also proportional to joint velocity
55+
robot2 = robot.nofriction('all');
56+
57+
if numrows(q) > 1
58+
if numrows(q) ~= numrows(qd)
59+
error('for trajectory q and qd must have same number of rows');
60+
end
61+
C = [];
62+
for i=1:numrows(q)
63+
C = cat(3, C, robot2.coriolis(q(i,:), qd(i,:)));
64+
end
65+
return
66+
end
67+
68+
N = robot2.n;
69+
C = zeros(N,N);
70+
Csq = zeros(N,N);
71+
72+
% find the torques that depend on a single finite joint speed,
73+
% these are due to the squared (centripetal) terms
74+
%
75+
% set QD = [1 0 0 ...] then resulting torque is due to qd_1^2
76+
for j=1:N
77+
QD = zeros(1,N);
78+
QD(j) = 1;
79+
tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]');
80+
Csq(:,j) = Csq(:,j) + tau';
81+
end
82+
83+
% find the torques that depend on a pair of finite joint speeds,
84+
% these are due to the product (Coridolis) terms
85+
% set QD = [1 1 0 ...] then resulting torque is due to
86+
% qd_1 qd_2 + qd_1^2 + qd_2^2
87+
for j=1:N
88+
for k=j+1:N
89+
% find a product term qd_j * qd_k
90+
QD = zeros(1,N);
91+
QD(j) = 1;
92+
QD(k) = 1;
93+
tau = robot2.rne(q, QD, zeros(size(q)), [0 0 0]');
94+
C(:,k) = C(:,k) + (tau' - Csq(:,k) - Csq(:,j)) * qd(j);
95+
end
96+
end
97+
C = C + Csq * diag(qd);

‎@SerialLink/fdyn.m

Lines changed: 111 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,111 @@
1+
%SerialLink.fdyn Integrate forward dynamics
2+
%
3+
% [T,Q,QD] = R.fdyn(T1, TORQFUN) integrates the dynamics of the robot over
4+
% the time interval 0 to T and returns vectors of time TI, joint position Q
5+
% and joint velocity QD. The initial joint position and velocity are zero.
6+
% The torque applied to the joints is computed by the user function TORQFUN:
7+
%
8+
% [TI,Q,QD] = R.fdyn(T, TORQFUN, Q0, QD0) as above but allows the initial
9+
% joint position and velocity to be specified.
10+
%
11+
% The control torque is computed by a user defined function
12+
%
13+
% TAU = TORQFUN(T, Q, QD, ARG1, ARG2, ...)
14+
%
15+
% where Q and QD are the manipulator joint coordinate and velocity state
16+
% respectively], and T is the current time.
17+
%
18+
% [T,Q,QD] = R.fdyn(T1, TORQFUN, Q0, QD0, ARG1, ARG2, ...) allows optional
19+
% arguments to be passed through to the user function.
20+
%
21+
% Note::
22+
% - This function performs poorly with non-linear joint friction, such as
23+
% Coulomb friction. The R.nofriction() method can be used to set this
24+
% friction to zero.
25+
% - If TORQFUN is not specified, or is given as 0 or [], then zero torque
26+
% is applied to the manipulator joints.
27+
% - The builtin integration function ode45() is used.
28+
%
29+
% See also SerialLink.accel, SerialLink.nofriction, SerialLink.RNE, ode45.
30+
31+
32+
33+
% Copyright (C) 1993-2011, by Peter I. Corke
34+
%
35+
% This file is part of The Robotics Toolbox for Matlab (RTB).
36+
%
37+
% RTB is free software: you can redistribute it and/or modify
38+
% it under the terms of the GNU Lesser General Public License as published by
39+
% the Free Software Foundation, either version 3 of the License, or
40+
% (at your option) any later version.
41+
%
42+
% RTB is distributed in the hope that it will be useful,
43+
% but WITHOUT ANY WARRANTY; without even the implied warranty of
44+
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
45+
% GNU Lesser General Public License for more details.
46+
%
47+
% You should have received a copy of the GNU Leser General Public License
48+
% along with RTB. If not, see <http://www.gnu.org/licenses/>.
49+
%
50+
% http://www.petercorke.com
51+
52+
function [t, q, qd] = fdyn(robot, t1, torqfun, q0, qd0, varargin)
53+
54+
% check the Matlab version, since ode45 syntax has changed
55+
if verLessThan('matlab', '7')
56+
error('fdyn now requires Matlab version >= 7');
57+
end
58+
59+
n = robot.n;
60+
if nargin == 2
61+
torqfun = 0;
62+
q0 = zeros(1,n);
63+
qd0 = zeros(1,n);
64+
elseif nargin == 3
65+
q0 = zeros(1,n);
66+
qd0 = zeros(1,n);
67+
elseif nargin == 4
68+
qd0 = zeros(1,n);
69+
end
70+
71+
% concatenate q and qd into the initial state vector
72+
q0 = [q0(:); qd0(:)];
73+
74+
[t,y] = ode45(@fdyn2, [0 t1], q0, [], robot, torqfun, varargin{:});
75+
q = y(:,1:n);
76+
qd = y(:,n+1:2*n);
77+
78+
end
79+
80+
81+
%FDYN2 private function called by FDYN
82+
%
83+
% XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN)
84+
%
85+
% Called by FDYN to evaluate the robot velocity and acceleration for
86+
% forward dynamics. T is the current time, X = [Q QD] is the state vector,
87+
% ROBOT is the object being integrated, and TORQUEFUN is the string name of
88+
% the function to compute joint torques and called as
89+
%
90+
% TAU = TORQUEFUN(T, X)
91+
%
92+
% if not given zero joint torques are assumed.
93+
%
94+
% The result is XDD = [QD QDD].
95+
function xd = fdyn2(t, x, robot, torqfun, varargin)
96+
97+
n = robot.n;
98+
99+
q = x(1:n)';
100+
qd = x(n+1:2*n)';
101+
102+
% evaluate the torque function if one is given
103+
if isa(torqfun, 'function_handle')
104+
tau = torqfun(robot, t, q, qd, varargin{:});
105+
else
106+
tau = zeros(n,1);
107+
end
108+
109+
qdd = accel(robot, x(1:n,1), x(n+1:2*n,1), tau);
110+
xd = [x(n+1:2*n,1); qdd];
111+
end

0 commit comments

Comments
 (0)