35 lines
1.1 KiB
Mathematica
35 lines
1.1 KiB
Mathematica
|
%*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
|
||
|
|
|
||
|
|
function [thetalistNext, dthetalistNext] ...
|
||
|
|
= EulerStep(thetalist, dthetalist, ddthetalist, dt)
|
||
|
|
% Takes thetalist: n-vector of joint variables,
|
||
|
|
% dthetalist: n-vector of joint rates,
|
||
|
|
% ddthetalist: n-vector of joint accelerations,
|
||
|
|
% dt: The timestep delta t.
|
||
|
|
% Returns thetalistNext: Vector of joint variables after dt from first
|
||
|
|
% order Euler integration,
|
||
|
|
% dthetalistNext: Vector of joint rates after dt from first order
|
||
|
|
% Euler integration.
|
||
|
|
% Example Inputs (3 Link Robot):
|
||
|
|
%{
|
||
|
|
clear; clc;
|
||
|
|
thetalist = [0.1; 0.1; 0.1];
|
||
|
|
dthetalist = [0.1; 0.2; 0.3];
|
||
|
|
ddthetalist = [2; 1.5; 1];
|
||
|
|
dt = 0.1;
|
||
|
|
[thetalistNext, dthetalistNext] = EulerStep(thetalist, dthetalist, ...
|
||
|
|
ddthetalist, dt)
|
||
|
|
%}
|
||
|
|
% Output:
|
||
|
|
% thetalistNext =
|
||
|
|
% 0.1100
|
||
|
|
% 0.1200
|
||
|
|
% 0.1300
|
||
|
|
% dthetalistNext =
|
||
|
|
% 0.3000
|
||
|
|
% 0.3500
|
||
|
|
% 0.4000
|
||
|
|
|
||
|
|
thetalistNext = thetalist + dt * dthetalist;
|
||
|
|
dthetalistNext = dthetalist + dt * ddthetalist;
|
||
|
|
end
|