rt_frne - fast rne. C version of algorithm that computes inverse dynamics via recursive Newton-Euler formulation[view code]
This function is a C-code implementation of recursive Newton-Euler (RNE) algorithm for robot inverse dynamics computation. Just like its Scilab-coded counterpart (rt_rne), it computes the equation of motion in an efficent manner, giving joint torques as a function of joint position, velocity and acceleration.
If q, qd and qdd are row vectors (p = 1), then tau is a row vector of joint torques. Else (p > 1) tau is a matrix in which each row is the joint torque for the corresponding rows of q, qd and qdd.
Gravity direction is defined by the robot object but may be overridden by providing the gravity acceleration vector grav.
An external generalized force acting on the end of the manipulator may also be specified by the 6-element vector fext in the end-effector coordinate frame.
The torque computed may contain contributions due to armature inertia and joint friction if these are specified in the parameter matrix dyn.
// The following example is a typical IDP (Inverse Dynamics Problem) in
// robotics. It shows why rt_frne() is more suitable than rt_rne() to
// be used to solve IDPs.
// Here is shown how to compute the joint torques for a simple two link
// RR planar, isolating each single torque contribution (i.e. inertial,
// Coriolis/centripetal and gravitational), when joint trajectories in
// terms of position, velocity and acceleration are given.
// Trajectories have the same duration and a triangular-velocity profile.
// Initially the manipulator has a down-elbow configuration such that its
// end-effector is at point (0.2, 0) metres in the operating space.
// Both its joints move through an excursion of %pi/2 radians in 0.5 sec.
//
// References: Chapter 4, Example 4.2, of
// L. Sciavicco, B. Siciliano, "Modelling and Control of Robot
// Manipulators", 2nd Edition, Springer-Verlag Advanced Textbooks in
// Control and Signal Processing Series, London, UK, 2000
// load the joint trajectories
load("<PATH>/demos/rne/config-tlexamp.dat",..
"t_tlexamp", "q_tlexamp", "qd_tlexamp", "qdd_tlexamp");
// load RR parameters
exec <PATH>/demos/rne/manip-tlexamp.sce;
// joint trajectories plotting
cfh = scf(); drawlater();
subplot(3,2,1); plot(t_tlexamp, q_tlexamp(:,1)); // POS.
xgrid(); xtitle("Position Joint 1", "[s]", "[rad]");
a1 = cfh.children(1); a1.data_bounds = [0 -2; 0.6 5];
a1.tight_limits = "on"; a1.auto_ticks = ["off" "off" "off"];
a1.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a1.y_ticks = tlist(["ticks", "locations", "labels"],..
[-2 -1 0 1 2 3 4 5], ["-2" "-1" "0" "1" "2" "3" "4" "5"]);
subplot(3,2,2); plot(t_tlexamp, q_tlexamp(:,2));
xgrid(); xtitle("Position Joint 2", "[s]", "[rad]");
a2 = cfh.children(1); a2.data_bounds = [0 -2; 0.6 5];
a2.tight_limits = "on"; a2.auto_ticks = ["off" "off" "off"];
a2.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a2.y_ticks = tlist(["ticks", "locations", "labels"],..
[-2 -1 0 1 2 3 4], ["-2" "-1" "0" "1" "2" "3" "4" "5"]);
subplot(3,2,3); plot(t_tlexamp, qd_tlexamp(:,1)); // VEL.
xgrid(); xtitle("Velocity Joint 1", "[s]", "[rad/s]");
a3 = cfh.children(1); a3.data_bounds = [0 -1; 0.6 7];
a3.tight_limits = "on"; a3.auto_ticks = ["off" "off" "off"];
a3.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a3.y_ticks = tlist(["ticks", "locations", "labels"],..
[0 2 4 6], ["0" "2" "4" "6"]);
subplot(3,2,4); plot(t_tlexamp, qd_tlexamp(:,2));
xgrid(); xtitle("Velocity Joint 2", "[s]", "[rad/s]");
a4 = cfh.children(1); a4.data_bounds = [0 -1; 0.6 7];
a4.tight_limits = "on"; a4.auto_ticks = ["off" "off" "off"];
a4.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a4.y_ticks = tlist(["ticks", "locations", "labels"],..
[0 2 4 6], ["0" "2" "4" "6"]);
subplot(3,2,5); plot(t_tlexamp, qdd_tlexamp(:,1)); // ACCEL.
xgrid(); xtitle("Accel. Joint 1", "[s]", "[rad/s^2]");
a5 = cfh.children(1); a5.data_bounds = [0 -30; 0.6 30];
a5.tight_limits = "on"; a5.auto_ticks = ["off" "off" "off"];
a5.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a5.y_ticks = tlist(["ticks", "locations", "labels"],..
[-30 -20 -10 0 10 20 30],..
["-30" "-20" "-10" "0" "10" "20" "30"]);
subplot(3,2,6); plot(t_tlexamp, qdd_tlexamp(:,2));
xgrid(); xtitle("Accel. Joint 2", "[s]", "[rad/s^2]");
a6 = cfh.children(1); a6.data_bounds = [0 -30; 0.6 30];
a6.tight_limits = "on"; a6.auto_ticks = ["off" "off" "off"];
a6.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a6.y_ticks = tlist(["ticks", "locations", "labels"],..
[-30 -20 -10 0 10 20 30],..
["-30" "-20" "-10" "0" "10" "20" "30"]);
drawnow();
// joint torques computation
tic();.. // start timer
tau = rt_frne(manip_tlexamp, q_tlexamp, qd_tlexamp, qdd_tlexamp);..
toc(), // stop timer
// compute each inertia torque contribution
// rt_inertia uses RNE formulation
tic();.. // start timer
I = rt_inertia(manip_tlexamp, q_tlexamp);..
toc(), // stop timer
I11 = I(1, 1, :); i11 = matrix(I11, length(t_tlexamp), 1);
I12 = I(1, 2, :); i12 = matrix(I12, length(t_tlexamp), 1);
I21 = I(2, 1, :); i21 = matrix(I21, length(t_tlexamp), 1);
I22 = I(2, 2, :); i22 = matrix(I22, length(t_tlexamp), 1);
taui_11 = i11 .* qdd_tlexamp(:, 1); // at J1, by J1 accel
taui_12 = i12 .* qdd_tlexamp(:, 2); // at J1, by J2 accel
taui_21 = i21 .* qdd_tlexamp(:, 1); // at J2, by J1 accel
taui_22 = i22 .* qdd_tlexamp(:, 2); // at J2, by J2 accel
// compute each Coriolis/centripetal torque contribution
// rt_coriolis uses RNE formulation
tic();.. // start timer
tauc = rt_coriolis(manip_tlexamp, q_tlexamp, qd_tlexamp);..
toc(), // stop timer
// compute each gravitational torque contribution
// rt_gravload uses RNE formulation
tic();.. // start timer
taug = rt_gravload(manip_tlexamp, q_tlexamp);..
toc(), // stop timer
// This task has shown that RNE formulation is very recurrent in
// pratice and have a fast routine which implements it is needed.
// Function rt_rne() takes too much time for a single computation,
// therefore it is suitable to be used neither in this simple case
// of study.
//
// Finally, simulation results are showed
// joint torques
cfh = scf();
drawlater();
subplot(2,1,1); plot(t_tlexamp, tau(:,1)); // JOINT 1
xgrid(); xtitle("Joint 1 Torque", "[s]", "[N*m]");
a7 = cfh.children(1); a7.data_bounds = [0 -6000; 0.6 7000];
a7.tight_limits = "on"; a7.auto_ticks = ["off" "off" "off"];
a7.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a7.y_ticks = tlist(["ticks", "locations", "labels"],..
[-6000 -4000 -2000 0 2000 4000 6000],..
["-6000" "-4000" "-2000" "0" "2000" "4000" "6000"]);
subplot(2,1,2); plot(t_tlexamp, tau(:,2)); // JOINT 2
xgrid(); xtitle("Joint 2 Torque", "[s]", "[N*m]");
a8 = cfh.children(1); a8.data_bounds = [0 -6000; 0.6 7000];
a8.tight_limits = "on"; a8.auto_ticks = ["off" "off" "off"];
a8.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a8.y_ticks = tlist(["ticks", "locations", "labels"],..
[-6000 -4000 -2000 0 2000 4000 6000],..
["-6000" "-4000" "-2000" "0" "2000" "4000" "6000"]);
drawnow();
// inertial torque contributions
cfh = scf();
drawlater();
subplot(2,2,1); plot(t_tlexamp, taui_11); // JOINT 1
xgrid(); xtitle("Inertial Torque 11", "[s]", "[N*m]");
a9 = cfh.children(1); a9.data_bounds = [0 -5000; 0.6 5000];
a9.tight_limits = "on"; a9.auto_ticks = ["off" "off" "off"];
a9.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a9.y_ticks = tlist(["ticks", "locations", "labels"],..
[-5000 0 5000], ["-5000" "0" "5000"]);
subplot(2,2,2); plot(t_tlexamp, taui_12);
xgrid(); xtitle("Inertial Torque 12", "[s]", "[N*m]");
a10 = cfh.children(1); a10.data_bounds = [0 -500; 0.6 100];
a10.tight_limits = "on"; a10.auto_ticks = ["off" "off" "off"];
a10.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a10.y_ticks = tlist(["ticks", "locations", "labels"],..
[-500 -400 -300 -200 -100 0 100],..
["-500" "-400" "-300" "-200" "-100" "0" "100"]);
subplot(2,2,3); plot(t_tlexamp, taui_21); // JOINT 2
xgrid(); xtitle("Inertial Torque 21", "[s]", "[N*m]");
a11 = cfh.children(1); a11.data_bounds = [0 -500; 0.6 100];
a11.tight_limits = "on"; a11.auto_ticks = ["off" "off" "off"];
a11.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a11.y_ticks = tlist(["ticks", "locations", "labels"],..
[-500 -400 -300 -200 -100 0 100],..
["-500" "-400" "-300" "-200" "-100" "0" "100"]);
subplot(2,2,4); plot(t_tlexamp, taui_22);
xgrid(); xtitle("Inertial Torque 22", "[s]", "[N*m]");
a12 = cfh.children(1); a12.data_bounds = [0 -5000; 0.6 5000];
a12.tight_limits = "on"; a12.auto_ticks = ["off" "off" "off"];
a12.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a12.y_ticks = tlist(["ticks", "locations", "labels"],..
[-5000 0 5000], ["-5000" "0" "5000"]);
drawnow();
// Coriolis/centripetal torque contributions
cfh = scf();
drawlater();
subplot(2,1,1); plot(t_tlexamp, tauc(:,1)); // JOINT 1
xgrid(); xtitle("Coriolis_12 & Centrif_2", "[s]", "[N*m]");
a13 = cfh.children(1); a13.data_bounds = [0 -600; 0.6 1800];
a13.tight_limits = "on"; a13.auto_ticks = ["off" "off" "off"];
a13.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a13.y_ticks = tlist(["ticks", "locations", "labels"],..
[-500 0 500 1000 1500], ["-500" "0" "500" "1000" "1500"]);
subplot(2,1,2); plot(t_tlexamp, tauc(:,2)); // JOINT 2
xgrid(); xtitle("Centrif_1", "[s]", "[N*m]");
a14 = cfh.children(1); a14.data_bounds = [0 -600; 0.6 1800];
a14.tight_limits = "on"; a14.auto_ticks = ["off" "off" "off"];
a14.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a14.y_ticks = tlist(["ticks", "locations", "labels"],..
[-500 0 500 1000], ["-500" "0" "500" "1000"]);
drawnow();
// gravitational torque contributions
cfh = scf();
drawlater();
subplot(2,1,1); plot(t_tlexamp, taug(:,1)); // JOINT 1
xgrid(); xtitle("Grav. Torque 1", "[s]", "[N*m]");
a15 = cfh.children(1); a15.data_bounds = [0 -300; 0.6 850];
a15.tight_limits = "on"; a15.auto_ticks = ["off" "off" "off"];
a15.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a15.y_ticks = tlist(["ticks", "locations", "labels"],..
[-200 0 200 400 600 800], ["-200" "0" "200" "400" "600" "800"]);
subplot(2,1,2); plot(t_tlexamp, taug(:,2)); // JOINT 2
xgrid(); xtitle("Grav. Torque 2", "[s]", "[N*m]");
a16 = cfh.children(1); a16.data_bounds = [0 -300; 0.6 850];
a16.tight_limits = "on"; a16.auto_ticks = ["off" "off" "off"];
a16.x_ticks = tlist(["ticks", "locations", "labels"],..
[0 0.1 0.2 0.3 0.4 0.5], ["0" "0.1" "0.2" "0.3" "0.4" "0.5"]);
a16.y_ticks = tlist(["ticks", "locations", "labels"],..
[-200 0 200 400 600 800], ["-200" "0" "200" "400" "600" "800"]);
drawnow();
rt_robot, rt_rne, rt_itorque, rt_coriolis, rt_gravload, rt_inertia, rt_cinertia, rt_accel, rt_fdyn,
Corke, P.I. "A Robotics Toolbox for MATLAB", IEEE Robotics and Automation Magazine, Volume 3(1), March 1996, pp. 24-32
M.W. Walker and D.E. Orin. Efficient dynamic computer simulation of robotic mechanisms. ASME Journal of Dynamic Systems, Measurement and Control, 104:205-211, 1982
L. Sciavicco, B. Siciliano, Modelling and Control of Robot Manipulators. 2nd Edition, Springer-Verlag Advanced Textbooks in Control and Signal Processing Series, London, UK, 2000.