See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/320331765

Workshop 1: Kinematics Modeling

Working Paper · October 2017

DOI: 10.13140/RG.2.2.33029.86249

CITATIONS READS

0 29

1 author:

Hans Milos Toquica Cáceres

National University of Colombia
4 PUBLICATIONS   0 CITATIONS   

SEE PROFILE

All content following this page was uploaded by Hans Milos Toquica Cáceres on 11 October 2017.

The user has requested enhancement of the downloaded file.

 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

Workshop 1: Kinematics Modeling

Toquica Cáceres Hans Milos, hmtoquicac@unal.edu.coζ
October 11, 2017
This workshop was typeset using the LATEX typesetting system originally developed by Leslie Lamport, based on TEX created by
Donald Knuth.

Abstract—ROS provides a way of communication between B. Practical Component

several devices, allowing robots to interact with each other
through the usage of several systems working interconnected.
In this workshop, a brief ROS script is implemented in order
to key-operate a Kobuki robot simulated in Gazebo.
Index Terms—Kobuki, Robotino, Kinematics Modeling,
ROS, Gazebo
I. I NTRODUCTION
T HE world of robotics, specially in the Industry 4.0 era
is aimed for interconnection in order to develop a single
task among several systems. In this document a script is
1) Make a basic model of the Kobuki robot for the Gazebo
simulator, based on its dimensions and calculations.
2) Create a Gazebo plugin in order to control the robot
from a computer keyboard. The ROS node must contain
a stop button, motion keys (forward, backward, right
and left), exit button (from the node), a topic containing
the linear and angular velocities. Take into account
the maximum linear speed is 1m/s and the maximum
angular speed is 180deg/s.

created in order to use a computer keyboard to control a IV. W ORKSHOP D EVELOPMENT

simulated robot in the same computer. A. Theoretical Component
Throughout this document, the reader will find the G OALS,
the W ORKSHOP D ESCRIPTION, the W ORKSHOP D EVELOPMENT, 1) Let XI and YI be the inertial reference system, and
the C ONCLUSIONS AND D ISCUSSION and lastly, the R EFER - XR and YR be the local (robot) reference system, then
ENCES, in case the reader would like to deepen in any
the robot pose can be describe as
 
subject. x
ξI =  y  ,
 
II. G OALS
θ
• Develop terrestrial mobile robot analysis, description
and programming abilities. and a rotation matrix relating the local and inertial
• Understand the concepts related with kinematics mod- reference frames is
els and apply model development using the Gazebo 
cos θ sin θ 0

simulator and the ROS framework.
R(θ) = − sin θ cos θ 0 ,
 
III. W ORKSHOP D ESCRIPTION 0 0 1
A. Theoretical Component then
1) Carry out the Kobuki mobile-robot kinematic descrip- ξ˙R = R(θ)ξ˙I . (*)
tion, include the description of the parameters r, l,
α and β. Additionally, describe the robot physical For forward motion, XR is positive, as shown in Fig.
dimensions, number and type of wheels it possess and 1, then the contribution of each wheel is
the wheels each wheel imposes on the robot motion
(J1 matrix). rϕ̇1
2) What would happen if parameter l is modified, letting X˙R1 = ,
2
l1 = l22 ? How does one modifies the contribution of ˙ rϕ̇2
XR2 = ,
each wheel so that the robot has pure linear motion 2
(v = V , ω = 0)? How are they compensated in order Y˙R = 0 ,
to have pure rotational motion (v = 0, ω = W )? rϕ̇1
3) Make the kinematic description of the mobile robot ωR1 = ,
2l
Robotino, according to the first item. rϕ̇2
ωR2 = − ,
ζ Mechatronics Engineering Student. 2l

1
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

The sliding restrictions for the wheels define

" #
cos(αl + βl ) sin(αl + βl ) l sin βl
C1 = ,
cos(αr + βr ) sin(αr + βr ) l sin βr
replacing αl , βl , αr and βr , it is obtained
" #
0 1 0
C1 = .
0 1 0
Also let
" #
→
− ϕl
ϕ = .
ϕr
Now, recalling that

