Laboiation iepoit: Biiect anu inveise kinematics

Robotica 2uu9, Instituto Supeiioi Tcnico

Nagnus Anueisson, ist662u9, anumagnu@stuuent.chalmeis.se
viktoi Ronneit, ist64Su4, ionneit@stuuent.chalmeis.se
Summaiy
This pioject will show how to contiol the iobot R0BSTRS in oiuei peifoim
simple tasks such as moving one object fiom one point to anothei. We weie
given some instiuctions in Natlab to communicate with oui iobot. We then
uevelop Natlab coue to be able to make the iobot move as we want. 0ui coue
uoes not only calculate cooiuinates oi angles to get the iobot to move to a
ceitain point, it also calculates if a point is out of ieach uue to the iobots physical
limitations anu it simulates the movement in Natlab.
We have useu uiiect kinematics to finu out what cooiuinates the enu effectoi
(the iobots claw) is in with given angles to each joint of the R0BSTRS. Biiect
kinematics is a mathematical way to finu the position in a space fiom knowing
the angles between iobots aims.
In the othei uiiection, to get the angles of each joint fiom a uesiieu position anu
oiientation of the enu effectoi we useu inveise kinematics. Inveise kinematics is
anothei mathematical methou, which can give seveial solutions foi each point
foi the enu effectoi.
Equipment
The tools useu aie Natlab, the iobot R0BSTRS anu a R0BSTRS RS2S2 cable to
connect Natlab with the iobot. The iobot has some limitations in how much it
can tuin each joint. Limits aie shown in Table 1 anu in Pictuie1 the angles anu
shown on the iobot.






Iointu Ioint1 Ioint2 IointS Ioint4
u-16u u-1uu u-1uu u-2uu u-2uu
Table 1

Pictuie1
Puipose
We want a iobot to peifoim tasks that we uo not want to uo ouiselves. This has
auvantages such as ieplacing a human woikei who peifoims tasks that involves
secuiity iisks.

uoal
The piojects puipose is to cieate Natlab coue with which we can contiol the
R0BSTRS. With the coue you shoulu be able to move the iobot anu peifoim a
task by just giving inputs as positions in x y z cooiuinates oi angles between the
iobots aims.

Realization
We founu it most piactical to solve the uiiect kinematics fiist as the inveise
kinematics neeus some cooiuinates foi some joints geneiateu by the uiiect
kinematics in oiuei to finish the calculations. When we hau the uiiect kinematics
woiking we completeu the inveise kinematics.
With these coineistones of oui pioject uone we maskeu it all togethei. To
finish it all we maue it possible to simulate the movements in Natlab anu
peifoimeu tests both in simulation anu with the iobot.

Results
0ui Natlab coue calculates if a uesiieu position foi the iobot is possible to ieach
with the given physical limitations. When a position in iange is wanteu the iobot
moves accoiuing to the instiuctions. In oiuei to see what the instiuctions you
give the iobot will iesult in you can simulate the positions.

Conclusions
The iobot moves as we tell it to uo both if we choose to contiol it by angles oi
positions. In this way we can make the iobot peifoim the tasks we wish. If we
woulu like to peifoim one specific task many times it woulu be moie efficient to
cieate seiies of movements. Then you coulu choose a sequence to be maue
insteau.
0ui goal was to contiol the iobot via Natlab, which we can. That is the
founuation to make the contiol of the iobot moie usei fiienuly.




Inveise Kinematics
In the inveise kinematics we stait by calculating the !1 anu !2 angles by using
the foimulas given on the sliues at the classes. To calculate !S we make vectois
of the last two iobot aims, we also make one that is tuineu 9u uegiees fiom the
seconu last iobot aim. By calculating the angles between the uiffeient vectois we
can use algebia to get !S.

!2 = acos((x^2+y^2-aim1^2-aim2^2) (2*aim1*aim2))
!1 = Beta +- o wheie
Beta = atan2(y,x) anu
o = acos((x^2+y^2+aim1^2-aim2^2) (2*aim1*sqit(x^2+y^2)))

Bow we calculate !S:
R9u is a iotation matiix that we use to iotate the seconu last aim of the iobot 9u
uegiees aiounu one of the axis x y oi z. Which axis we choose to tuin aiounu
uepenus on wheie the last aim of the iobot is, we make suie it tuins aiounu an
axis that cieates an angle between the actual iobot aims anu the cieateu vectoi.
We then use the iotateu vectoi to compaie with vectois of the two last aims of
the iobot. By iotating the seconu last aim by 9u uegiees as a iefeience we get a
vectoi that is in the same plane as the aims. Aftei those steps we can calculate
!3.
To get the angle between two vectois we use the function acosu(). Ex:
alfa=acosu(uot(Seconu_last_aim, Last_aim)noim(Seconu_last_aim)*noim(Last_aim)).

Example of the calculation of !S:


