mode(-1);
// Copyright (C) 1993-2002, by Peter I. Corke
// Copyright (C) 2007  Interdepartmental Research Center "E. Piaggio", University of Pisa
//
// This file is part of RTSS, the Robotics Toolbox for Scilab/Scicos.
//
// RTSS is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// RTSS is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with RTSS; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA



// File name:       rt_rtfkdemo.sce
//
// Description:     demo for Forward kinematics
//
// Annotations:     this code is a Scilab port of corresponding demo in the
//                  Robotics Toolbox for MATLAB(R) written by Peter I. Corke.
//
// References:      Robotics Toolbox for MATLAB(R), robot7.1/rtfkdemo.m
//
// Author:          Matteo Morelli, I.R.C. "E. Piaggio", University of Pisa
//
// Date:            April 2007
//
// $LastChangedDate: 2007-10-05 23:28:34 +0200(ven, 05 ott 2007) $

text = [..
"// Forward kinematics is the problem of solving the Cartesian position and ";..
"// orientation of a mechanism given knowledge of the kinematic structure and";.. 
"// the joint coordinates.";..
"//";..
"//  Consider the Puma 560 example again, and the joint coordinates of zero,";..
"//  which are defined by qz";..
"-->qz,";..
" qz  =";..
"";..
"// The forward kinematics may be computed using rt_fkine() with an appropropriate ";..
"// kinematic description, in this case, the robot object p560 which defines ";..
"// kinematics for the 6-axis Puma 560.";..
"-->rt_fkine(p560, qz),";..
" ans  =";..
"";..
"// returns the homogeneous transform corresponding to the last link of the ";..
"// manipulator";..                     // parag1
"";..
"// rt_fkine() can also be used with a time sequence of joint coordinates, or ";..
"// trajectory, which is generated by rt_jtraj()";..
"-->t = [0:0.056:2];                // generate a time vector";..
" ";..
"-->q = rt_jtraj(qz, qready, t);    // compute the joint coordinate trajectory";..
"";..
"// then the homogeneous transform for each set of joint coordinates is given by";..
"-->tic(); T = rt_fkine(p560, q); et = toc(),";..
" et  =";..
"";..
"// where T is a 3-dimensional matrix, the first two dimensions are a 4x4 ";..
"// homogeneous transformation and the third dimension is time.";..
"//";..
"// For example, the first point is";..
"-->T(:,:,1)";..
" ans  =";..
"";..
"// and the tenth point is";..
"-->T(:,:,10)             ";..
" ans  =";..                            // parag2
"";..
"// Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and ";..
"// may be plotted against time";..
"-->cfh = scf();";..
" ";..
"-->drawlater();";..
" ";..
"-->subplot(3,1,1); plot(t, matrix(T(1,4,:), 1, 36));           // X";..
" ";..
"-->xgrid(); xtitle("""", ""Time (s)"", ""X (m)"");";..
" ";..
"-->a0 = cfh.children(1); a0.data_bounds = [0 0; t($) 1];";..
" ";..
"-->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""], [0 0.5 1], [""0"" ""0.5"" ""1""]);";..
" ";..
"-->subplot(3,1,2); plot(t, matrix(T(2,4,:), 1, 36));           // Y";..
" ";..
"-->xgrid(); xtitle("""", ""Time (s)"", ""Y (m)"");";..
" ";..
"-->a1 = cfh.children(1); a1.data_bounds = [0 -0.2; t($) -0.1];";..
" ";..
"-->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 T(2,4,1) -0.1], [""-0.2"" sci2exp(T(2,4,1)) ""-0.1""]);";..
" ";..
"-->subplot(3,1,3); plot(t, matrix(T(3,4,:), 1, 36));           // Z";..
" ";..
"-->xgrid(); xtitle("""", ""Time (s)"", ""Z (m)"");";..
" ";..
"-->a2 = cfh.children(1); a2.data_bounds = [0 0.4; t($) 1];";..
" ";..
"-->a2.tight_limits = ""on""; a2.auto_ticks = [""off"" ""off"" ""off""];";..
" ";..
"-->a2.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""]);";..
" ";..
"-->a2.y_ticks = tlist([""ticks"", ""locations"", ""labels""], [0.4 0.6 0.8 1], [""0.4"" ""0.6"" ""0.8"" ""1""]);";..
" ";..
"-->drawnow();";..                      // parag3
"";..
"// or we could plot X against Z to get some idea of the Cartesian path followed";..
"// by the manipulator.";..
"-->xdel(cfh.figure_id);";..
" ";..
"-->cfh = scf();";..
" ";..
"-->drawlater();";..
" ";..
"-->plot(matrix(T(1,4,:), 1, 36), matrix(T(3,4,:), 1, 36));     // X against Z";..
" ";..
"-->xgrid(); xtitle("""", ""X (m)"", ""Z (m)"");";..
" ";..
"-->a3 = cfh.children(1); a3.data_bounds = [0 0.4; 0.5 0.9];";..
" ";..
"-->a3.tight_limits = ""on""; a3.auto_ticks = [""off"" ""off"" ""off""];";..
" ";..
"-->a3.x_ticks = tlist([""ticks"", ""locations"", ""labels""], [0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5], [""0"" ""0.05"" ""0.1"" ""0.15"" ""0.2"" ""0.25"" ""0.3"" ""0.35"" ""0.4"" ""0.45"" ""0.5""]);";..
" ";..
"-->a3.y_ticks = tlist([""ticks"", ""locations"", ""labels""], [0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9], [""0.4"" ""0.45"" ""0.5"" ""0.55"" ""0.6"" ""0.65"" ""0.7"" ""0.75"" ""0.8"" ""0.85"" ""0.9""]);";..
" ";..
"-->drawnow();";..                      // parag4
"";..
""..
];

