Simulating a differential wheeled robot in V-REP

After simulating a robotic arm, we will now discuss how to set up a simulation scene to control a mobile robot. In this case, we will use one of the simulation models already implemented in V-REP. To import a model from the V-REP database, select the desired model class and object from the Model Browser panel and drag it into the scene.

For our simulation, we will simulate the Pioneer 3dx, one of the most popular differential wheeled mobile robots used as a research platform:

By default, the Pioneer robot is equipped with 16 sonar sensors, both forward and rear facing. In the next section, we will discuss how to equip the robot with other sensors.

To actuate the robot using ROS, we should add a script that receives the desired linear and angular velocity and convert it in wheel velocities. We can use the same script to enable sensor data streaming via a ROS topic.

Here is the complete code of the script associated to Pioneer_p3dx object in the Scene Hierarchy panel. Part of this code is released with the V-REP simulator and is already available when the Pioneer_p3dx is imported in the simulation scene:

if (sim_call_type==sim_childscriptcall_initialization) then  
    usensors={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1} 
    sonarpublisher={-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1,-1} 
    for i=1,16,1 do 
       usensors[i]=simGetObjectHandle("Pioneer_p3dx_ultrasonicSensor"..i) 
sonarpublisher[i] = simExtROS_enablePublisher('/sonar'..i , 0, simros_strmcmd_read_proximity_sensor, usensors[i], -1, '') 
    end 
 
    motorLeft=simGetObjectHandle("Pioneer_p3dx_leftMotor") 
    motorRight=simGetObjectHandle("Pioneer_p3dx_rightMotor") 
 
--Input 
simExtROS_enableSubscriber('/linear_vel', 0, simros_strmcmd_set_float_signal, -1, -1, 'vl')  
simExtROS_enableSubscriber('/angular_vel', 0, simros_strmcmd_set_float_signal, -1, -1, 'va') 
     
--output 
robotHandler=simGetObjectHandle('Pioneer_p3dx') -- body position odomPublisher=simExtROS_enablePublisher('/odometry',1,simros_strmcmd_get_odom_data,robotHandler,-1,'') 
     
axes_length = 0.331; 
   wheel_radius = 0.0970; 
end  
 
if (sim_call_type==sim_childscriptcall_cleanup) then  
  
end  
 
if (sim_call_type==sim_childscriptcall_actuation) then  
  
    local v_l = simGetFloatSignal( 'vl' ) 
    local v_a = simGetFloatSignal( 'va' ) 
 
    if not v_l then 
        v_l = 0.0 
    end 
 
    if not v_a then 
        v_a = 0.0 
    end 
     
    local v_left = 0.0 
    local v_right = 0.0 
    v_left = ( 1/wheel_radius)*(v_l-axes_length/2*v_a) 
    v_right = ( 1/wheel_radius)*(v_l+axes_length/2*v_a) 
         
    simSetJointTargetVelocity(motorLeft,v_left) 
    simSetJointTargetVelocity(motorRight, v_right) 
end  

Here is the explanation of the code:

for i=1,16,1 do 
usensors[i]=simGetObjectHandle("Pioneer_p3dx_ultrasonicSensor"..i)sonarpublisher[i] = simExtROS_enablePublisher('/sonar'..i , 0, simros_strmcmd_read_proximity_sensor, usensors[i], -1, '') 
  end 
  motorLeft=simGetObjectHandle("Pioneer_p3dx_leftMotor") 
  motorRight=simGetObjectHandle("Pioneer_p3dx_rightMotor") 

Here, we stream the sonar data and initialize the handlers of the wheels to be controlled:

simExtROS_enableSubscriber('/linear_vel', 0, simros_strmcmd_set_float_signal, -1, -1, 'vl')  
simExtROS_enableSubscriber('/angular_vel', 0, simros_strmcmd_set_float_signal, -1, -1, 'va') 
 
robotHandler=simGetObjectHandle('Pioneer_p3dx') -- body position odomPublisher=simExtROS_enablePublisher('/odometry',1,simros_strmcmd_get_odom_data,robotHandler,-1,'') 

This allows the robot to receive the desired cartesian velocities from the ROS topic and stream its odometry. The simros_strmcmd_set_float_signal command is used to read a float value from the /linear_vel and /angular_vel topics, while the simros_strmcmd_get_odom_data command enables the streaming of the odometry data of the robot via a nav_msgs::Odom message.

In the actuation part of the script, we can calculate the velocity of the wheels:

    local v_l = simGetFloatSignal( 'vl' ) 
    local v_a = simGetFloatSignal( 'va' ) 

This block of code retrieves the value of signals of the desired linear and angular velocities from the input topic:

    if not v_l then 
        v_l = 0.0 
    end 
    if not v_a then 
        v_a = 0.0 
    end 

Since, by default, signals are created with null values, it is recommended to check that they have been initialized before we use them:

    v_left = ( 1/wheel_radius)*(v_l-axes_length/2*v_a) 
    v_right = ( 1/wheel_radius)*(v_l+axes_length/2*v_a)         
    simSetJointTargetVelocity(motorLeft,v_left) 
    simSetJointTargetVelocity(motorRight, v_right) 

Finally, we can calculate and set the wheel velocities needed to actuate the robot with the desired control input. In our simulation, a wheel is represented by a simple joint. To move it, we can use simSetJointTargetVelocity, which sets the desired target velocity to the joint.

After running the simulation, the user should be able to read the sonar values and the position of the robot, calculated via the robot odometry, and set the linear and angular velocity:

$ rostopic pub /linear_vel std_msgs/Float32 "data: 0.2"

This will apply a linear velocity of 0.2 m/s to the robot.