J2 →
−
" # " #
J1 ϕ
˙
R(θ)ξI = →− ,
C1 0
−
→
Fig. 1: Reference system and parameters for kobuki analysis with replacing J1 , J2 , J2 and C1
P in the center.[1]
" # 
1 0 −l
 
rl 0 →−
1 ϕ
0 l   0

rr
so that  R(θ)ξ˙I =  ,
  
 rϕ̇1 +rϕ̇2  
2
0 1 0   0 
ξ˙I = R(θ)−1  0  , 0 1 0 0
 
r ϕ̇1 −r ϕ̇2
2l where the two last rows are redundant, and can be
also note that reduced to one, as follows
 
− sin θ
  " # 
cos θ 0 1 0 −l rl 0 → −
ϕ
R(θ)−1 =  sin θ cos θ 0 . (1) 1 0 l  R(θ)ξ˙I =  0 r .
 
(2)
  
r
0 0 1 0 1 0 0
The rolling restriction for the fixed standard wheels In order to obtain the velocity in the inertial refer-
makes the rolling restriction matrix be (the fixed stan- ence system, the J1 -C1 matrix needs to be inverted,
dard wheels non-motorized are dismissed because their multiplication shall be performed between the inverted
friction on the surface is taken as negligible, they don’t matrix and the expression obtained. This same process
contribute to J1 , J2 nor C1 ) is performed with the R(θ) matrix.
The inverse of the J1 -C1 matrix can be obtained by
means of using the Gauss-Jordan process as follows
" #
sin(αl + βl ) − cos(αl + βl ) −l cos βl
J1 = ,
sin(αr + βr ) − cos(αr + βr ) −l cos βr  
1 0 −l 1 0 0
for the Kobuki robot, αl = π/2, βl = 0, αr = −π/2  1 0

l 0  ,

0 1
and βr = π, evaluating in the J1 matrix
0 1 0 0 0 1
 
" # 1 0 −l 1 0 0
1 0 −l −1 1
J1 = .  0 0 l 0  ,
 
2 2
1 0 l
0 1 0 0 0 1
On the other hand, the constant diagonal matrix 
1 1
0 0 2 1
0

" # 2
rl 0  0

1 0 0 0 1  ,

J2 = .
0 rr 0 0 1 −1
2l
1
2l 0

2
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

then (2) becomes letting rr = rl = rw

1 1
 
2 2 0 " →−
# ϕr = ϕl = ϕw .
 J2 ϕ
ξ˙I = R(θ)−1  0 0 1 ,

0 For a linear speed v = V
−1 1
2l 2l 0
V = rw ϕw ,
R(θ)−1 comes from (1)
then the needed wheel speed in radians is
1 1
  
cos θ − sin θ 0 2 2 0 " →−
# V
 J2 ϕ ϕw = ϕl = ϕr = .
˙ξI = 
 sin θ cos θ 0  0

0 1 , rw
−1 1
0
0 0 1 2l 2l 0 On the other hand, for angular motion only (v = 0)
computing the first two matrices rl ϕl rr ϕr
0= + ,
2 2
 cos θ cos θ
 " #  rl ϕl = −rr ϕr ,
2 2 − sin θ rl 0 →−
ϕ
ξ˙I = letting rr = rl = rw
 sin θ sin θ
cos θ   0  ;

 2 2 rr
−1 1
2l 2l 0 0 −ϕl = ϕr = ϕw .
taking into account that
For an angular speed θ̇ = ω
rw ϕw
ω= ,
 
ẋ
l
˙ξI = 
ẏ  ,

then the needed wheel speed in radians is
θ̇
ωl
so ϕw = ,
rw
ωl
cos θ cos θ ϕr = ,
ẋ = rl ϕl + rr ϕr , rw
2 2 −ωl
sin θ sin θ ϕl = .
ẏ = rl ϕl + rr ϕr , rw
2 2
−rl ϕl rr ϕr ll
2) If lr = 2, as shown in Fig. 2, J1 matrix changes
θ̇ = + .
2l 2l
The speed can be calculatedp with its components, by
" #
1 0 −ll
the simple relation v = ẋ2 + ẏ 2 , whose calculation J1 = ll
,
is 1 0 2

