Framework and Function of Each Module
RTB
©À©¤ load.sci
©À©¤ Transformations
©À©¤ Kinematics
©À©¤ Dynamics
©À©¤ System
©À©¤ Trajectory
©À©¤ GUI opengl
©¦ ©À©¤ tkogl
©¦ ©À©¤ opengl
©¦ ©À©¤ openglu
©¦ ©¸©¤ tkogldemos
©À©¤ doc
©¸©¤ demo
©¸©¤ robot_3d
©À©¤ puma560
©¸©¤ yas
Transformation Module
Our toolbox describes the location of rigid body by homogeneous transformation, connects movements and transformations with matrix operations. For robot arm, the function of joint variables ¦È 1 £¬ ¦È 2 £¬¡¡£¬ ¦È 6 is used to describe the location where the end coordinate frame is with the base frame as reference object. Calculating the transformation matrix with the joint variables ¦È 1 £¬ ¦È 2 £¬¡¡£¬ ¦È 6 is direct kinematics; and calculating its joint variables with given matrix is inverse kinematics.
So, transformation module is the base of direct and inverse kinematics.
The framework of this module:
transformations
©À©¤ rotx.sci revolve around x axes
©À©¤ roty.sci revolve around y axes
©À©¤ rotz.sci revolve around z axes
©À©¤ tr2eul.sci switch transformation matrix to Euler angles
©À©¤ eul2tr.sci switch Euler angles to transformation matrix
©À©¤ tr2rot.sci homogeneous transform to rotation submatrix
©À©¤ tr2rpy.sci switch transformation matrix to RPY(roll,pitch,yaw)
©À©¤ transl.sci
set or extract the translational component of a homogeneous transform
©À©¤ trnorm.sci
normalize a homogeneous transform
©¸©¤ rpy2tr.sci switch RPY to transformation matrix
Functions and usages in this module £º
function 
r = rotx £¨ t £© 
input 
t (radian) 
output 
r 4x4 transformation matrix 
usage 
get x axes transformation matrix r by t 
function 
r = roty £¨ t £© 
input 
t (radian) 
output 
r 4x4 transformation matrix 
usage 
get y axes transformation matrix r by t 
function 
r = rotz £¨ t £© 
input 
t (radian) 
output 
r 4x4 transformation matrix 
usage 
get z axes transformation matrix r by t 
function 
euler = tr2eul(m) 
input 
m is 4x4 transformation matrix 
output 
Euler angles 
usage 
get Euler angles by transformation matrix 
function 
r = eul2tr(phi,theta,psi) 
input 
phi theta psi are Euler angles 
output 
r is transformation matrix 
usage 
get transformation matrix by Euler angles 
function 
R = tr2rot(T) 
input 
T is 4x4 transformation matrix 
output 
R 3x3 rotary matrix 
usage 
get rotary matrix by transformation matrix 
function 
rpy = tr2rpy(m) 
input 
m is 4x4 transformation matrix 
output 
rpy is RPY angle 
usage 
get RPY by transformation matrix 
function 
r = transl(x,y,z) 
input 
x,y,z are the horizontal movement of x,y,z axes 
output 
r is transformation matrix 
usage 
set or extract the translational component of a homogeneous transform

function 
r = trnorm(t) 
input 
t is 4x4 transformation matrix 
output 
r is unitized transformation matrix 
usage 
normalize a homogeneous transform

function 
r = rpy2tr(roll, pitch, yaw) 
input 
roll pitch yaw are the parameters of RPY angle 
output 
r is 4x4 transformation matrix 
usage 
get transformation matrix by RPY 
Kinematics Module
Based on displacement analyses, there is velocity analyses in this module. Jacobian matrix is used to show the velocity linear mapping connection between operational space and joint space.
The framework of this module:
Kinematics
©À©¤ ikine.sci solve inverse kinematics
©À©¤ fkine.sci solve direct kinematics
©À©¤ jacobn.sci get
compute Jacobian in endeffector coordinate frame
©À©¤ jacob0.sci get
compute Jacobian in base coordinate frame
©À©¤ tr2diff.sci
homogeneous transform to differential motion vector
©À©¤ tr2jac.sci switch transformation matrix to Jacobian matrix
©¸©¤ diff2tr.sci switch differential coefficient vector to transformation matrix
Functions and usages in this module £º
function 
qt = ikine(robot,tr,q,m) 
input 
robot is the object, tr is location matrix, q is a hypothesis angle, m is 1x6 filtrate vector 
output 
qt is an angle vector 
usage 
solve inverse kinematics 
function 
t = fkine(robot,q) 
input 
robot is the object, q is a joint angle vector 
output 
t is a location matrix 
usage 
solve direct kinematics 
function 
J0 = jacob0(robot,q) 
input 
robot is the object, q is a joint angle vector 
output 
J0 is Jacobian matrix 
usage 
compute Jacobian in base coordinate frame

function 
J = jacobn(robot,q) 
input 
robot is the object, q is a joint angle vector 
output 
J is Jacobian matrix 
usage 
compute Jacobian in endeffector coordinate frame

