Robotino Vrep scene with twist message controller (ROS)

The robotic platform Robotino is a nice omnidirectional Robot manufactured by the company Festo. It was surprising for me to find out that the Robot Simulator VREP doesn’t have a working model of this robot,  even though there exist an opensource model of this robot for the simulator Gazebo.

Just like I did before for a Quadcopter, now I implemented a model of the Robotino for Vrep based on the URDF model from Gazebo. The simulation of the omnidirectional wheels was based on the Kuka Youbot available in Vrep. Additionally I programmed a control script using velocity Twist reference messages from ROS, so just like the quadrotor it is possible to control this robot from ROS using a Joystick (or your custom Node).

 

VrepRobotino

Robotino Model in Vrep

This is the control script that I programmed in the Robotino Model:

[code language=”python” collapse=”true” wraplines=”true”]
— DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)
if (sim_call_type==sim_childscriptcall_initialization) then
——– START OF ROS INITIALIZATION ——————————–
— Check if the required plugin is there (libv_repExtRos.so or libv_repExtRos.dylib):
local moduleName=0
local moduleVersion=0
local index=0
local pluginNotFound=true
while moduleName do
moduleName,moduleVersion=simGetModuleName(index)
if (moduleName==’Ros’) then
pluginNotFound=false
end
index=index+1
end
if (pluginNotFound) then
— Display an error message if the plugin was not found:
simDisplayDialog(‘Error’,’ROS plugin was not found.&&nSimulation will not run properly’,sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
else
— Enable topic subscription:
simExtROS_enableSubscriber("/robotino_cmd_twist",1,simros_strmcmd_set_twist_command,-1,-1,’Command_Twist’)
end

——– END OF ROS INITIALIZATION ——————————–
— Put some initialization code here

— Make sure we have version 2.4.12 or above (the omni-wheels are not supported otherwise)
v=simGetIntegerParameter(sim_intparam_program_version)
if (v<20412) then
simDisplayDialog(‘Warning’,’The YouBot model is only fully supported from V-REP version 2.4.12 and above.&&nThis simulation will not run as expected!’,sim_dlgstyle_ok,false,”,nil,{0.8,0,0,0,0,0})
end

wheelJoints={-1,-1,-1}
wheelJoints[1]=simGetObjectHandle(‘wheel0_joint’)
wheelJoints[2]=simGetObjectHandle(‘wheel1_joint’)
wheelJoints[3]=simGetObjectHandle(‘wheel2_joint’)
robotino_handle = simGetObjectHandle(‘robotino’)

— Make sure you read the section on "Accessing general-type objects programmatically"
— For instance, if you wish to retrieve the handle of a scene object, use following instruction:

— handle=simGetObjectHandle(‘sceneObjectName’)

— Above instruction retrieves the handle of ‘sceneObjectName’ if this script’s name has no ‘#’ in it

— If this script’s name contains a ‘#’ (e.g. ‘someName#4’), then above instruction retrieves the handle of object ‘sceneObjectName#4’
— This mechanism of handle retrieval is very convenient, since you don’t need to adjust any code when a model is duplicated!
— So if the script’s name (or rather the name of the object associated with this script) is:

— ‘someName’, then the handle of ‘sceneObjectName’ is retrieved
— ‘someName#0’, then the handle of ‘sceneObjectName#0’ is retrieved
— ‘someName#1’, then the handle of ‘sceneObjectName#1’ is retrieved
— …

— If you always want to retrieve the same object’s handle, no matter what, specify its full name, including a ‘#’:

— handle=simGetObjectHandle(‘sceneObjectName#’) always retrieves the handle of object ‘sceneObjectName’
— handle=simGetObjectHandle(‘sceneObjectName#0’) always retrieves the handle of object ‘sceneObjectName#0’
— handle=simGetObjectHandle(‘sceneObjectName#1’) always retrieves the handle of object ‘sceneObjectName#1’
— …

— Refer also to simGetCollisionhandle, simGetDistanceHandle, simGetIkGroupHandle, etc.

— Following 2 instructions might also be useful: simGetNameSuffix and simSetNameSuffix
old_velx = 0
old_vely = 0
old_vel_angz = 0
acc_x = 0
acc_y = 0
acc_omega = 0

end

if (sim_call_type==sim_childscriptcall_actuation) then

— Put your main ACTUATION code here
— From CMDVEL
— Signals from ROS
local packedData=simGetStringSignal(‘Command_Twist’)
if (packedData) then
local ref_twist=simUnpackFloats(packedData)
— Process the twist data
— X and Y velocities (related to the heading of the robotino)
ref_velx = ref_twist[1]*0.18 –reference values between 0 and 1, Max Vel = 0.18 m/s
ref_vely = ref_twist[2]*0.18

— Angular velocity in Z (this changes the heading of the robotino)
ref_angz = ref_twist[6]
end

— Robotino Model (Open Loop control)
omega = ref_angz
vx = ref_velx
vy = ref_vely
— Sets the distance from robot center to wheel center.
rb = 0.13
— Sets the radius of the wheels.
rw = 0.040
— Sets the gear.
gear = 0.11 –revisar este numero
— Projection matrix
v0 = { -0.5 * math.sqrt(3), 0.5 }
v1 = { 0.0 , -1.0 }
v2 = { 0.5 * math.sqrt( 3.0 ), 0.5 }
— Scale omega with the radius of the robot
vOmegaScaled = rb * omega
–Convert from m/s to RPM
k = 60.0 * gear / ( 2.0 * math.pi * rw )
–Compute the desired velocity
m1 = (v0[1] * vx + v0[2] * vy + vOmegaScaled ) * k
m2 = (v1[1] * vx + v1[2] * vy + vOmegaScaled ) * k
m3 = (v2[1] * vx + v2[2] * vy + vOmegaScaled ) * k

— Transformation to obtain velocities related to the quadrotor heading
————————————————————————–
— Orientation of the quadcopter related to the World
euler_robotino = simGetObjectOrientation(robotino_handle,-1)
— We create a Transformation Matrix from this orientation
pos_robotino=simGetObjectPosition(robotino_handle,-1)
m_heading = simBuildMatrix(pos_robotino,euler_robotino)

— We create a Transformation Matrix for converting the global referenced velocities
— to a reference frame based on the heading of the quadrotor
global_pos = {0,0,0}
m_global = simBuildMatrix(global_pos,euler_robotino)
m = simGetInvertedMatrix(m_global)

vel_global, vel_angz = simGetObjectVelocity(robotino_handle)

vel_local=simMultiplyVector(m,vel_global)

— Velx Control
error_x = ref_velx – vel_local[1]
velxControl = error_x*0.1 +acc_x*0.01 + (vel_local[1]-old_velx)*0.001
old_velx = vel_local[1]
acc_x = error_x+acc_x

— Vely Control
error_y = ref_vely – vel_local[2]
velyControl = error_y*0.1 +acc_y*0.01 + (vel_local[2]-old_vely)*0.001
old_vely = vel_local[2]
acc_y = error_y+acc_y

— Omega Velocity Control
error_omega = ref_angz – vel_angz[3]
rotControl = error_omega*0.05 +acc_omega*0.01 — (vel_local[3]-old_vel_angz)*1.1
old_vel_angz = vel_angz[3]
acc_omega = error_omega+acc_omega

— Robotino Model (Closed Loop control)
— Sets the distance from robot center to wheel center.
rb = 0.13
— Sets the radius of the wheels.
rw = 0.040
— Sets the gear.
gear = 1.0 –revisar este numero
— Projection matrix
v0 = { -0.5 * math.sqrt(3), 0.5 }
v1 = { 0.0 , -1.0 }
v2 = { 0.5 * math.sqrt( 3.0 ), 0.5 }
— Scale omega with the radius of the robot
vOmegaScaled = rb * rotControl
–Convert from m/s to RPM
k = 60.0 * gear / ( 2.0 * math.pi * rw )
–Compute the desired velocity
m1 = (v0[1] * velxControl + v0[2] * velyControl + vOmegaScaled ) * k
m2 = (v1[1] * velxControl + v1[2] * velyControl + vOmegaScaled ) * k
m3 = (v2[1] * velxControl + v2[2] * velyControl + vOmegaScaled ) * k
–m1 = 1000
–m2 = 000
–m3 = -1000
–vel_global, vel_ang = simGetObjectVelocity(robotino_handle)
–print(vel_global[1],vel_global[2],vel_global[3],vel_ang[3])
print(‘Global’,vel_global[1],vel_global[2],vel_global[3],vel_angz[3])
print(‘Ref’,ref_velx,ref_vely,ref_angz)
print(‘Local’,vel_local[1],vel_local[2],vel_angz[3])
print(‘Motors’,m1,m2,m3)

— For example:

— local position=simGetObjectPosition(handle,-1)
— position[1]=position[1]+0.001
— simSetObjectPosition(handle,-1,position)
simSetJointTargetVelocity(wheelJoints[1],m1)
simSetJointTargetVelocity(wheelJoints[2],m2)
simSetJointTargetVelocity(wheelJoints[3],m3)

end

if (sim_call_type==sim_childscriptcall_sensing) then

— Put your main SENSING code here

end

if (sim_call_type==sim_childscriptcall_cleanup) then

— Put some restoration code here

end
[/code]

Here you can download the model. Any feedback will be greatly appreciated.

Download Robotino model

2 comments on “Robotino Vrep scene with twist message controller (ROS)
  1. Abdeslam says:

    thank you for sharing the robotino model
    it is really helpfull

Leave a Reply

Your email address will not be published. Required fields are marked *

*