then  
v 1 0 −ll
J2 →
−
u 2 2 " #
u rl ϕl cos2 θ rr2 ϕ2r cos2 θ 2 cos2 θrl ϕl rr ϕr ϕ
u + + + 1

0 ll  ˙
 R(θ)ξI = , (3)
4 4 4 2
v=u
u
, 0
2 2 2
t rl2 ϕ2l sin θ rr2 ϕ2r sin θ 2 sin θrl ϕl rr ϕr 0 1 0
+ +
4 4 4 calculating the inverse of the J1 -C1 matrix
so  
1 0 −l 1 0 0
1p
v= (rl ϕl + rr ϕr )2 ,  1 0 l2l 0 1 0  ,
 
2
rl ϕl + rr ϕr 0 1 0 0 0 1
v= ,
2
 
1 0 −ll 1 0 0
−rl ϕl rr ϕr −2 2
θ̇ = + .  0 0 ll

0  ,

2l 2l 3 3
0 1 0 0 0 1
For linear motion only (θ̇ = 0)
1 0 0 13 2
 
0
−rl ϕl rr ϕr 3
0= + ,  0 1 0 0 0 1   ,
2l 2l 
−2 2
rr ϕr = rl ϕl , 0 0 1 3ll 3ll 0

3
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

The speed can be calculated

p with its components, by
the simple relation v = ẋ2 + ẏ 2 , whose calculation
is

v
u 2 2
u rl ϕl cos2 θ 4rr2 ϕ2r cos2 θ 4 cos2 θrl ϕl rr ϕr
u + + +
v=u
u 9 9 9 ,
2 2 2 2 2 2 2
t rl ϕl sin θ 4rr ϕr sin θ 4 sin θrl ϕl rr ϕr
+ +
9 9 9
so
1p
v= (rl ϕl + 2rr ϕr )2 ,
3
rl ϕl + 2rr ϕr
v= ,
3
−2rl ϕl 2rr ϕr
θ̇ = + .
3ll 3ll
For linear motion only (θ̇ = 0)
−2rl ϕl 2rr ϕr
0= + ,
3l 3l
Fig. 2: Reference system and parameters for kobuki analysis with rr ϕr = rl ϕl ,
ll = 2lr .[1]
letting rr = rl = rw
ϕr = ϕl = ϕw .
then (3) becomes
 1 2

0 " → For a linear speed v = V
J2 −
3 3
#
ϕ
ξ˙I = R(θ)−1  0 0 1 , V = rw ϕw ,
 
−2 2 0
3ll 3ll 0
then the needed wheel speed in radians is
−1
using R(θ) from (*) V
  1 2
 ϕw = ϕl = ϕr = .
cos θ − sin θ 0 0 " → rw
J2 −
3 3
#
ϕ
ξ˙I =  sin θ cos θ 0  0 0 1 , On the other hand, for angular motion only (v = 0)
  
−2 2 0
0 0 1 3ll 3ll 0 rl ϕl 2rr ϕr
0= + ,
computing the first two matrices 3 3
 cos θ 2 cos θ  " #  rl ϕl = −2rr ϕr ,
− sin θ rl 0 →
3 3 −
ϕ
˙ξI =  sin θ 2 sin θ
cos θ   0

rr  ; letting rr = rl = rw
 3 3
−2 2
3ll 3ll 0 0 ϕl = −2ϕr = ϕw .
taking into account that For an angular speed θ̇ = ω
  2rw ϕw
ẋ ω= ,
l
ξ˙I = ẏ  ,
 
then the needed wheel speed in radians is
θ̇
ωl
so ϕw = ,
2rw
cos θ 2 cos θ
ẋ = rl ϕl + rr ϕr , ωl
3 3 ϕr = ,
sin θ 2 sin θ 2rw
ẏ = rl ϕl + rr ϕr , −ωl
3 3 ϕl = .
−2rl ϕl 2rr ϕr rw
θ̇ = + .
3l 3l 3) In order to make the analysis, it is important to define

4
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

