Page 1 of 1

Unexpected fail in if loop

Posted: 14 Nov 2014, 21:38
by mechame
Hi everyone.

I have an unexpected fail at a if loop. I'm trying to make that the khpera3 robot takes an object, and when the grippers sensors detected an object really close it means that object is picked, and then it has to move to a determined coordinates (they are represented by a dummy called "objetivoa", and the coordinates are pos3={1.75, -2, 0), and after reaching that point it has to move to another point (pos3={1.75, -2.2, 0}, and after that second point it has to move to a third point (pos3={2.1, -2.2, 0}, and then drop the object picked before.

The problem is that I made that the dummy changes its position when its close to the point (a threshold distance) and when some conditions are coincident, but when the robot reaches the threshold distance of the first point the dummy changes to the second one and instantly to the third one and then returns to the first one, so the robot only moves to the first one and continues with the rest of the program but without dropping the object because it has to be done after reaching the third point.

THis is the part of the code that doesn't works correctly:

Code: Select all

if o~=0 and c==0 and (simExtK3_getGripperProxSensor(0)<0.015)or(simExtK3_getGripperProxSensor(1)<0.015) then
	pos3={1.75, -2, poszr}
	simSetObjectPosition(objetivoa,-1,pos3)
	simExtK3_setArmPosition(900)
	g=1
	c=1
end

if distAP<0.1 and c~=0 and p==0 then
pos3={1.75, -2.2, poszr}
simSetObjectPosition(objetivoa,-1,pos3)
g=0
p=1
end

if distAP<0.08 and p~=0 and h==0 then
pos3={2.1, -2.2, poszr}
simSetObjectPosition(objetivoa,-1,pos3)
g=0
h=1
end
The dummy changes the position correctly, but it does it instantly from the first one to the third one throught the second one. I took a look at the console window to chek the variables, and "p" and "h" responds correctly, but "c" must be the problem because it's the condition to set the first position. The "o" is a previous fullfilled condition and it isn't the problem. An object it's picked and detected by the sensors on the grippers.

When I checked the console window the "c" variable changes to 1 and reminds on that value, so it shouldn't be able to do that loop again, but I supposed that somehow that first loop is executed a second time and makes that the other loops are carried out instantly to return to the first one.

I don't understand how is posible that the first point appears the second time and the other two only appear for an instant. And I don't understand how can the first loop can be executed a second time if the "c" variable is set to 1 after reaching that first point.

The rest of the code works correctly. In fact, if I add another part on the code to return to the starting point after doing that part of the changing points and dropping the object the robots returns to the start position just subsequently of reaching the first point, ignoring the part of the other two points and the dropping part.

I can send the hole code if it is necessary. Or try to explain something in another way if it's not clear.

Thanks for your help.

Re: Unexpected fail in if loop

Posted: 15 Nov 2014, 07:37
by Eric
Hi

Not sure that s the problem but try to substitute your code by the following line

Code: Select all

if o~=0 and c==0 and ((simExtK3_getGripperProxSensor(0)<0.015)or(simExtK3_getGripperProxSensor(1)<0.015)) then
false and false and false or true returns true, while
false and false and (false or true) returns false

cheers

Eric

Re: Unexpected fail in if loop

Posted: 15 Nov 2014, 13:46
by mechame
Thanks Eric!

That was the problem.

I checked that condition previously, but looks like that when I tried to do other changes for other reasons sometime I removed that brackets and I didn't checked it again because I thought that it was still correct.

Thanks again and sorry for bothering because of a stupid mistake like that.

Re: Unexpected fail in if loop

Posted: 15 Nov 2014, 16:02
by Eric
No worries.... i ve been having a similar problem recently

Cheers

Re: Unexpected fail in if loop

Posted: 15 Nov 2014, 17:01
by mechame
Now, if you don't mind, I want to make an specific question (I think is better to use this topic again instead of opning a new one).

Is possible to make the robot (khepera3) know what's the position of its own arm? And if its possible, how can it be done? Using what commands?

I know how to set the arm position (using the implemented command "simExtK3_setArmPosition(position_300_to_900)"), but not how to use its position as a condition.

Thanks again.

Re: Unexpected fail in if loop

Posted: 16 Nov 2014, 03:26
by Eric
There are no command implemented in the plugin to get the position of the arm apparently.. However you still can get the position of the joint named "K3_gripper_armJoint1" and compute the position of the arm.

Code: Select all


--in the init phase
-- get the joint handle
gripperJointHandle=simGetObjectHandle("K3_gripper_armJoint1")


-- get some points for conversion armPos-JointAngle (x:armPos from 900 to 300 and y: joint angle from 86.5 deg to 285 deg)
x0=900
y0= 86.5
x1=300
y1= 285
a=(y0-y1)/(x0-x1)
b=y1-(x1*a)

...

-- in the main loop:
gripperAngle=((simGetJointPosition(gripperJointHandle)*180/math.pi)+90)%360
armPos= (gripperAngle-b)/a

if armPos > a_certain_value then
      -- do something
end
There is also an other way to proceed (less elegant?), that would be to read the value of the label of button 120, that the plugin writes in the K3_stateVisualization user interface.

Code: Select all

-- in initialization phase
ui=simGetUIHandle('K3_stateVisualization')


--in the main loop
armPos=tonumber(simGetUIButtonLabel(ui,120))

if armPos > a_certain_value then
      -- do something
end
cheers

Eric

Re: Unexpected fail in if loop

Posted: 16 Nov 2014, 23:12
by mechame
Wow thanks!

I try the first option (the elegant one) and I could made it work. Now everything works correctly.

Thanks again man. You and the coppelia people and this forum where really helpfull.

Thanks for your time.