mprintf("%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n", text(1:8)');
disp(qz);
mprintf("%s\n%s\n%s\n%s\n%s\n%s\n", text(9:14)');
disp(rt_fkine(p560, qz));
mprintf("%s\n%s\n%s\n", text(15:17)');
disp("");
mprintf("halt mode: enter empty lines to continue.");
mscanf("%c");

mprintf("%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n", text(18:26)');
t = [0:0.056:2];
q = rt_jtraj(qz, qready, t);
tic(); T = rt_fkine(p560, q); et = toc();
mprintf("%s\n", text(27:27)');
disp(et);
mprintf("%s\n%s\n%s\n%s\n%s\n%s\n%s\n", text(28:34)');
disp(T(:,:,1));
mprintf("%s\n%s\n%s\n%s\n", text(35:38)');
disp(T(:,:,10));
disp("");
mprintf("halt mode: enter empty lines to continue.");
mscanf("%c");

mprintf("%s\n%s\n%s\n%s\n", text(39:42)');
cfh = scf();
mprintf("%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n", text(43:82)');
drawlater();
subplot(3,1,1); plot(t, matrix(T(1,4,:), 1, 36));           // X
xgrid(); xtitle("", "Time (s)", "X (m)");
a0 = cfh.children(1); a0.data_bounds = [0 0; t($) 1];
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"],..
    [0 0.5 1], ["0" "0.5" "1"]);

subplot(3,1,2); plot(t, matrix(T(2,4,:), 1, 36));           // Y
xgrid(); xtitle("", "Time (s)", "Y (m)");
a1 = cfh.children(1); a1.data_bounds = [0 -0.2; t($) -0.1];
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 T(2,4,1) -0.1], ["-0.2" sci2exp(T(2,4,1)) "-0.1"]);

subplot(3,1,3); plot(t, matrix(T(3,4,:), 1, 36));           // Z
xgrid(); xtitle("", "Time (s)", "Z (m)");
a2 = cfh.children(1); a2.data_bounds = [0 0.4; t($) 1];
a2.tight_limits = "on"; a2.auto_ticks = ["off" "off" "off"];
a2.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"]);
a2.y_ticks = tlist(["ticks", "locations", "labels"],..
    [0.4 0.6 0.8 1], ["0.4" "0.6" "0.8" "1"]);
drawnow();
disp("");
mprintf("halt mode: enter empty lines to continue.");
mscanf("%c");

mprintf("%s\n%s\n%s\n%s\n", text(83:86)');
xdel(cfh.figure_id);
mprintf("%s\n%s\n", text(87:88)');
cfh = scf();
mprintf("%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n%s\n", text(89:104)');
drawlater();
plot(matrix(T(1,4,:), 1, 36), matrix(T(3,4,:), 1, 36));     // X against Z
xgrid(); xtitle("", "X (m)", "Z (m)");
a3 = cfh.children(1); a3.data_bounds = [0 0.4; 0.5 0.9];
a3.tight_limits = "on"; a3.auto_ticks = ["off" "off" "off"];
a3.x_ticks = tlist(["ticks", "locations", "labels"],..
    [0 0.05 0.1 0.15 0.2 0.25 0.3 0.35 0.4 0.45 0.5],..
    ["0" "0.05" "0.1" "0.15" "0.2" "0.25" "0.3" "0.35" "0.4" "0.45" "0.5"]);
a3.y_ticks = tlist(["ticks", "locations", "labels"],..
    [0.4 0.45 0.5 0.55 0.6 0.65 0.7 0.75 0.8 0.85 0.9],..
    ["0.4" "0.45" "0.5" "0.55" "0.6" "0.65" "0.7" "0.75" "0.8" "0.85" "0.9"]);
drawnow();
disp("");
mprintf("halt mode: enter empty lines to continue.");
mscanf("%c");

mprintf("%s\n%s\n", text(105:106)');
clear text
