Framework and Function

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