function 
d = tr2diff(t1, t2) 
input 
t1 is a location matrix £¬ t2 is 0 or location matrix 
output 
d is differential matrix £¬ t1,t2 are two differential matrix apart unit hour 
usage 
1.if t2 is 0 £¬ get the differential matrix t1describes
2.if t2 is location matrix £¬ get the differential matrix apart t1,t2 unit hour 
function 
J = tr2jac(t) 
input 
t Îª transformation matrix 
output 
J is Jacobian matrix 
usage 
switch transformation matrix to Jacobian matrix 
function 
delta = diff2tr(d) 
input 
d is differential matrix 
output 
delta is transformation matrix 
usage 
switch differential matrix to transformation matrix 
Dynamics Module
There are two parts in dynamics of robot arm: direct dynamics ¡ª¡ª calculate the joint displacement, velocity and acceleration of robot arm by the joint force or moment; inverse dynamics ¡ª¡ª calculate the joint force or moment by the joint displacement, velocity and acceleration in movement. Both direct and inverse dynamics are solved in this module by using SCILAB. To solve problems in dynamics completely, centrifugal force and friction are also considered.
The framework of this module:
Dynamics
©À©¤ rne.sci solve inverse dynamics
©À©¤ accel.sci solve direct dynamics
©À©¤ cinertia.sci solve Descartes inertia matrix
©À©¤ coriolis.sci solve complex acceleration centripetal matrix
©À©¤ friction.sci calculate friction
©À©¤ itorque.sci
Compute the manipulator inertia torque
©À©¤ inertia.sci Compute the manipulator inertia matrix
©À©¤ gravload.sci
Compute the gravity loading on manipulator joints
©¸©¤ nofriction.sci without friction
Functions and usages in this module £º
function 
tau = rne(robot,a1,a2,a3) 
input 
robot is the object £¬ a1,a2,a3 are angle, angular speed£¬
is angular acceleration

output 
tau is moment 
usage 
solve inverse dynamics 
function 
qdd = accel(robot, Q, qd, torque) 
input 
robot is the object £¬ Q is an angle £¬ qd is angular speed£¬ torque is hypothesize moment 
output 
Qdd
angular acceleration

usage 
solve direct dynamics 
function 
Mx = cinertia(robot, q) 
input 
robot is the object £¬ Q is an angle 
output 
Mx is inertia moment 
usage 
solve Descartes inertia matrix 
function 
c = coriolis(robot, q, qd) 
input 
robot is the object £¬ q is an angle, qd
is angular speed

output 
c is complex acceleration centripetal matrix 
usage 
solve complex acceleration centripetal matrix 
function 
tau = friction(l, qd) 
input 
l is a joint £¬ qd
angular speed

output 
Tau is friction 
usage 
calculate friction 
function 
M = inertia(robot, q) 
input 
robot is the object £¬ q is an angle 
output 
manipulator inertia matrix 
usage 
Compute the manipulator inertia matrix 
function 
it = itorque(robot, q, qdd) 
input 
robot is the object £¬ q is an angle, qdd
is angular acceleration

output 
it is inertia matrix 
usage 
Compute the manipulator inertia torque 
function 
tg = gravload(robot, q) 
input 
robot is the object £¬ q is an angle 
output 
Tg is the centrifugal force 
usage 
calculate the centrifugal force 
function 
l2 = nofriction(l, only) 
input 
l is an joint, only is to eliminate parameter 
output 
L2 is the joint without friction 
usage 
eliminate friction 
System Module
System module is also a module built on SCILAB directly. Robot and matrix are constructed in this module.
The framework of this module:
System
©À©¤ unit.sci unitize matrix
©À©¤ cross.sci C=AxB
©À©¤ linktrans.sci calculate the transformation matrix of two joints
©À©¤ puma560.sci construct puma560 robot
©À©¤ createlink.sci construct joint
©À©¤ setI.sci setup joint I variable
©À©¤ m2str.sci change matrix into character
©¸©¤ tk_sim.sci simulation function for demo
Functions and usages in this module £º
function 
u = unit(v) 
input 
v is a matrix 
output 
u is a unitized matrix 
usage 
unitize transformation matrix 
function 
C = cross(p,n) 
input 
p £¬ n are two vectors 
output 
C is a vector 
usage 
C=AxB 
function 
T = linktrans(Robolink,q) 
input 
Robolink is a joint, q is joint angle 
output 
T is connected matrix 
usage 
calculate the transformation matrix of two joints 
function 
function robot = puma560() 
input 

output 
Robot is puma560 robot 
usage 
construct puma560 
function 
function L = createlink(dh,RP) 
input 
dh is dh parameter £¬ RP is characters to show the joint do turning or horizontal movement. 
output 
L is a joint 
usage 
construct joint 
function 
I = setI(v) 
input 
v is vector I 
output 

usage 
setup variable joint I 
function 
str = m2str(m) 
input 
m is a matrix 
output 
str is the character string of m 
usage 
switch matrix to character 
Trajectory Module
Trajectory planning of robot arm is an absolutely necessary module. As long as inputting the beginning and end location of robot arm, the movement trajectory can be plansned quickly by the results from modules above.
Functions and usages in this module £º
Contrail
©À©¤ jtraj.sci plan the trajectory among joints
©¸©¤ jtraj5.sci plan the trajectory among joints
function 
[qt,qdt,qddt] = jtraj(q0, q1, tv) 
input 
q0 is the beginning condition of joint angle, q1 is the end condition of joint angle £¬ tv is the control cycle 
output 
qt is joint angle £¬ qdt is joint angular speed£¬ qddt is joint
angular acceleration

usage 
plan the trajectory among joints 
function 
[qt,qdt,qddt] = jtraj5(q0, q1, tv,qd0,qd1) 
input 
q0 is the beginning condition of joint angle, q1 is the end condition of joint angle £¬ tv is the control cycle £¬ qd0 is initial angular speed, qd1 is final angular speed 
output 
qt is joint angle £¬ qdt angular speed£¬ qddt angular acceleration 
usage 
plan the trajectory among joints 
Demo Module
This module shows the interface for users, from where users can feel all functions of other modules clearly.
Demo
©¸©¤ robotdemo.sci run demo
function 
Robotdemo 
input 

output 

usage 
run demo 
