Speed and stop on path

Typically: "How do I... ", "How can I... " questions
Post Reply
Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Speed and stop on path

Post by Leizah »

Hi

I'm working on a scene with a walking Bill that follows a path between three different customizable Tables and a UR10 robot that performs a pick-and-place operation.

I would like to obtain the following results:
  • Bill has to stop and wait during the path (i.e. Go to the first control point, wait until the robot has performed its task and then move to the next control point)
  • The robot has to change its speed when a specific action performed by the walking Bill is triggered (i.e. When Bill is moving from Table1 to Table2/colliding with one of the tables, reduce your speed to 50%)
There's a way to achieve these results using MATLAB or Lua script?

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

Re: Speed and stop on path

Post by coppelia »

Hello,

first of all, make sure to use CoppeliaSim V4.2.0.
Then, have a look at the child script attached to the model. Around line 160, you could add something like:

Code: Select all

if currentPosOnPath>pathL*0.5 and walkingDir>0 and not alreadyPausedMiddleWay then
    alreadyPausedMiddleWay=true
    pauseUntil=simTime+3
end
which will perform a pause of 3 seconds when at the middle of the path. Just once. You can modulate velocity with following:

Code: Select all

nominalVelocity=newVelocity
vel=nominalVelocity*0.8/0.56 -- in order to keep correct movement interpolation 
Of course, best would be to rewrite the script according to what you need exactly, maybe additionally using a threaded script, which would make things much easier. Above is just a quick and dirty way to archive what you want, quickly.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

Leizah wrote: 17 Jul 2021, 09:16 The robot has to change its speed when a specific action performed by the walking Bill is triggered (i.e. When Bill is moving from Table1 to Table2/colliding with one of the tables, reduce your speed to 50%)
UPDATE

To achieve this goal, I've added two proximity sensors at the edge of the tables and wrote the following code in the robot child script (Starting from the existing code linked to the UR10 model):

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
end

function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
end

-- This is a threaded script, and is just an example!
SafetySensorHandle=sim.getObjectHandle('Safety_Zone_Sensor')
result,distance,detectedPoint,detectedObjectHandle,detectedSurfaceNormalVector=sim.handleProximitySensor(sim.handle_all)


function movCallback(config,vel,accel,handles)
    for i=1,#handles,1 do
        if sim.getJointMode(handles[i])==sim.jointmode_force and sim.isDynamicallyEnabled(handles[i]) then
            sim.setJointTargetPosition(handles[i],config[i])
        else    
            sim.setJointPosition(handles[i],config[i])
        end
    end
end

function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf)
    local currentConf={}
    for i=1,#handles,1 do
        currentConf[i]=sim.getJointPosition(handles[i])
    end
    sim.moveToConfig(-1,currentConf,nil,nil,maxVel,maxAccel,maxJerk,targetConf,nil,movCallback,handles)
end

function coroutineMain()
    local jointHandles={-1,-1,-1,-1,-1,-1}
    for i=1,6,1 do
        jointHandles[i]=sim.getObjectHandle('UR10_joint'..i)
    end
    
    if result==0 then
        local vel=120
        local accel=40
        local jerk=80
    else if result > 0 then do
        local vel=vel/2
        local accel=accel/2
        local jerk=jerk/2
    end
    end
    
    local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel_math.pi/180,vel*math.pi/180,vel*math.pi/180}
    local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
    
    local targetPos1={90*math.pi/180,90*math.pi/180,-90*math.pi/180,90*math.pi/180,90*math.pi/180,90*math.pi/180}
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos1)

    local targetPos2={-90*math.pi/180,45*math.pi/180,90*math.pi/180,135*math.pi/180,90*math.pi/180,90*math.pi/180}
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos2)

    local targetPos3={0,0,0,0,0,0}
    moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos3)
end
end
The code does not returns errors, but the robot doesn't move at all!
What's wrong in my code?

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

coppelia wrote: 20 Jul 2021, 10:43 Hello,

first of all, make sure to use CoppeliaSim V4.2.0.
Then, have a look at the child script attached to the model. Around line 160, you could add something like:

Code: Select all

if currentPosOnPath>pathL*0.5 and walkingDir>0 and not alreadyPausedMiddleWay then
    alreadyPausedMiddleWay=true
    pauseUntil=simTime+3
end
which will perform a pause of 3 seconds when at the middle of the path. Just once. You can modulate velocity with following:

Code: Select all

nominalVelocity=newVelocity
vel=nominalVelocity*0.8/0.56 -- in order to keep correct movement interpolation 
Of course, best would be to rewrite the script according to what you need exactly, maybe additionally using a threaded script, which would make things much easier. Above is just a quick and dirty way to archive what you want, quickly.

Cheers
Ok, thank you, I've tried your solution and it actually works fine, but I've the following problems still going on:
  • I should get the time range for start and stop from the operating robot (i.e. restart when the robot has finished its task), and I've not understand how to get that.
  • I don't know the percentage of the path length where I want Bill to stop, I would like to stop him when he reaches the fourth control point of the path, there's a way to achieve that?
I would also like to know if there is a way to get the Bill model dimensions and to edit them, because when I go to the Dummy model I'm not able to find this information.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

coppelia wrote: 20 Jul 2021, 10:43 Hello,

first of all, make sure to use CoppeliaSim V4.2.0.
Then, have a look at the child script attached to the model. Around line 160, you could add something like:

Code: Select all

if currentPosOnPath>pathL*0.5 and walkingDir>0 and not alreadyPausedMiddleWay then
    alreadyPausedMiddleWay=true
    pauseUntil=simTime+3
end
which will perform a pause of 3 seconds when at the middle of the path. Just once. You can modulate velocity with following:

Code: Select all

nominalVelocity=newVelocity
vel=nominalVelocity*0.8/0.56 -- in order to keep correct movement interpolation 
Of course, best would be to rewrite the script according to what you need exactly, maybe additionally using a threaded script, which would make things much easier. Above is just a quick and dirty way to archive what you want, quickly.

Cheers
Hi,

As I said in my previous answer, the code that you proposed was very useful, but I want to stop Bill on a specific control point of the path, but I don't know if it's possible to get it's value along path (You set a 0.5 for the middle of the path, I evaluate that the value I need is around 0.6, but to get that I had to change it several times, I would like to compute it using the script).
I also would like to set the time of stop from an external information (i.e. StopTime = until the robot has finished its task), but I still have no clue on how I could get that situation.
Plus, if possible, I would like that, when Bill stops, he gets a pose as "standing Bill" (Right now, he stops randomly on a frame where he's walking)

I hope that there's some hint to fix this problems in my simulation.
Thanks in advance for your support.

Cheers

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

Re: Speed and stop on path

Post by coppelia »

if you use CoppeliaSim 4.2+, then if you open the hierarchy of the path object, you'll find all the control points there, as dummy objects. Then you could use sim.getClosestPosOnPath to get the position along the path that is closest to a given coordinate.

Cheers

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

coppelia wrote: 27 Jul 2021, 19:52 if you use CoppeliaSim 4.2+, then if you open the hierarchy of the path object, you'll find all the control points there, as dummy objects. Then you could use sim.getClosestPosOnPath to get the position along the path that is closest to a given coordinate.

Cheers
Hi, thanks for your reply.

I tried the function you suggested, but it returns a value d=0.0.
I used the following code:

Code: Select all

    local pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(pathHandle,'PATH'))
    local m=Matrix(#pathData//7,7,pathData)
    pathPositions=m:slice(1,1,m:rows(),3):data()
    pathLengths,pathL=sim.getPathLengths(pathPositions,3)
    pathM=sim.getObjectMatrix(pathHandle,-1)
    
    StopPosition_Handle=sim.getObjectHandle('CtrlP_4')
    StopPosition_Coords=sim.getObjectPosition(StopPosition_Handle,-1)
    StopPosition=sim.getClosestPosOnPath(pathPositions,pathLengths,StopPosition_Coords)

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

coppelia wrote: 20 Jul 2021, 10:43 Hello,

first of all, make sure to use CoppeliaSim V4.2.0.
Then, have a look at the child script attached to the model. Around line 160, you could add something like:

Code: Select all

if currentPosOnPath>pathL*0.5 and walkingDir>0 and not alreadyPausedMiddleWay then
    alreadyPausedMiddleWay=true
    pauseUntil=simTime+3
end
which will perform a pause of 3 seconds when at the middle of the path. Just once. You can modulate velocity with following:

Code: Select all

nominalVelocity=newVelocity
vel=nominalVelocity*0.8/0.56 -- in order to keep correct movement interpolation 
Of course, best would be to rewrite the script according to what you need exactly, maybe additionally using a threaded script, which would make things much easier. Above is just a quick and dirty way to archive what you want, quickly.

Cheers
I'm sorry to bother you again, but I got a little problem with the code you proposed.

Even if you said that I could have used it just once, I tried to modify it in order to get several stops during the simulation.
Unfortunately, when the model stops for the second time, then the rest of the walk proceeds really slowly (like 1 step every 2 seconds). I understood that it's linked to the alreadyPausedMiddleWay string, but I found nothing about that here on the forum or into the manual, so I don't know how to fix this problem.

Have you any hint about that?

Cheers

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: Speed and stop on path

Post by fferri »

Isn't simpler to rewrite your path following script, as suggested?

You can use sim.getPathInterpolatedConfig like in the last example of https://www.coppeliarobotics.com/helpFiles/en/paths.htm and provide a timing law for posAlongPath that does exactly what you want.

Leizah
Posts: 61
Joined: 20 Jun 2021, 16:47

Re: Speed and stop on path

Post by Leizah »

fferri wrote: 30 Jul 2021, 08:40 Isn't simpler to rewrite your path following script, as suggested?

You can use sim.getPathInterpolatedConfig like in the last example of https://www.coppeliarobotics.com/helpFiles/en/paths.htm and provide a timing law for posAlongPath that does exactly what you want.
Hi, thanks for your reply.

I've actually tried to rewrite the code but, due to my poor knowledge of the Lua language, the script I made didn't work properly, even if I analized deeply the examples into the Coppelia folder.
Furthermore, the pre-existing code actually works ok for the goal that I'm trying to archieve, even if it still gives me the problems that I mentioned into my previous reply to this post.

The solution that you are actually suggested doesn't really fits my needs, because the main goal of this script is to stop the model when he reaches a selected position.

Actually, the path is generated using the MATLAB remoteAPI, according to a previous optimization I've made, and is made up of 10 control points.
I need to set conditions like "When Bill reaches the first control point, then this happens", and I must archieve this result, in order to be able to perform further optimizations in MATLAB.
In fact, If I set up a timing law for trigger the Bill stops, when the position of a control point changes I'll have a Bill that just stops where he is not supposed to.

I've tried to get the control points positions along path, but, as I said in a previous reply, I was not able to get a value from the sim.getClosestPosOnPath function.

I asked about the alreadyPausedMiddleWay string because, as I said, I've understood that this is the command that gives me problem, and I thought that there was an easy fix to that issue.

Now I don't know how to proceed, but I'm still positive that I can modify the code in order to get what I need.
However, thank you for your time and support.

Cheers

Post Reply