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 end-effector 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 end-effector 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 |
|