Vrep quadrotor control script using Twist messages from ROS

Vrep has a very nice quadrotor model, however for my research I needed a controller based on velocity Twist messages obtained from ROS.

 

VrepQuadcopter

Quadcopter simulated in Vrep

I programmed a new LUA control script  for the quadcopter based on the original script. With this new script it is possible to control the quadrotor using Twist messages from ROS. In my particular implementation I connected the generic ROS Joystick node to a simple Node that I programmed (quad_joy_control) which converts the information from the Joystick to a velocity twist message and publishes it in the topic /quad_cmd_twist. I launch the necessary nodes using this launch file:

[code language=”xml”]
<launch>
<node pkg="coop_uv_sim" name="quad_joy_control" type="quad_joy_control"/>

<arg name="joy_config" default="logitech_joy" />
<arg name="joy_dev" default="/dev/input/js0" />

<node pkg="joy" type="joy_node" name="joy_node">
<param name="dev" value="$(arg joy_dev)" />
<param name="deadzone" value="0.1" />
<param name="autorepeat_rate" value="20" />
</node>

</launch>
[/code]

rqtgraphVrepQuad

ROS Nodes Graph

 

To use your own code with this Vrep Quadcopter Model you have to create a Node in ROS that publish a Twist Message in the topic /quad_cmd_twist. If this message is not present the quadcopter won’t move.

This is the LUA script that is running inside the model of the quadcopter in Vrep, just replace the default Quadrotor script in Vrep with this one (Double click on it to select and Copy). You can change the name of the subscribed topic to one that better suit your needs.

[code language=”python” collapse=”true” wraplines=”true”]
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("/quad_cmd_twist",1,simros_strmcmd_set_twist_command,-1,-1,’Command_Twist’)
end

——– END OF ROS INITIALIZATION ——————————–

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

— Detatch the manipulation sphere:
target_obj = simGetObjectHandle(‘Quadricopter_target’)
simSetObjectParent(target_obj,-1,true)

— I modified the original quuadrotor control to receive Twist commands from ROS, feel free to play with the parameters.

quad_handle = simGetObjectHandle(‘Quadricopter_base’)

particlesAreVisible=simGetScriptSimulationParameter(sim_handle_self,’particlesAreVisible’)
simSetScriptSimulationParameter(sim_handle_tree,’particlesAreVisible’,tostring(particlesAreVisible))
simulateParticles=simGetScriptSimulationParameter(sim_handle_self,’simulateParticles’)
simSetScriptSimulationParameter(sim_handle_tree,’simulateParticles’,tostring(simulateParticles))

propellerScripts={-1,-1,-1,-1}
for i=1,4,1 do
propellerScripts[i]=simGetScriptHandle(‘Quadricopter_propeller_respondable’..i)
end

particlesTargetVelocities={0,0,0,0}

old_velx = 0
old_vely = 0
ref_velx = 0
ref_vely = 0
old_vel_angz = 0

pParam=2
iParam=0.001
dParam=0.001
vParam=-2

cumul=0
lastE=0
pAlphaE=0
pBetaE=0
psp2=0
psp1=0

prevEuler=0

fakeShadow=simGetScriptSimulationParameter(sim_handle_self,’fakeShadow’)
if (fakeShadow) then
shadowCont=simAddDrawingObject(sim_drawing_discpoints+sim_drawing_cyclic+sim_drawing_25percenttransparency+sim_drawing_50percenttransparency+sim_drawing_itemsizes,0.2,0,-1,1)
end

— Prepare 2 floating views with the camera views:
floorCam=simGetObjectHandle(‘Quadricopter_floorCamera’)
frontCam=simGetObjectHandle(‘Quadricopter_frontCamera’)
floorView=simFloatingViewAdd(0.9,0.9,0.2,0.2,0)
frontView=simFloatingViewAdd(0.7,0.9,0.2,0.2,0)
simAdjustView(floorView,floorCam,64)
simAdjustView(frontView,frontCam,64)
end

if (sim_call_type==sim_childscriptcall_cleanup) then
end

if (sim_call_type==sim_childscriptcall_sensing) then
end

if (sim_call_type==sim_childscriptcall_actuation) then
end

if (sim_call_type==sim_childscriptcall_cleanup) then
simRemoveDrawingObject(shadowCont)
simFloatingViewRemove(floorView)
simFloatingViewRemove(frontView)
end

if (sim_call_type==sim_childscriptcall_actuation) then
s=simGetObjectSizeFactor(quad_handle)
pos_quad=simGetObjectPosition(quad_handle,-1)
if (fakeShadow) then
itemData={pos_quad[1],pos_quad[2],0.002,0,0,1,0.2*s}
simAddDrawingObjectItem(shadowCont,itemData)
end

— Quadcopter Global Velocity
vel_quad_global, vel_ang = simGetObjectVelocity(quad_handle)