recall that

J2 →
−
" # " #
J1 ϕ
R(θ)ξ˙I = →− ,
C1 0

then
√
3 −1
 
2 2 −l
 0 1 −l R(θ)ξ˙I = [J2 →
−
ϕ] ,
 
√
− 3 −1
2 2 −l

calculation of the J1 inverse matrix goes as follows

√
3 −1
 
2 2 −l 1 0 0
0 1 −l 0 1 0  ,
 

√
− 3 −1
2 2 −l 0 0 1
 √ 
3 0 −3l 2 1 0
 0 −1 −l 0 1 0  ,
 
−1 −1 −1
0 0 l 3 3 3
Fig. 3: Reference system and parameters for robotino analysis with −1
√1
 
P in the center.[2] 1 0 0 3
0 √
3
−1 2 −1
 0 1 0  ,
 
3 3 3
−1 −1 −1
0 0 1 3l 3l 3l

it can be defined
π √1

0 −1
√

α1 = , 3 3 
3 →
−
ξ˙I = R(θ)−1  −1 2 −1 
3  J2 ϕ ,

α2 = π , 3 3
−1 −1 −1
−π 3l 3l 3l
α3 = ,
3
β1 = 0 , using R(θ)−1 from (*)
β2 = 0 , −1
  1 
cos θ − sin θ 0 √
3
0 √
3 
β3 = 0 , ˙ξI =    −1 2 −1  →
−
 sin θ cos θ 0  3 3 3  J2 ϕ ,
γ1 = 0 , 0 0 1 −1 −1 −1
3l 3l 3l
γ2 = 0 ,
γ3 = 0 , computing the first two matrices
 cos θ sin θ −2 sin θ −√
cos θ
+ sin3 θ

√ +
3 3 3 3
according to Fig. 3, then ξ˙I =  sin
√ θ − cos θ 2 cos θ −√
sin θ
− cos3 θ 
 
3 3 3 3
−1 −1 −1
√ 3l 3l 3l
3 −1
 
−l
 
2 2 r1 0 0
0→
J1 =  0

1 −l ,
 −
0 r2 ϕ ;

√
− 3 −1
2 2 −l 0 0 r3
C1 = ∅ ,
  taking into account that
r1 0 0
 
J2 =  0

r2 0  ,
 ẋ
0 0 r3 ξ˙I = ẏ  ,
 
→
−ϕ = [ϕ
0
1 , ϕ2 , ϕ3 ] , θ̇

5
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

then R EFERENCES
 
cos θ sin θ 2 sin θ [1] Index of /kobuki/hardware/drawings/pdf, Kobuki File-
ẋ = √ + r1 ϕ1 − r2 ϕ2
3 3 3 Server. [Online]. Available: http : / / files . yujinrobot .

− cos θ sin θ
 com / kobuki / hardware / drawings / pdf/ (visited on
+ √ + r3 ϕ3 , 10/09/2017).
3
  3 [2] Robotino myrio toolkit, Robotino Wiki. [Online].
sin θ cos θ 2 cos θ
ẏ = √ − r1 ϕ1 + r2 ϕ2 Available: http : / / wiki . openrobotino . org / index .
3 3 3 php ? title = Robotino myRIO Toolkit (visited on
 
sin θ cos θ 10/09/2017).
− √ + r3 ϕ3 ,
3 3 [3] DanielStonier (d.stonier@gmail.com), Gazebo simula-
−r1 ϕ1 r2 ϕ2 r3 ϕ3 tion, ROS Wiki. [Online]. Available: http://wiki.ros.
θ̇ = − − .
3l 3l 3l org / kobuki / Tutorials / Gazebo % 20Simulation (visited
on 10/04/2017).
B. Practical Component
1) In order to make the workshop development correctly,
it was used the ROS kobuki model, instead of creating
one from scratch.
2) The process made in order to create the gazebo
plugin was by means of using the Gazebo Simulation
tutorial[3] , it was used the tabletop playground
for the process while running roslaunch
kobuki_keyop keyop.launch --screen
as a publisher, a sniffer, with rostopic echo
/mobile_base/commands/velocity allowed
identifying the keyop plugin modus operandi,
the type of data was retrieved with rostopic
type /mobile_base/commands/velocity,
forging information into the topic provided
confirmation of the message type and proper
operation upon the corresponding injection,
this was done with rostopic pub -1
/mobile_base/commands/velocity
geometry_msgs/Twist --’[2.0, 0.0,
0.0]’ ’[0.0, 0.0, 1.8]’.
The synthesized code is shown in
Code 1. In order to use it, it should
be ran roslaunch kobuki_gazebo
kobuki_playground.launch --screen
and (after running catkin_make and sourcing
with catkin) rosrun beginner_tutorials
myKeyop.py.

