How to create the Romeo model - lagadic/v_repExtNaoqi GitHub Wiki
ROMEO model in V-REP
Import URDF file
To create the model of Romeo in V-REP we need the URDF model and the meshes. You can find everything in the package called romeo_robot in the folder romeo_description.
We import the URDF model, using the plugin in V-Rep:
If you have some meshes which are not recognized, you might need to change the paths in the romeo.urdf :
<mesh filename="package://romeo_description/meshes/Torso.dae" scale="1 1 1"/>
to
<mesh filename="/path/of/the/right/directory/romeo_description/meshes/Torso.dae" scale="1 1 1"/>
Once your model has been correctly imported in V-Rep, you have to check if all the inertia and mass are also correct. It can be quickly see once you start the simulation. If some parts are doing weird stuff, the problem might be the inertia parameters which are wrong. In our case, we have the body, the right and left thigh which are going crazy.
And if we take a look into the dynamic properties of these parts, we have the right masses, but not the right Inertia parameters. They are way too low.
So we change them with the inertia written in the urdf file. We need to change all the inertia matrices, until we have this for the Body, Left Thigh and Right Thigh :
Inertia/mass
Body L_Thigh R_Thigh
xx 0.0110338974 0.0082935538 0.0082935538
xy 1.42043701E-5 -5.132414E-6 -0.0001141706
xz -0.0112472792 0.0053089816 0.0053089816
yy 0.0101897966 0.0083871652 0.0083871652
yz -6.631360E-6 0.0001511944 -0.0005922339
zz 0.0041430603 0.0026024207 0.0026024207
Now, our simulation is no more falling apart.
In order to be able to execute the plugin that Marco Cognetti wrote, we have now to enable the control loop of all the joints, and to choose the right PID. We choose a PID of 0.1 by default, but for the :
- NeckPith
- R/LHipRoll
- R/LHipPitch
- R/LAnkleRoll
- all the fingers,
We chose a Proportional parameter of 1.
And for the :
- R/LAnklePitch and
- HeadPitch,
We chose a Proportional parameter of 2.
While we are in the joint control, take care of the Maximum torque of the joints of the two arms, which should be around 30 N.m . If they are too low or too high, Romeo will have the arms that are trembling. You can let the maximum torque of the fingers at 20.
Now, we just need to add the following child script to the parent respondable part.
------------------------------------------------------------------------------
-- Following few lines automatically added by V-REP to guarantee compatibility
-- with V-REP 3.1.3 and later:
if (sim_call_type==sim_childscriptcall_initialization) then
simSetScriptAttribute(sim_handle_self,sim_childscriptattribute_automaticcascadingcalls,false)
end
if (sim_call_type==sim_childscriptcall_cleanup) then
end
if (sim_call_type==sim_childscriptcall_sensing) then
simHandleChildScripts(sim_call_type)
end
if (sim_call_type==sim_childscriptcall_actuation) then
if not firstTimeHere93846738 then
firstTimeHere93846738=0
end
simSetScriptAttribute(sim_handle_self,sim_scriptattribute_executioncount,firstTimeHere93846738)
firstTimeHere93846738=firstTimeHere93846738+1
------------------------------------------------------------------------------
--simDelegateChildScriptExecution()
if (simGetScriptExecutionCount()==0) then
-- Check what OS we are using:
platf=simGetInt32Parameter(sim_intparam_platform)
if (platf==0) then
pluginFile='v_repExtRomeo.dll'
end
if (platf==1) then
pluginFile='libv_repExtRomeo.dylib'
end
if (platf==2) then
pluginFile='libv_repExtRomeo.so'
end
-- Check if the required plugin is there:
moduleName=0
moduleVersion=0
index=0
NaoModuleNotFound=true
while moduleName do
moduleName,moduleVersion=simGetModuleName(index)
if (moduleName=='Romeo') then
NaoModuleNotFound=false
end
index=index+1
end
if (NaoModuleNotFound) then
-- Plugin was not found
simDisplayDialog('Error',"Nao plugin was not found. ('"..pluginFile.."')&&nSimulation will not run properly",sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
else
simExtRomeo_init_threads()
-- simExtNao_init()
cond=0
end
end
if simGetSimulationTime()>=3.0 then
if cond==0 then
-- Activate the inizialization of the robot
simExtRomeo_init()
cond=1
end
end
--******************************
-- Controller is onboard
--******************************
-- Following function executes the other scripts attached to that hierarchy tree (i.e. also the control script of this robot):
simHandleChildScripts(sim_call_type)
-- At each simulation pass, retrieve the robot state...
robotState={0,0,0,0,0,0,0,0}
--for i=1,8,1 do
-- robotState[i]=simGetJointPosition(robotJointHandles[i])
--end
--simExtNao_receiveState()
if simGetSimulationTime()>=6.0 then
simExtRomeo_sendState(robotState) -- the plugin will print out the robot state to the console
end
--******************************
-- Controller is external
--******************************
-- In a similar way, you can retrieve the state from the plugin (e.g. if your robot is controlled via NaoQi).
-- In that case, we would have to disable inverse kinematics for the legs (see further up), and disable the
-- onboard controller (by simply not calling "simHandleChildScripts(sim_call_type)")
--
-- At each simulation pass, get the new joint positions from the plugin:
--robotState=simExtNao_receiveState()
--if (robotState) then
-- simSetJointPosition(robotJointHandles[i],robotState[i])
--end
------------------------------------------------------------------------------
-- Following few lines automatically added by V-REP to guarantee compatibility
-- with V-REP 3.1.3 and later:
end
------------------------------------------------------------------------------
Our only problem is that right now, Romeo is flying instead of standing on the floor. To resolve this problem, we just need to delete the base_link_visual bloc, which is not useful to us.
And now, Romeo is working.