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