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

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

-- 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
Posts: 7820
Joined: 14 Dec 2012, 00:25

### Re: Need help with this program

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

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.

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

### Re: Need help with this program

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

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.

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

### Re: Need help with this program

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

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

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

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

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

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.

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

hi

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

2_ line 2: