=====================================================
 ROBOOP -- A robotics object oriented package in C++ 
 DEMO program 
=====================================================


=====================================================
Homogeneous transformations
=====================================================

Translation of [1;2;3]
  1.000   0.000   0.000   1.000 
  0.000   1.000   0.000   2.000 
  0.000   0.000   1.000   3.000 
  0.000   0.000   0.000   1.000 


Rotation of pi/6 around axis X
  1.000   0.000   0.000   0.000 
  0.000   0.866  -0.500   0.000 
  0.000   0.500   0.866   0.000 
  0.000   0.000   0.000   1.000 


Rotation of pi/8 around axis Y
  0.924   0.000   0.383   0.000 
  0.000   1.000   0.000   0.000 
 -0.383   0.000   0.924   0.000 
  0.000   0.000   0.000   1.000 


Rotation of pi/3 around axis Z
  0.500  -0.866   0.000   0.000 
  0.866   0.500   0.000   0.000 
  0.000   0.000   1.000   0.000 
  0.000   0.000   0.000   1.000 


Rotation of pi/3 around axis [1;2;3]
  0.536  -0.623   0.570   0.000 
  0.766   0.643  -0.017   0.000 
 -0.356   0.446   0.821   0.000 
  0.000   0.000   0.000   1.000 


Rotation of pi/6 around line [1;2;3] -- [3;2;1]
  0.750   0.612  -0.250  -0.225 
 -0.612   0.500  -0.612   3.449 
 -0.250   0.612   0.750  -0.225 
  0.000   0.000   0.000   1.000 


Roll Pitch Yaw Rotation [1;2;3]
  0.412  -0.834  -0.368   0.000 
 -0.059  -0.427   0.902   0.000 
 -0.909  -0.350  -0.225   0.000 
  0.000   0.000   0.000   1.000 


Euler ZXZ Yaw Rotation [3;2;1]
 -0.485   0.865   0.128   0.000 
  0.423   0.104   0.900   0.000 
  0.765   0.491  -0.416   0.000 
  0.000   0.000   0.000   1.000 


Inverse of Rotation around axis
  0.267 
  0.535 
  0.802 
  1.047 


Inverse of Roll Pitch Yaw Rotation
 -2.142 
  1.142 
 -0.142 


Inverse of Euler ZXZ Rotation
  3.000 
  2.000 
  1.000 


Cross product between [1;2;3] and [3;2;1]
 -4.000 
  8.000 
 -4.000 


=====================================================
Robot RR kinematics
=====================================================

Robot D-H parameters
   type     theta      d        a      alpha
  0.000   0.000   0.000   1.000   0.000 
  0.000   0.000   0.000   1.000   0.000 

Robot joints variables
  0.000 
  0.000 

Robot position
  1.000   0.000   0.000   2.000 
  0.000   1.000   0.000   0.000 
  0.000   0.000   1.000   0.000 
  0.000   0.000   0.000   1.000 

Robot joints variables
  0.785 
  0.785 

Robot position
  0.000  -1.000   0.000   0.707 
  1.000   0.000   0.000   1.707 
  0.000   0.000   1.000   0.000 
  0.000   0.000   0.000   1.000 

Robot Jacobian w/r to base frame
 -1.707  -1.000 
  0.707   0.000 
  0.000   0.000 
  0.000   0.000 
  0.000   0.000 
  1.000   1.000 

Robot Jacobian w/r to tool frame
  0.707   0.000 
  1.707   1.000 
  0.000   0.000 
  0.000   0.000 
  0.000   0.000 
  1.000   1.000 

Robot position partial derivative with respect to joint 1 
 -1.000   0.000   0.000  -1.707 
  0.000  -1.000   0.000   0.707 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 2 
 -1.000   0.000   0.000  -1.000 
  0.000  -1.000   0.000   0.000 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot inverse kinematics
  q start  q final  q real
  0.785   0.785   0.785 
  0.785   0.785   0.785 


=====================================================
Robot RP kinematics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.000   0.000  -1.571 
  1.000   0.000   0.000   0.000   0.000 

Robot joints variables
  0.000 
  0.000 

Robot position
  1.000   0.000   0.000   0.000 
  0.000   0.000   1.000   0.000 
  0.000  -1.000   0.000   0.000 
  0.000   0.000   0.000   1.000 

Robot joints variables
  0.785 
  4.000 

Robot position
  0.707   0.000  -0.707  -2.828 
  0.707   0.000   0.707   2.828 
  0.000  -1.000   0.000   0.000 
  0.000   0.000   0.000   1.000 

Robot Jacobian w/r to base frame
 -2.828  -0.707 
 -2.828   0.707 
  0.000   0.000 
  0.000   0.000 
  0.000   0.000 
  1.000   0.000 

Robot Jacobian w/r to tool frame
 -4.000   0.000 
  0.000   0.000 
  0.000   1.000 
  0.000   0.000 
 -1.000   0.000 
  0.000   0.000 

Robot position partial derivative with respect to joint 1 
 -0.707   0.000  -0.707  -2.828 
  0.707   0.000  -0.707  -2.828 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 2 
  0.000   0.000   0.000  -0.707 
  0.000   0.000   0.000   0.707 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot inverse kinematics
 q start q final q real
  0.196   0.785   0.785 
  1.000   4.000   4.000 


=====================================================
Robot PUMA560 kinematics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.000   0.000   1.571 
  0.000   0.000   0.000   0.432   0.000 
  0.000   0.000   0.150   0.020  -1.571 
  0.000   0.000   0.432   0.000   1.571 
  0.000   0.000   0.000   0.000  -1.571 
  0.000   0.000   0.000   0.000   0.000 

Robot joints variables
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 

Robot position
  1.000   0.000   0.000   0.452 
  0.000   1.000   0.000  -0.150 
  0.000   0.000   1.000   0.432 
  0.000   0.000   0.000   1.000 

Robot joints variables
  0.785 
  0.785 
  0.785 
  0.785 
  0.785 
  0.785 

Robot position
 -0.957   0.250  -0.146   0.017 
  0.250   0.457  -0.854  -0.196 
 -0.146  -0.854  -0.500   0.326 
  0.000   0.000   0.000   1.000 

Robot Jacobian w/r to base frame
  0.196  -0.230  -0.014   0.000   0.000   0.000 
  0.017  -0.230  -0.014   0.000   0.000   0.000 
  0.000  -0.126  -0.432   0.000   0.000   0.000 
  0.000   0.707   0.707  -0.707   0.500  -0.146 
  0.000  -0.707  -0.707  -0.707  -0.500  -0.854 
  1.000   0.000   0.000   0.000   0.707  -0.500 

Robot Jacobian w/r to tool frame
 -0.183   0.181   0.073   0.000   0.000   0.000 
  0.057  -0.055   0.358   0.000   0.000   0.000 
 -0.043   0.293   0.230   0.000   0.000   0.000 
 -0.146  -0.854  -0.854   0.500  -0.707   0.000 
 -0.854  -0.146  -0.146  -0.500  -0.707   0.000 
 -0.500   0.500   0.500   0.707   0.000   1.000 

Robot position partial derivative with respect to joint 1 
 -0.250  -0.457   0.854   0.196 
 -0.957   0.250  -0.146   0.017 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 2 
  0.104   0.604   0.354  -0.230 
  0.104   0.604   0.354  -0.230 
 -0.500   0.500  -0.707  -0.126 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 3 
  0.104   0.604   0.354  -0.014 
  0.104   0.604   0.354  -0.014 
 -0.500   0.500  -0.707  -0.432 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 4 
  0.104   0.604   0.354   0.000 
 -0.104  -0.604  -0.354   0.000 
 -0.854  -0.146   0.500   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 5 
 -0.104   0.104   0.854   0.000 
 -0.604   0.604   0.146   0.000 
 -0.354   0.354  -0.500   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 6 
  0.250   0.957   0.000   0.000 
  0.457  -0.250   0.000   0.000 
 -0.854   0.146   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot inverse kinematics
 q start q final q real
  0.196   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 


=====================================================
Robot STANFORD kinematics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.412   0.000  -1.571 
  0.000   0.000   0.154   0.000   1.571 
  1.000  -1.571   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000  -1.571 
  0.000   0.000   0.000   0.000   1.571 
  0.000   0.000   0.263   0.000   0.000 

Robot joints variables
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 

Robot position
  0.000   1.000   0.000   0.000 
 -1.000   0.000   0.000   0.154 
  0.000   0.000   1.000   0.675 
  0.000   0.000   0.000   1.000 

Robot joints variables
  0.785 
  0.785 
  0.785 
  0.785 
  0.785 
  0.785 

Robot position
  0.073  -0.280   0.957   0.536 
  0.280   0.927   0.250   0.567 
 -0.957   0.250   0.146   1.006 
  0.000   0.000   0.000   1.000 

Robot Jacobian w/r to base frame
 -0.567   0.420   0.500  -0.027   0.066   0.000 
  0.536   0.420   0.500   0.159  -0.120   0.000 
  0.000  -0.780   0.707  -0.093  -0.224   0.000 
  0.000  -0.707   0.000   0.500  -0.146   0.957 
  0.000   0.707   0.000   0.500   0.854   0.250 
  1.000   0.000   0.000   0.707  -0.500   0.146 

Robot Jacobian w/r to tool frame
  0.109   0.895  -0.500   0.131   0.186   0.000 
  0.655   0.077   0.500   0.131  -0.186   0.000 
 -0.409   0.393   0.707   0.000   0.000   0.000 
 -0.957   0.146   0.000  -0.500   0.707   0.000 
  0.250   0.854   0.000   0.500   0.707   0.000 
  0.146  -0.500   0.000   0.707   0.000   1.000 

Robot position partial derivative with respect to joint 1 
 -0.280  -0.927  -0.250  -0.567 
  0.073  -0.280   0.957   0.536 
  0.000   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 2 
 -0.677   0.177   0.104   0.420 
 -0.677   0.177   0.104   0.420 
 -0.250  -0.457  -0.854  -0.780 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 3 
  0.000   0.000   0.000   0.500 
  0.000   0.000   0.000   0.500 
  0.000   0.000   0.000   0.707 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 4 
 -0.677  -0.530  -0.104  -0.027 
  0.530  -0.323   0.604   0.159 
  0.104   0.604  -0.354  -0.093 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 5 
 -0.677   0.677   0.250   0.066 
 -0.177   0.177  -0.457  -0.120 
 -0.104   0.104  -0.854  -0.224 
  0.000   0.000   0.000   0.000 

Robot position partial derivative with respect to joint 6 
 -0.280  -0.073   0.000   0.000 
  0.927  -0.280   0.000   0.000 
  0.250   0.957   0.000   0.000 
  0.000   0.000   0.000   0.000 

Robot inverse kinematics
 q start q final q real
  0.196   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 
  1.000   0.785   0.785 



=====================================================
Robot RR dynamics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.000   1.000   0.000 
  0.000   0.000   0.000   1.000   0.000 

Robot D-H inertial parameters
  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz
  2.000  -0.500   0.000   0.000   0.000   0.000   0.000   0.167   0.000   0.167 
  1.000  -0.500   0.000   0.000   0.000   0.000   0.000   0.083   0.000   0.083 

Robot joints variables
  0.000 
  0.000 

Robot Inertia matrix
  3.000   0.833 
  0.833   0.333 

Feedback gain matrix K
 25.000   0.000   7.071   0.000 
  0.000  25.000   0.000   7.071 

  time     q1       q2      q1p    q2p
  0.000   0.000   0.000   0.000   0.000 
  0.058  -0.011   0.081  -0.068   1.715 
  0.112  -0.004   0.169   0.325   1.537 
  0.178   0.032   0.264   0.738   1.329 
  0.229   0.076   0.327   0.991   1.196 
  0.279   0.132   0.385   1.195   1.081 
  0.332   0.199   0.439   1.353   0.977 
  0.387   0.277   0.490   1.465   0.881 
  0.448   0.367   0.541   1.526   0.790 
  0.516   0.472   0.591   1.527   0.701 
  0.597   0.593   0.645   1.448   0.608 
  0.693   0.725   0.698   1.268   0.510 
  0.773   0.819   0.736   1.069   0.432 
  0.845   0.888   0.765   0.869   0.363 
  0.911   0.939   0.786   0.680   0.300 
  0.972   0.975   0.803   0.504   0.240 
  1.031   1.000   0.815   0.343   0.185 
  1.088   1.016   0.824   0.198   0.134 
  1.143   1.023   0.830   0.071   0.087 
  1.196   1.024   0.834  -0.039   0.047 
  1.269   1.016   0.836  -0.168  -0.002 
  1.333   1.002   0.834  -0.257  -0.037 
  1.385   0.987   0.832  -0.314  -0.060 
  1.446   0.967   0.827  -0.363  -0.080 
  1.518   0.939   0.821  -0.398  -0.095 
  1.603   0.904   0.813  -0.412  -0.103 
  1.707   0.862   0.802  -0.395  -0.100 
  1.762   0.841   0.796  -0.374  -0.095 
  1.813   0.823   0.792  -0.349  -0.088 
  1.904   0.793   0.784  -0.294  -0.073 
  1.985   0.772   0.779  -0.238  -0.058 
  2.061   0.756   0.775  -0.185  -0.044 
  2.132   0.744   0.773  -0.135  -0.031 
  2.200   0.737   0.771  -0.090  -0.020 
  2.264   0.732   0.770  -0.051  -0.010 
  2.323   0.730   0.770  -0.018  -0.001 
  2.398   0.730   0.770   0.018   0.007 
  2.456   0.732   0.770   0.041   0.013 
  2.524   0.736   0.772   0.063   0.018 
  2.603   0.741   0.773   0.081   0.023 
  2.695   0.749   0.775   0.093   0.026 
  2.748   0.754   0.777   0.096   0.026 
  2.806   0.760   0.778   0.097   0.026 
  2.873   0.766   0.780   0.094   0.026 
  2.946   0.773   0.782   0.088   0.024 
  3.018   0.779   0.784   0.079   0.021 
  3.083   0.784   0.785   0.069   0.019 
  3.142   0.788   0.786   0.060   0.016 
  3.197   0.791   0.787   0.050   0.014 
  3.249   0.793   0.787   0.042   0.011 
  3.300   0.795   0.788   0.033   0.009 
  3.396   0.797   0.789   0.018   0.005 
  3.485   0.799   0.789   0.006   0.002 
  3.563   0.799   0.789  -0.003  -0.001 
  3.637   0.798   0.789  -0.010  -0.003 
  3.727   0.797   0.788  -0.017  -0.004 
  3.779   0.796   0.788  -0.019  -0.005 
  3.837   0.795   0.788  -0.021  -0.006 
  3.900   0.793   0.788  -0.023  -0.006 
  3.973   0.792   0.787  -0.023  -0.006 
  4.000   0.791   0.787  -0.023  -0.006 


Press Enter to continue...
Press Enter to continue...
=====================================================
Robot RP dynamics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.000   0.000  -1.571 
  1.000   0.000   0.000   0.000   0.000 

Robot D-H inertial parameters
  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz
  2.000   0.000   0.000   0.000   1.000   0.000   0.000   1.000   0.000   1.000 
  1.000   0.000   0.000  -1.000   0.083   0.000   0.000   0.083   0.000   0.083 

Robot joints variables
  0.000 
  0.000 

Robot Inertia matrix
  2.083   0.000 
  0.000   1.000 


=====================================================
Robot PUMA560 dynamics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.000   0.000   1.571 
  0.000   0.000   0.000   0.432   0.000 
  0.000   0.000   0.150   0.020  -1.571 
  0.000   0.000   0.432   0.000   1.571 
  0.000   0.000   0.000   0.000  -1.571 
  0.000   0.000   0.000   0.000   0.000 

Robot D-H inertial parameters
  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz
  0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.350   0.000   0.000 
 17.400  -0.364   0.006   0.228   0.130   0.000   0.000   0.524   0.000   0.539 
  4.800  -0.020  -0.014   0.070   0.066   0.000   0.000   0.086   0.000   0.013 
  0.820   0.000   0.019   0.000   0.002   0.000   0.000   0.001   0.000   0.002 
  0.340   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000   0.000 
  0.090   0.000   0.000   0.032   0.000   0.000   0.000   0.000   0.000   0.000 

Robot joints variables
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 

Robot Inertia matrix
  3.1771  -0.1627  -0.1389   0.0016  -0.0004   0.0000 
 -0.1627   2.1318   0.3727   0.0000   0.0019   0.0000 
 -0.1389   0.3727   0.3618   0.0000   0.0019   0.0000 
  0.0016   0.0000   0.0000   0.0016   0.0000   0.0000 
 -0.0004   0.0019   0.0019   0.0000   0.0006   0.0000 
  0.0000   0.0000   0.0000   0.0000   0.0000   0.0000 

Robot Torque
  0.0000 
 37.4837 
  0.2489 
  0.0000 
  0.0000 
  0.0000 

Robot acceleration
 -0.1640 
-21.3015 
 21.1946 
  0.1640 
  0.2037 
  0.0000 


=====================================================
Robot STANFORD dynamics
=====================================================

Robot D-H parameters
  type    theta     d       a     alpha
  0.000   0.000   0.412   0.000  -1.571 
  0.000   0.000   0.154   0.000   1.571 
  1.000  -1.571   0.000   0.000   0.000 
  0.000   0.000   0.000   0.000  -1.571 
  0.000   0.000   0.000   0.000   1.571 
  0.000   0.000   0.263   0.000   0.000 

Robot D-H inertial parameters
  mass     cx       cy      cz     Ixx     Ixy     Ixz     Iyy     Iyz     Izz
  9.290   0.000   0.018  -0.111   0.276   0.000   0.000   0.255   0.000   0.071 
  5.010   0.000  -1.054   0.000   0.108   0.000   0.000   0.018   0.000   0.100 
  4.250   0.000   0.000  -6.447   2.510   0.000   0.000   2.510   0.000   0.006 
  1.080   0.000   0.092  -0.054   0.002   0.000   0.000   0.001   0.000   0.001 
  0.630   0.000   0.000   0.566   0.003   0.000   0.000   0.003   0.000   0.000 
  0.510   0.000   0.000   1.554   0.013   0.000   0.000   0.013   0.000   0.000 

Robot joints variables
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 
  0.000 

Robot Inertia matrix
  4.691   4.037   0.000   0.005   0.000   0.000 
  4.037 181.090   0.058   0.000   0.000   0.000 
  0.000   0.058   6.470   0.000   0.000   0.000 
  0.005   0.000   0.000   0.005   0.000   0.000 
  0.000   0.000   0.000   0.000   1.902   0.000 
  0.000   0.000   0.000   0.000   0.000   0.000 

