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.

 

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:

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

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.

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

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