rt_fdyn - integrate the forward dynamics[view code]

Calling Sequence

[q, qd] = rt_fdyn(robot, t0, t [, torqfun [, q0 , qd0 [, <args>]]])

Parameters

• robot : robot object. The n-link manipulator for which forward dynamics will be integrated.
• t0 : scalar. It represents the time at which integration starts.
• t : p-element vector (p is arbitrary). It represents times at which the solution is computed.
• torqfun : string. It is the name of an external function provided by user to specify a torque law for robot's actuators. External function can be a Scilab function, but a Fortran subroutine or a C function too. Of course, in these last two cases the external function has to be build and dynamically linked to Scilab before it can be used.
• q0 : n-element vector. Initial joint coordinates.
• qd0 : n-element vector. Initial joint velocities.
• <args> : a comma separated sequence of Scilab variable names. They are additional arguments for user-provided function torqfun.
• q : n-by-p matrix. It represents the matrix of robot's joint state.
• qd : n-by-p matrix. It represents the matrix of robot's joint velocities.

Description

This function integrates the manipulator equations of motion starting at time t0 and for each time specified in vector t using Scilab's numerical integration function ode (calling default solver of package ODEPACK, lsoda). Given the manipulator kinematic and dynamic characteristics by means of robot object robot, it returns matrices of manipulator joint state q and joint velocities qd. These matrices have one row per joint and one column per time step.

Actuator torque may be specified by the user-provided external function named torqfun. If torqfun is given, the Scilab syntax of corresponding external function must be as follows:

     tau = torqfun(t, q, qd, <args>)

where t is the current time (scalar), q and qd are the manipulator joint coordinate and velocity state respectively (both n-element vectors) and <args> is a comma separated sequence of existing Scilab variable names. Tipically this would be used to implement some axis control scheme. If torqfun is not specified, or it's an empty string, then zero torque is applied to the manipulator.

Initial joint coordinates and velocities may be specified by the optional arguments q0 and qd0 respectively.

Examples

   // The following example shows how rt_fdyn() can be used to simulate a
   // robot and its controller. The manipulator is a Puma 560 with simple
   // proportional and derivative (PD) controller. The simulation results
   // pick out the point that further gain tuning is required. Note the
   // high gains are required on joints 2 and 3 in order to counter the
   // significant disturbance torque due to gravity.

   // taufunc definition
   taufunc_code = ["if t > qt(size(qt,1), 1) then";..
        "t = qt(size(qt,1), 1);";..
        "end";..
        "q_dmd = interp1(qt(:,1), qt(:,2:7), t);";..
        "e = q_dmd.'' - q;";..
        "tau = (e.'' * diag(Pgain) + qd.'' * diag(Dgain)).'';"];
   deff("[tau] = taufunc(t, q, qd, qt, Pgain, Dgain)", taufunc_code);

   // rt_fdyn() execution
   exec <PATH>/models/rt_puma560.sce;       // load Puma 560 parameters
   t = [0:.056:5].';                        // time vector
   q_dmd = rt_jtraj(qz, qready, t);         // create a path
   qt = [t q_dmd];
   Pgain = [20 100 20 5 5 5];               // proportional gain
   Dgain = [-5 -10 -2 0 0 0];               // derivative gain
   q0 = qz;                                 // initial joint coordinates
   qd0 = zeros(6, 1);                       // initial joint velocities
   [q, qd] = rt_fdyn(rt_nofriction(p560), 0, t, "taufunc", q0, qd0, qt, Pgain, Dgain);

   // results plotting: simulated path shown as solid, reference path as
   // dashed
   cfh = scf();
   drawlater();
   subplot(3,1,1); plot(t, q(1,:)); plot(t, q_dmd(:,1),"--");   // JOINT 1
   xgrid(); xtitle("", "Time (s)", "Joint 1 (rad)");
   a1 = cfh.children(1); a1.data_bounds = [0 -0.05; 5 0.05];
   a1.tight_limits = "on"; a1.auto_ticks = ["off" "off" "off"];
   a1.x_ticks = tlist(["ticks", "locations", "labels"],..
        [0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5],..
        ["0" "0.5" "1" "1.5" "2" "2.5" "3" "3.5" "4" "4.5" "5"]);
   a1.y_ticks = tlist(["ticks", "locations", "labels"],..
        [-0.05 0 0.05], ["-0.05" "0" "0.05"]);

   subplot(3,1,2); plot(t, q(2,:)); plot(t, q_dmd(:,2),"--");   // JOINT 2
   xgrid(); xtitle("", "Time (s)", "Joint 2 (rad)");
   a2 = cfh.children(1); a2.data_bounds = [0 -1; 5 2];
   a2.tight_limits = "on"; a2.auto_ticks = ["off" "off" "off"];
   a2.x_ticks = tlist(["ticks", "locations", "labels"],..
        [0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5],..
        ["0" "0.5" "1" "1.5" "2" "2.5" "3" "3.5" "4" "4.5" "5"]);
   a2.y_ticks = tlist(["ticks", "locations", "labels"],..
        [-1 0 1 2], ["-1" "0" "1" "2"]);

   subplot(3,1,3); plot(t, q(3,:)); plot(t, q_dmd(:,3),"--");   // JOINT 3
   xgrid(); xtitle("", "Time (s)", "Joint 3 (rad)");
   a3 = cfh.children(1); a3.data_bounds = [0 -2; 5 1];
   a3.tight_limits = "on"; a3.auto_ticks = ["off" "off" "off"];
   a3.x_ticks = tlist(["ticks", "locations", "labels"],..
        [0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5],..
        ["0" "0.5" "1" "1.5" "2" "2.5" "3" "3.5" "4" "4.5" "5"]);
   a3.y_ticks = tlist(["ticks", "locations", "labels"],..
        [-2 -1 0 1], ["-2" "-1" "0" "1"]);
   drawnow();   
   
  

Cautionary

The presence of friction in the dynamic model can prevent the integration from converging. The function rt_nofriction can be used to return a friction-free robot object.

Simulation execution may take a few minutes.

See Also

ode,  rt_accel,  robot/rt_nofriction,  rt_frne,  rt_rne,  rt_robot,  

Authors

original Matlab version by

Peter I. Corke CSIRO Manufacturing Science and Technology

Scilab implementation by

Matteo Morelli Interdepartmental Research Center "E. Piaggio", University of Pisa

Bibliography

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