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).

Robotino Model in Vrep

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

-- 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

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

Download Robotino model