The angle we calculate in the inveise kinematics anu use in oui uiiect kinematics
is the one calleu uiff oi uiff1 in Pictuie2. We use the foimula acosu() to get the
angle alfa anu uiff. acosu() calculates the closest angles between two aims anu
will theiefoie give the same angle foi both aims calleu "Last aim1" anu "Last
aim" compaieu to the aim calleu "Seconu last aim". As we also know the angle
uiff we can be suie of wheie the Last aim is. If it woulu be Last aim1 in Pictuie2
then:
18u - alfa = 9u - uiff.
If we compaie those angles we can be ceitain of wheie the enu effectoi is.


Pictuie 2
Biiect Kinematics

In the uiiect kinematics we use 6 uiffeient fiamescooiuinate systems assigneu
names A, B,.,u. Wheie the fiamecooiuinate system A iepiesents "The woilu"
anu the u iepiesents the "Enu effectoi". We use 6 cooiuinate systems because
R0BS has 6 movable joints which gives the iobot 6 uegiees of fieeuom.
Each fiamecooiuinate system has a vectoi connecting the oiigin of the fiames
oiigin with a point . The point which is the position of the oiigin in the next
fiamecooiuinate system. Foi example, fiame A has a vectoi "
a
Pb oiigin"
connecting the oiigin of fiame aP with the oiigin of fiam B,
b
P.
The iotation between each fiame is maue by a iotation matiix. This matiix is
built up like this:
a
R
b
= |
a
Xb
a
Yv
a
Zb] = |Xb*Xa Yb*Xa Zb*Xa
Xb*Ya Yb*Ya Zb*Ya
Xb*Za Yb*Za Zb*Za]
(The iotation matiix useu in the R0BS pioblem aie on the next page)
Each "position" in the matiix iepiesents a piojection on an axis in the fiame
befoie. Foi example
a
Xb is the cooiuinates of Xb in ielation to fiame A. The
cooiuinates of Xb is piojecteu on each axis in fiame A. In the iotation matiix we
have also useu two constants, -9u anu 9u uegiees. These aie useu foi tuining
fiame C anu F 9u uegiees.
In compact foim we now have that:
a
P =
a
bR*
b
P
To solve this equation we use homogeneous tiansfoimations. We uo this by
setting up oui matiix like this:
a
bT = |
a
bR
a
Pb oiigin
u u u 1]
All example until now has been between fiame A anu B. If we want to have the
position of the oiigin in fiam u, the enu effectoi. Then we just multiply all
homogeneous tiansfoimations, we now have all the infoimation we neeu foi
solving the uiiect kinematics pioblem.
aP =
a
gT*
g
P
The equation above gives us the position of the enu effectoi in the woilu fiame.
If we want to know the position of "jointS" we just change the equation:
a
P =
c
uT*
u
P.
!"#$#%"&'($#)%*'!"#$%$&#'()*$+**'(,-%.*/01##-2&'%$*(/3/$*./4'
'
$+!'','56#/!$7*$%!8449(:/&'!$7*$%!8449(;<( ( ( (
((((((((( /&'!$7*$%!8449(6#/!$7*$%!8449(;<(
((((((((( ;9(;9(8=<(
'''''''''
+-!','56#/!$7*$%!>449(:/&'!$7*$%!>449(;<(
( ((/&'!$7*$%!>44?6#/!@7&!8449(6#/!$7*$%!>44?6#/!@7&!8449(:/&'!@7&!844<(
( ((/&'!$7*$%!>44?/&'!@7&!8449(6#/!$7*$%!>44?/&'!@7&!8449(6#/!@7&!844=<(
(((
-.!'A(56#/!$7*$%!B449(:/&'!$7*$%!B449(;<(
( (((/&'!$7*$%!B44(9(6#/!$7*$%!B449(;<(
( (((;9(;9(;=<(
''''
./!'A(56#/!$7*$%!C449:/&'!$7*$%!C44(9(;<(
(((((((( /&'!$7*$%!C449(6#/!$7*$%!C449(;<(
((((((( ;9(;9(8=<(
''''''''
/0!'A(56#/!$7*$%!D449(:/&'!$7*$%!D449(;<(
/&'!$7*$%!D44?6#/!@7&!>449(6#/!$7*$%!D44?6#/!@7&!>449(:/&'!@7&!>44<(
( /&'!$7*$%!D44?/&'!@7&!>449(6#/!$7*$%!D44?/&'!@7&!>449(6#/!@7&!>44=<(
' ''''
01!'A(589(;9(;<(
;9(89(;<(
;9(;9(8=<(
!"#$#%
!
&'(")#%*+'",-$+.#%/0'12"#%34%-22%53+'$#6%
"#$%&!'()*!!()*!!)*+,!'-!!-!!*+!
.#%/0$#1&!'2)!!234*2!!562477*8!!5364-8--!!*!!*+!
"#$%&!'88*!!*!!-8*+,!'*!!*!!-+!
.#%/0$#1&!!'*!!*!!*!!*!!*!!*+!
"#$%&!'29*!!*!!*+!'-!!*!!*+!
.#%/0$#1&!'*!!*!!*!!56*!!*!!*+!
"#$%&!'()*!!5()*!!2*+!'-!!5-!!*+!
.#%/0$#1&!'-()!!()4-2)6!!7)47392!!794863(!!-)!!*+!
"#$%&!'23*!!*!!*+!'-!!*!!*+!
.#%/0$#1&!:00;0&!<;=>%>;$!>=!;/%!;?!0@$A#!;0!%;!?@0!@B@CD!
%
7+)".$%*+'",-$+.#%/83#+$+3'%34%"'9%"44".$3)6%
"#$%&!'*!!*!!*!!*!!*!!*+!
.#%/0$#1&!'88*!!*!!-8*+!
"#$%&!'52*!!)*!!52*!5)!!*!!*+!
.#%/0$#1&!'-734732-!!5-)3492)-!!8*)4(77)+!
"#$%&!'2)!!234*2!!562477*8!!5364-8--!!*!!*+!
.#%/0$#1&!'(8-4)-32!!(8-4)-32!!5(74-72(+!
"#$%&!'2)!!2)!!2)!!2)!!*!!*+!
.#%/0$#1&!:00;0&!!E%!F#@=%!;$#!@$AF#!>=!;/%!;?!0@$A#D!
%
7+)".$%*+'",-$+.#%/83#+$+3'%:3+'$%;6%
"#$%&!'52*!!)*!!52*!5)!!*!!*+!
.#%/0$#1&!'67427*7!!57(498)(!!-)84(*76+!
"#$%&!'*!!*!!*!!*!!*!!*+!
.#%/0$#1&!'(**!!*!!*+!
!
7+)".$%*+'",-$+.#%/83#+$+3'%-22%53+'$#6%
"#$%&!'*!!*!!56*!!5-7*!!*!!*+!
.#G>#H#1&!
! ! I;>$%-&!'*!!*!!*+!
! ! I;>$%(&!'*!!*!!*+!
I;>$%8&!'(**!!*!!*+!!
I;>$%2&!'(**!!*!!5-8*+!!
I;>$%)&!'(**!!*!!5-8*+!!
I;>$%9&!'3*!!*!!5-8*+!
!
!
!
!
!
!
Manual


Direct Kinematics

Function: directkinematics(frame,theta)

Input: "frame", a number between 1 and 6. 1 will tell the function
to return the position of the 1st frame "The world". 6 will
tell the function to return the position of the "end
effector".
"theta", a vector uncluding the angles of all joints.

Output: "vector_to_choosen_point" A vector including the
coordinates(X,Y,Z) for the choosen frame/point/joint.

Frames: 1 gives Joint 1, "The World"
2 gives Joint 2, "Spinning Sideways"
3 gives Joint 3, "Up and Down, big arm"
4 gives Joint 4, "Up and Down, small arm"
5 gives Joint 5, "Up and Down, gripper"
6 gives Joint 6, "Gripper"

Ex: directkinematics(6,[0 0 0 0 0 0])
Returns: [330; 0; 130]


Inverse Kinematics

Function: invkinematics(pos,orientation)

Input:"pos", a vector with the coordinates(x,y,z) for the end
effector
"orientation", a vector pointing in the direction the end
effector should point at.
Output: "theta", a vector uncluding the angles of all joints.

Ex: invkinematics([330 0 130][0 0 1])
Returns: [0 0 0 0 0 0]

Move ROB3 by giving it angles

Function: move_angles(obj,theta)

Input: "obj", is an object created by the set_serial.
"theta", a vector uncluding the angles of all joints.

Output: "vector_including_all_frames" A vector including the
positions of all the frames/points/joints.
"Movement of ROB3"

Ex: move_angles(obj, [0 0 -90 -180 0 0])
Returns: [0 0 200 200 200 70; 0 0 0 0 0 0; 0 0 0 -130 -130 -
130]






Move ROB3 by giving it coordinates

Function: move_coord(obj,pos,orientation)

Input: "obj", is an object created by the set_serial.
"pos", a vector with the coordinates(x,y,z) for the end
effector
"orientation", a vector pointing in the direction the end
effector should point at.

Output: "vector_including_all_frames" A vector including the
positions of all the frames/points/joints.
"Movement of ROB3"

Ex: move_coord(obj, [70 0 -130] [-1 0 0])
Returns: [0 0 200 200 200 70; 0 0 0 0 0 0; 0 0 0 -130 -130 -
130]

Simulator

Function: simulator(obj)

Input: "obj", is an object created by the set_serial.

Output: "Graphical preview"
"Movement of ROB3"

Ex: -

Set up communication with ROB3

Function: obj = set_serial

Input: -

Output: "obj", is an object that takes care of the communication.


Reset the communication with ROB3

Function: res_serial

Input: -

Output: -