— Transformation to obtain velocities related to the quadrotor heading
————————————————————————–
— Orientation of the quadcopter related to the World
eulerQuad = simGetObjectOrientation(quad_handle,-1)

— We create a Transformation Matrix centered at the quadrotor and rotated only in Z
eulerHeading = eulerQuad
eulerHeading[1] = 0
eulerHeading[2] = 0
m_heading = simBuildMatrix(pos_quad,eulerHeading)

— 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,eulerHeading)
m = simGetInvertedMatrix(m_global)
vel_quad_local=simMultiplyVector(m,vel_quad_global)

— We calculate the attitude of the Quadrotor in the frame based on the heading
m_quad=simGetObjectMatrix(quad_handle,-1)
m_heading_inv = simGetInvertedMatrix(m_heading)
m = simMultiplyMatrices(m_heading_inv,m_quad)
euler = simGetEulerAnglesFromMatrix(m)

— 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 quadrotor)
ref_velx = ref_twist[1]
ref_vely = ref_twist[2]

— NOTE: twist.linear.z component its not de velocity in Z axis but a height reference.
— This is just for convenience. Feel free to modify it.
height = 2 + ref_twist[3]

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

— Vertical control (from previous controler)
e=(height-pos_quad[3])
cumul=cumul+e
pv=pParam*e
thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel_quad_local[3]*vParam
lastE=e

— Horizontal control
vel_errorx = ref_velx – vel_quad_local[1]
vel_errory = ref_vely – vel_quad_local[2]

— Error in Alpha angle (simple PD controler)
alphaE = euler[1]
alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE)

— Error in Beta angle (simple PD controler)
betaE = -euler[2]
betaCorr=-0.25*betaE-2.1*(betaE-pBetaE)
pAlphaE=alphaE
pBetaE=betaE

— We add the Velocity reference controller to the previous attitude control
— also a simple PD controler
alphaCorr=alphaCorr+vel_errory*0.05 + (vel_quad_local[2]-old_vely)*0.05
betaCorr=betaCorr-vel_errorx*0.05 – (vel_quad_local[1]-old_velx)*0.05
old_velx = vel_quad_local[1]
old_vely = vel_quad_local[2]

— Rotational control
vel_angz_error = ref_angz – vel_ang[3]
rotCorr= -vel_angz_error*1.0 – (vel_ang[3]-old_vel_angz)*1.0
old_vel_angz = vel_ang[3]

— Decide the motor velocities:
particlesTargetVelocities[1]=thrust*(1-alphaCorr+betaCorr+rotCorr)
particlesTargetVelocities[2]=thrust*(1-alphaCorr-betaCorr-rotCorr)
particlesTargetVelocities[3]=thrust*(1+alphaCorr-betaCorr+rotCorr)
particlesTargetVelocities[4]=thrust*(1+alphaCorr+betaCorr-rotCorr)

— Move Target Object (Green Sphere in the simulation)
— Just for visualization of the Reference Height assigned to the Quadrotor.
pos_target=simGetObjectPosition(target_obj,-1)
pos_target[1] = pos_quad[1]
pos_target[2] = pos_quad[2]
pos_target[3] = height
simSetObjectPosition(target_obj,-1,pos_target)

— Send the desired motor velocities to the 4 rotors:
for i=1,4,1 do
simSetScriptSimulationParameter(propellerScripts[i],’particleVelocity’,particlesTargetVelocities[i])
end
end
[/code]

If you prefer you can download the model of the quadcopter with the integrated script here: Download Quadcopter Vrep model with Twist control

9 comments on “Vrep quadrotor control script using Twist messages from ROS
  1. Carlos Pereira says:

    Hello Raúl! Congratulations for you research! I need you help! Could you explain how can I control the velocity in Z Axis? What modifications are necessary in this code?

  2. Carlos Pereira says:

    Hello Raúl! Could you share the documents where you based you transformation of the problem? Have you same papers or thesis about it?

  3. Carlos Padilla says:

    hi , can you post the ros side code? im traying to do the same but with a keyboard.

    ros (python) + vrep quadrotor control….. any suggestion?

  4. Risdiyan says:

    I confused with the nodes. Hehe

  5. Risdiyan says:

    Its not working for me, 😅😅😅
    I confused with the nodes.

  6. Great implementation. But am trying to implement an autonomous behavior for this, any idea how i can do this? Am just getting started with ros and V–rep. Thanks

    • For autonomous behaviour I obtain the simulated sensor information from V-REP (published in topics using v-rep ros tools), then I process the information in a ROS node which later sends back to V-REP the twist command to move the quadcopter.

1 Pings/Trackbacks for "Vrep quadrotor control script using Twist messages from ROS"
  1. […] 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 […]

Leave a Reply

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

*