V. C ONCLUSIONS AND D ISCUSSION

It is important to take into account the physical limitations
of the robot when doing a model.
Gazebo simulates physics in its models, in this case,
the wheels seem to slide a bit according to the model,
ergo applying the same rotations and translations in reverse
doesn’t provide the same initial state.

6
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

Code 1: myKeyOp.py
#!/usr/bin/env python
import rospy
import curses
from geometry_msgs.msg import Twist

def main(win):
pub = rospy.Publisher(’/mobile_base/commands/velocity’, Twist, queue_size=10)
rospy.init_node(’HansKeyOp’, anonymous=True)
rate = rospy.Rate(10)
global cmd
win.nodelay(True)
key=""
win.clear()
win.scrollok(1)
win.addstr("Kobuki keyoperation:\n\n")
win.addstr("Press P to stop the robot\n")
win.addstr("Press A to spin to the left\n")
win.addstr("Press D to spin to the right\n")
win.addstr("Press W to move forward\n")
win.addstr("Press S to move backwards\n")
win.addstr("Press Q to quit KeyOp\n")
while not rospy.is_shutdown():
try:
key = str(win.getkey())
processKey(win, key)
print cmd
if key == os.linesep:
break
except Exception as e:
# No input
pass
pub.publish(cmd)
rate.sleep()

def processKey(win, key):

global cmd
win.addstr("Detected key:")
win.addstr(key)
win.addstr("\n")
if key == ’p’ or key == ’P’:
stop(win)
elif key == ’q’ or key == ’Q’:
quit(win)
elif key == ’a’ or key == ’A’ or key == ’d’ or key == ’D’:
changingAngular(win, key)
elif key == ’w’ or key == ’W’ or key == ’s’ or key == ’S’:
changingLinear(win, key)

def stop(win):
global cmd
win.addstr("Robot is stopping now\n")
cmd.linear.x = 0.0
cmd.angular.z = 0.0

def quit(win):
win.addstr("Exiting node\n")
rospy.signal_shutdown("Manual shutdown")

def changingAngular(win, key):

global cmd
win.addstr("Changing angular speed\n")
if key == ’a’ or key == ’A’:
cmd.angular.z = cmd.angular.z + angularStep
if cmd.angular.z > 180:
cmd.angular.z = 180.0
elif key == ’d’ or key == ’D’:
cmd.angular.z = cmd.angular.z - angularStep
if cmd.angular.z < -180:

7
 E NGINEERING FACULTY
M ECHANICS AND M ECHATRONICS D EPARTMENT
ROBOTICS AND V ISUAL S ERVOING

Code 1 (continued): myKeyOp.py

cmd.angular.z = -180.0
def changingLinear(win, key):
global cmd
win.addstr("Changing linear speed\n")
if key == ’w’ or key == ’W’:
cmd.linear.x = cmd.linear.x + linearStep
if cmd.linear.x > 1:
cmd.linear.x = 1.0
elif key == ’s’ or key == ’S’:
cmd.linear.x = cmd.linear.x - linearStep
if cmd.linear.x < -1:
cmd.linear.x = -1.0

cmd = Twist()
cmd.linear.x = 0.0
cmd.linear.y = 0.0
cmd.linear.z = 0.0
cmd.angular.x = 0.0
cmd.angular.y = 0.0
cmd.angular.z = 0.0

linearStep = 0.2
angularStep = 36

curses.wrapper(main)

View publication stats