rt_gravload - compute the manipulator gravity torque components[view code]
This function computes the joint torque due to gravity for the manipulator in pose q. If q is a row vector (p = 1), tau_g returns a row vector of joint torques. If q is a matrix (p > 1) each row is interpreted as a joint state vector, and tau_g is a matrix in which each row is the gravity torque for the corresponding row in q.
The default gravity direction comes from the robot object robot, but may be overridden by the optional grav argument.
// Consider the example of a path which will move the Puma robot from its // zero-angle pose to the "ready" pose. exec <PATH>/models/rt_puma560.sce; // load Puma 560 parameters t = [0:0.056:2]; // motion in 2s(sample time 56ms) [q, qd, qdd] = rt_jtraj(qz, qready, t); // path // The required joint torques for each point of the trajectory are given // by tic(); tau = rt_frne(p560, q, qd, qdd); toc(), // Much of the torque on joints 2 and 3 of this robot is due to gravity. // That component can be computed separately tic(); taug = rt_gravload(p560, q); toc(), // and plotted by using standard Scilab plotting commands cfh = scf(); drawlater(); subplot(2,1,1); plot(t, [tau(:,2), taug(:,2)]); xgrid(); xtitle("", "Time (s)", "Torque on joint 2 (Nm)"); a0 = cfh.children(1); a0.data_bounds = [0 -20; t($) 80]; a0.tight_limits = "on"; a0.auto_ticks = ["off" "off" "off"]; a0.x_ticks = tlist(["ticks", "locations", "labels"],.. [0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8],.. ["0" "0.2" "0.4" "0.6" "0.8" "1" "1.2" "1.4" "1.6" "1.8"]); a0.y_ticks = tlist(["ticks", "locations", "labels"],.. [-20 0 20 40 60 80], ["-20" "0" "20" "40" "60" "80"]); subplot(2,1,2); plot(t, [tau(:,3), taug(:,3)]); xgrid(); xtitle("", "Time (s)", "Torque on joint 3(Nm)"); a1 = cfh.children(1); a1.data_bounds = [0 0; t($) 8]; a1.tight_limits = "on"; a1.auto_ticks = ["off" "off" "off"]; a1.x_ticks = tlist(["ticks", "locations", "labels"],.. [0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8],.. ["0" "0.2" "0.4" "0.6" "0.8" "1" "1.2" "1.4" "1.6" "1.8"]); a1.y_ticks = tlist(["ticks", "locations", "labels"],.. [0 2 4 6 8], ["0" "2" "4" "6" "8"]); drawnow();
rt_robot, rt_link, rt_frne, rt_rne, rt_itorque, rt_coriolis,
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