Need help with this program

Typically: "How do I... ", "How can I... " questions
TonyStark
Posts: 19
Joined: 05 Aug 2014, 20:36

Need help with this program

Post by TonyStark » 08 Aug 2014, 10:52

Hello everyone!

First of all three notes. One it's that I'm new on this forum, this is my first post and message. If I do something bad or not correct, please tell me and I would try to correct my faults. The second thing it's that I'm from Spain, and somethings in the code I'm going to expose to you had some notes and variable names in spanish, but I think it will not be difficult to you [because you are experts ;)] to figure out what it's important and what things mean. The last thing it's that I'm not informatic, I know about programming but I'm not an expert.

The reason of this topic it's that a need some help. I'm trying to do a code in lua to run in v-rep (I'm using the PRO EDU 3.1.0 version). The objective of the project it's to program a robot (I'm using the kheperaIII with grippers, because it was in the program and I thing it's good to do what I want) wich has to work as a library clerk. Someone introduces the coordinates where the book he/she wants is, and the robot has to go to that place, avoiding to crash with objects and people in its way, and (in theory) grab the correct book and deliver it to the person who orders it.

The basic I want to do it's the giving and receiving coordinates, and the move arround parts, going to the coordinates. If I can't or run out of time, I can leave aside the grabbing and the delivering parts.

The strategy of movement I'm using for the robot it's the artificial potential fields. This is the code I have:

Code: Select all

simDelegateChildScriptExecution()

-- Check if the required plugin is there:
-- ************************************************
moduleName=0
moduleVersion=0
index=0
kheperaModuleNotFound=true
while moduleName do
	moduleName,moduleVersion=simGetModuleName(index)
	if (moduleName=='K3') then
		kheperaModuleNotFound=false
	end
	index=index+1
end
if (kheperaModuleNotFound) then
	simDisplayDialog('Error','Khepera3 plugin was not found. (v_repExtK3.dll)&&nSimulation will not run properly',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
end
-- ************************************************

-- For this simulation we don't use the ultrasonic proximity sensors (because since they encompass such a large volume, they take quite long computation time):
disableUltrasonicSensors=true
if (disableUltrasonicSensors) then
	for i=1,5,1 do
		simSetExplicitHandling(simGetObjectHandle('K3_ultrasonicSensor'..i),1)
	end
end


simExtK3_setGripperGap(170) -- open the fingers


if (simGetScriptExecutionCount()==0) then

	robotID=simGetObjectHandle('K3_robot')
	objetivoID=simGetObjectHandle('Objetivo')

	objectID={}

	for i=0,15,1 do
		table.insert(objectID,simGetObjectHandle('Estanteria' .. i))
	end

-- CONSTANTES DEL ROBOT (KEPHERA 3)	

	r=0.05021 --0.10042
	b=0.0884 --0.1768
	e=0.6

-- CONSTANTES FUERZA ATRACTIVA DEL OBJETIVO
	Ec=0.3
	Ep=1

-- CONSTANTES FUERZA REPULSIVA A LAS ESTANTERIAS
	rhoo=1
	deltao=0.5
	ko=10
end

simHandleChildScript(sim_handle_all_except_explicit)

-- Put your main code here

	Fx=0
	Fy=0
	Fox1=0
	Foy1=0

	posDR=simGetObjectPosition(objetivoID,robotID)
	distDR=math.sqrt(posDR[1]*posDR[1]+posDR[2]*posDR[2])


-- FUERZA ATRACTIVA DEL OBJETIVO
	if(distDR<=(Ec/Ep)) then

	Fdx=Ep*(posDR[1])
	Fdy=Ep*(posDR[2])

	elseif(distDR>(Ec/Ep)) then

	Fdx=Ec*(posDR[1]/distDR)
	Fdy=Ec*(posDR[2]/distDR)

	end

	
-- FUERZA REPULSIVA A LAS ESTANTERIAS
	eo=ko*((deltao*deltao*rhoo)/(deltao+rhoo))
	

	for i=0,15,1 do
		posOR=simGetObjectPosition(objectID[i+1],robotID)
		distOR=math.sqrt(posOR[1]*posOR[1]+posOR[2]*posOR[2])

		if(distOR>=rhoo) then
			Fox=0
			Foy=0
		elseif(deltao<=distOR) and (distOR<rhoo) then
			Fox=-eo*((1/distOR)-(1/rhoo))*(posOR[1]/(distOR*distOR))
			Foy=-eo*((1/distOR)-(1/rhoo))*(posOR[2]/(distOR*distOR))
		elseif(distOR<=deltao) then
			Fox=-ko*(posOR[1]/distOR)
			Foy=-ko*(posOR[2]/distOR)
		end

		Fox1=Fox1+Fox
		Foy1=Foy1+Foy
	
	end

	
	Fx=Fdx+Fox1
	Fy=Fdy+Foy1

	vl=(Fx-(b/e)*Fy)/r
	vr=(Fx+(b/e)*Fy)/r

	
	simExtK3_setVelocity(-vl,-vr)


if (simGetSimulationState()==sim_simulation_advancing_lastbeforestop) then

	-- Put some restoration code here

end
For now I'm just moving with the mouse the target (Objetivo). The robot only turns around in circles, I don't Know what can be the problem. The cheking part and the deactivation of the ultrasonics at the beggining of the code are from the original default code that appears in the script of the khepereIII. The rest it's mine. I wish to upload a couple of images to show you the scene and the names of the objecs, but I don't knoe how to do it.

If you have any question or doubt just ask me and I will try to answer as best as I can.

Sorry for my english and the long extension. And thaks for for your time and help.

coppelia
Site Admin
Posts: 7388
Joined: 14 Dec 2012, 00:25

Re: Need help with this program

Post by coppelia » 11 Aug 2014, 09:36

Hello,

we cannot directly help you with your algorithm, since that would take quite some time to work ourselves into this. But if there is another problem with the scene, we can have a look at it. The best would be to upload the scene to a file sharing service such as Google drive or similar.

Cheers

TonyStark
Posts: 19
Joined: 05 Aug 2014, 20:36

Re: Need help with this program

Post by TonyStark » 16 Aug 2014, 19:37

If you can't help me with that, at least I hope you can say me how can I do a dummy appears in the coordinates I introduce manually.

Thanks for your time.

Eric
Posts: 186
Joined: 11 Feb 2013, 16:39

Re: Need help with this program

Post by Eric » 16 Aug 2014, 21:54

Hi

SimSetObjectPosition().... By doing the dummy appear you mean create it? My suggestion is to have it already created somewhere in ur scene and just move it during the simulation.

TonyStark
Posts: 19
Joined: 05 Aug 2014, 20:36

Re: Need help with this program

Post by TonyStark » 17 Aug 2014, 09:12

Thanks for your reply Eric, but I don't mean that. I want to change the position of the dummy (which is already created and collocated somewhere in the scene) by introducing the coordinates writing in the code.

For example:

(20,10,0) in order to make the dummy change its previous position to that coordinates. I don't know if I'm explaining well my self.

Thanks for your time.

Eric
Posts: 186
Joined: 11 Feb 2013, 16:39

Re: Need help with this program

Post by Eric » 17 Aug 2014, 19:43

nope, it s not clear at all.... I had to imply a lot of things in order to get a grip of what you ask....
During the simulation if you do

Code: Select all

pos={x,y,z}
simSetObjectPosition(objetivoID,-1,pos)
with x, y and z changing along the simulation, the dummy position (or any object in the scene you get the handle of) will be updated accordingly. Now how do you want to input the target dummy if not with the mouse? a custom user interface? If so you have to make one following this, Then getting the x and y values using simGetUIButtonLabel()

Also,
you are mixing threaded child script codes with not threaded one... be sure you understand this and this. There must be an error displayed in the status bar ... Please be sure that when running a code there are no errors displayed there....

Finally, if you correct your code by this one (assuming that your script is non-threaded)

Code: Select all

    if (simGetScriptExecutionCount()==0) then
      -- Check if the required plugin is there:
    -- ************************************************
    moduleName=0
    moduleVersion=0
    index=0
    kheperaModuleNotFound=true
    while moduleName do
       moduleName,moduleVersion=simGetModuleName(index)
       if (moduleName=='K3') then
          kheperaModuleNotFound=false
       end
       index=index+1
    end
    if (kheperaModuleNotFound) then
       simDisplayDialog('Error','Khepera3 plugin was not found. (v_repExtK3.dll)&&nSimulation will not run properly',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    end
    -- ************************************************

    -- For this simulation we don't use the ultrasonic proximity sensors (because since they encompass such a large volume, they take quite long computation time):
    disableUltrasonicSensors=true
    if (disableUltrasonicSensors) then
       for i=1,5,1 do
          simSetExplicitHandling(simGetObjectHandle('K3_ultrasonicSensor'..i),1)
       end
    end


    simExtK3_setGripperGap(170) -- open the fingers

       robotID=simGetObjectHandle('K3_robot')
       objetivoID=simGetObjectHandle('Objetivo')

       objectID={}

       for i=0,15,1 do
          table.insert(objectID,simGetObjectHandle('Estanteria' .. i))
       end

    -- CONSTANTES DEL ROBOT (KEPHERA 3)   

       r=0.05021 --0.10042
       b=0.0884 --0.1768
       e=0.6

    -- CONSTANTES FUERZA ATRACTIVA DEL OBJETIVO
       Ec=0.3
       Ep=1

    -- CONSTANTES FUERZA REPULSIVA A LAS ESTANTERIAS
       rhoo=1
       deltao=0.5
       ko=10
    end

    simHandleChildScript(sim_handle_all_except_explicit)

    -- Put your main code here

       Fx=0
       Fy=0
       Fox1=0
       Foy1=0

       posDR=simGetObjectPosition(objetivoID,robotID)
       distDR=math.sqrt(posDR[1]*posDR[1]+posDR[2]*posDR[2])


    -- FUERZA ATRACTIVA DEL OBJETIVO
       if(distDR<=(Ec/Ep)) then

       Fdx=Ep*(posDR[1])
       Fdy=Ep*(posDR[2])

       elseif(distDR>(Ec/Ep)) then

       Fdx=Ec*(posDR[1]/distDR)
       Fdy=Ec*(posDR[2]/distDR)

       end

       
    -- FUERZA REPULSIVA A LAS ESTANTERIAS
       eo=ko*((deltao*deltao*rhoo)/(deltao+rhoo))
       

       for i=0,15,1 do
          posOR=simGetObjectPosition(objectID[i+1],robotID)
          distOR=math.sqrt(posOR[1]*posOR[1]+posOR[2]*posOR[2])

          if(distOR>=rhoo) then
             Fox=0
             Foy=0
          elseif(deltao<=distOR) and (distOR<rhoo) then
             Fox=-eo*((1/distOR)-(1/rhoo))*(posOR[1]/(distOR*distOR))
             Foy=-eo*((1/distOR)-(1/rhoo))*(posOR[2]/(distOR*distOR))
          elseif(distOR<=deltao) then
             Fox=-ko*(posOR[1]/distOR)
             Foy=-ko*(posOR[2]/distOR)
          end

          Fox1=Fox1+Fox
          Foy1=Foy1+Foy
       
       end

       
       Fx=Fdx+Fox1
       Fy=Fdy+Foy1

       vl=(Fx-(b/e)*Fy)/r
       vr=(Fx+(b/e)*Fy)/r

       
       simExtK3_setVelocity(-vl,-vr)


    if (simGetSimulationState()==sim_simulation_advancing_lastbeforestop) then
       -- Put some restoration code here
    end
No more error messages will appear in the status bar. Now if the robot is spinning, the error is related to your implementation of the potential fields (in the calculus of vr and vl).

Cheers

Eric

TonyStark
Posts: 19
Joined: 05 Aug 2014, 20:36

Re: Need help with this program

Post by TonyStark » 20 Aug 2014, 20:59

Thanks man, what you said about repositioning the dummy was what I want and it's clear and ok.

But I try what you said of adding that first line, and after correcting a couple of problem that appeared when adding that first line (eliminated this line -> simHandleChildScript(sim_handle_all_except_explicit), and added a end at the last line), no error messeages appear in the status bar, but the robot stills doing the same, running in small circles.

I think the implementation of the potential fields I made it's correct, bacause I used it in another robot (build by my self, following some instructions), the Miniskybot2, and it works ok.

I don't know how I can post images, videos o files (like .ttt), in order to show you what I'm basing on my work.

I really appreciate your help, you are my last hope of achieve to make this work.

Eric
Posts: 186
Joined: 11 Feb 2013, 16:39

Re: Need help with this program

Post by Eric » 20 Aug 2014, 22:51

Hi

I did try the code in a scene having all the elements of your simulation, and the robot rotated the same way you described.... so I can tell you for sure that the problem is in the potential fields code not being compatible with the robot... I thought it might be the motor rotation but even by inverting the left and right velocities in many combination, the robot did not head toward the dummy at all.

Post you scene (and the one with the Miniskybot2) in a dropbox shared link or google drive or whatever service allowing to share files and I ll have an other look.

Cheers

Eric

TonyStark
Posts: 19
Joined: 05 Aug 2014, 20:36

Re: Need help with this program

Post by TonyStark » 21 Aug 2014, 18:41

Ok, here is a link to a shared folder on dropbox with everything I have (done or used to base my work):

https://www.dropbox.com/sh/2wq8js1dq8n0 ... su6VjBjmva

I'll delete it when everything it's solved, so feel free to add, delete, change or whatever you consider necessary (or just want).

The last file I'm working on it's the one called "Version 19" (it's the one that doesn't work correctly).

I uploaded the setup file of the v-rep version I'm using (maybe the problem it's on that, but I don't think so. It's the same version I used with the other robot, Miniskybot2, and all worked well).

Inside the folder "Practices" you have everythimg I used to learn too use this program, and the exercises I made to practice.

I'm sorry about everything it's in spanish. You can use a translator, or ask me to translate the parts you want to know.

I'm using the practice 6 exercise 11 (which it's inside the folder "Practice6", and it's the file named as "Practica6_Ejercicio11").
The button and the two sliders in the right top corner doesn't work on that code, they where from a previous practice and I didn't removed them (and the graphic isn't important neither). The information I'm basing my work on it's in the image 020 and the left side othe 021, inside the folder "Modificated", which is inside the folder "Practices_guide".

If you have any question or doubt, just ask me.

I hope you can find what's wrong, because I turn insane myself trying to fix the code.

Thanks for your time man.

P.D: I copied and pasted the code from the script of the Miniskybot2 to the khepera script, doing the necessary changes. And after doing the modifications to make it fit to the KheperaIII, I tryed to copy the code I was modifying and paste it in the script of the Miniskybot2 in the practice I said before. And after the necessary changes of element names and that things, everything worked properly and everything was ok with the Miniskybot robot. That's the why I said I thing the algorithm it's correct.

P.D.2: Sorry for the extension :S

Eric
Posts: 186
Joined: 11 Feb 2013, 16:39

Re: Need help with this program

Post by Eric » 22 Aug 2014, 19:27

hi

1_ structure... all the code was under a simGetScriptExecutionCount()==0 codition-> executed just once

2_ line 2:
-- This script runs in a thread. You can also use a non-threaded script instead
simDelegateChildScriptExecution()
this cann t be launched from a script that did not launch a thread... remove this line !

3_ you put code of non threaded scripts into threaded scripts
The color of the icon of the script tells you its kind... threaded is cyan, non threaded is grey... So your script is THREADED even if you put the code for non thread. I suggest you to add a non threaded script on any object DefaultFloor for example, then copy/past the code of K3_robot in it, then click on "Scripts" icon on the vertical icon bar on the left (5th icon from the top of the bar ), select Child script (associated with K3_robot), hit delete key, then select Child (script associated with DefaultfFloor). Finally, in Associated Object select K3_robot .

4_ the axis of your robot from the original working code are not the same as the one with the khepera... so the calculation are all mixed up... change the axes of the khepera by selecting K3_robot object, rotating it so that its front is oriented along the x axes of the world frame (the red one) and then right click on the scene-> edit-> reoriente bounding box-> with reference frame of world

5_ now i imagine you have to tune your algorithm parameters if no other problem

Cheers

Eric

Post Reply