Ruckig Trajectory change Velocity

[Chimchimini]

Hello all,

I am currently trying to implement a trajectory using the Ruckig algorithm.

Target:
The object moves from point A to point B. The trajectory is to be determined by the algorithm. The important thing is that I want to change the speed of movement on the way from A to B.

What works so far:
What I have managed to do is to change the speed between several points. For example, I travel from A to B with speed x, and from B to C with speed y.

In general, I have also managed to vary the speed during the movement from A to B, but I have a kind of bug, which I would like to show with the following minimal example.

In the model, there is a sphere that always moves between two points using the Ruckig algorithm.
I can change the speed via a user interface with a checkbox.
If the box is checked, the sphere moves at a higher speed, otherwise at a lower speed.

The transition from slow to fast seems to work without any problems. When I change the speed from fast to slow by unchecking the box, the ball sometimes jumps to a different place. Afterwards, the movement (from the new position) continues normally.

Strangely, I cannot see the position jump when I display the current position of the sphere in the log.

I hope someone knows what to do.


Here's the Child-Script of the Sphere

Code: Select all

function closeEventHandler(h)
    sim.addLog(sim.verbosity_scriptinfos,'Window '..h..' is closing...')
    simUI.hide(h)
end

-- Non-threaded child script, using sim.ruckigPos and sim.ruckigStep:

function sysCall_init()
    --Description of User Interface
    xml = [[
<ui closeable="true" on-close="closeEventHandler" resizable="true">
    <label text="This is a demo of the CustomUI plugin. Browse through the tabs below to explore all the widgets that can be created with the plugin." wordwrap="true" />
    <tabs>
        <tab title="Numeric">
            <label text="Sliders can be oriented horizontally or vertically, and have optional properties that can be set (in the XML) such as minimum and maximum value." wordwrap="true" />
            <label text="" id="3000" wordwrap="true" />
            <checkbox text="fast" id="3001"/>

        </tab>
    </tabs>
</ui>
]]
    
    ui=simUI.create(xml)
    
    --Initialization of global speed Variable
    fast = true
    --Get Sphere Handle
    h=sim.getObject('.')
    
    --Get initial Pose of Sphere
    p=sim.getObjectPosition(h,sim.handle_world)
    local p_init=sim.getObjectPosition(h,sim.handle_world)
    
    -- Two fixed traget Positions to alternat between.
    targetPosVel1={p_init[1]-0.2,p_init[2]+0.4,p_init[3],0,0,0} -- x,y,z in m, vx,vy,vz in m/s Used when "tragetSelector = true"
    targetPosVel2={p_init[1],p_init[2],p_init[3],0,0,0} -- x,y,z in m, vx,vy,vz in m/s Used when "tragetSelector = false"
    
    --Boolean Variable to set next target Position correctly + target allcation
    targetSelector = true
    targetPosVel = targetPosVel1

    --Parameters for ruckig Algorithm
    currentPosVelAccel={p[1],p[2],p[3],0,0,0,0,0,0}
        
    maxVelAccelJerk1={0.02,0.02,0.02,0.05,0.05,0.05,0.025,0.025,0.025}-- vx,vy,vz in m/s, ax,ay,az in m/s^2, jx,jy,jz is ignored (i.e. infinite) with RML type 2
    maxVelAccelJerk2={0.5,0.5,0.5,0.05,0.05,0.05,0.025,0.025,0.025}-- vx,vy,vz in m/s, ax,ay,az in m/s^2, jx,jy,jz is ignored (i.e. infinite) with RML type 2

    
end

function sysCall_sensing()
end

function sysCall_actuation()
    --Get CheckboxValue from UI
    fast = simUI.getCheckboxValue(ui, 3001)
    
    --set maxSpeed accordingly
    if fast == 2 then  -- 2 is the Integer Value when checked
        maxVelAccelJerk = maxVelAccelJerk2
    else 
        maxVelAccelJerk = maxVelAccelJerk1
    end
    
    --instantiate rmlObject with respective maxVelAccelJerk
    rmlObject = nil
    rmlObject=sim.ruckigPos(3,0.01,-1,currentPosVelAccel,maxVelAccelJerk,{1,1,1},targetPosVel)
    
    --Only for Debugging --> checking the Position of the sphere and comparing with results from ruckig
    p=sim.getObjectPosition(h,sim.handle_world)
    print(p)
    print(currentPosVelAccel)

    --This is mostly the ruckig example from the blue cylinder from the example scenes
    if rmlObject then
        print("Running")
        local result,newPosVelAccel=sim.ruckigStep(rmlObject,sim.getSimulationTimeStep())
        
        --save newPosVelAccel for the instantiation of the rmlObject in the next iteration
        --I personally think the problem is in this new allocation of maxVelAccelJerk. However I
        --I do not understand why slow-->fast is working but fast-->slow isn't
        currentPosVelAccel = newPosVelAccel   
   
   
        if result~=-1 then
            sim.setObjectPosition(h,sim.handle_world,newPosVelAccel)
        end
        if result==1 then
            print("Finished")
            rmlObject=nil
            -- set new targetpoint depending on targetSelector (changed from example implementation)
            if targetSelector then
                targetPosVel = targetPosVel2
                rmlObject=sim.ruckigPos(3,0.01,-1,currentPosVelAccel,maxVelAccelJerk,{1,1,1},targetPosVel)
                targetSelector = false
            else
                targetPosVel = targetPosVel1
                rmlObject=sim.ruckigPos(3,0.01,-1,currentPosVelAccel,maxVelAccelJerk,{1,1,1},targetPosVel)
                targetSelector = true
            end
        elseif result==-1 then
            print("Error")
            sim.ruckigRemove(rmlObject)
            rmlObject=nil        
        end
    end
end

function sysCall_cleanup()
end

I also have the Coppelia Model for Download here:
https://drive.google.com/file/d/1tjC5kS ... share_link

Thank you in advance and best regards
Chimchimini

[coppelia]

Hello,

in your example you are constantly creating a new Ruckig object (i.e. in each actuation pass), and you do not destroy it. Usually you would creat a Ruckig object, then step until you reached the goal position/velocity, then destroy the ruckig object.

Have a look at the demo scene that shows many different examples on how to use the Ruckig library correctly: scenes/trajectoryAndMotion/ruckigOnlineTrajectoryGeneratorExamples.ttt

Cheers

[Chimchimini]

Hello,

first, thank you for the fast response.

I am aware, of creating a new Ruckig object with each step. That way, I control the "maxVelAccelJerk" at every Simulation Step.
However, I indeed did not delete the Ruckig object after each Step. I tried this solution as well, but the Bug with the Object jumping still remains.

I have taken a look at the examples before. All of the examples either use "moveToConfig" or "moveToPose" which are blocking-mode (I assume this means you can't interfere till the movement is finished). That is the reason I tried the seperated type with "ruckigPos" "ruckigStep" and "ruckigRemove".

All the examples in ruckigOnlineTrajectoryGeneratorExamples.ttt do not change the maximum allowed speed during the movement between start- and goal-Point. That's the reason, why I create a new ruckig Object every actuation pass. (to be honest, this also feels somehow wrong to me, but it was the only way I could change the velocity during movement).

However, as described above, that is giving me the jumping bug.
I did not find a way to pass the new allowed maximum velocity without creating a new ruckigObject every time step. However, if there is one, I would be thankful for advice.

Best regards
Chimchimini

[coppelia]

So I noticed that this jump only happens when going from fast mode to slow mode, while the sphere travels fast. In that situation the sphere has to overshoot the target position, since it is not allowed to decelerate fast enough. In your situation, you have 3 DoFs, so those 3 DoFs are also linked with each other and you get that strange effect.

Try with one DoF first and make sure to catch possible Ruckig errors.

Cheers

[Chimchimini]

Hi again,

thanks for your response.
Indeed, you were right.

Changing the according maximum Accelerations and Jerks, the Model is now working perfectly.
When changing from fast to slow, there still are "additional" movements, but all of them within a fluid movement and without jumps.

As you mentioned, the additional movements are probably, because the new target point when changing to slow can already lay "behind" the sphere. The Ruckig algorithm therefore calculates a new trajectory also covering this new point behind the sphere, resulting in, let's call them "small correction slopes".

However, my issue can be considered as solved, since the "slopes" are another topic, but I can accept them at the moment.

Thanks for the help and best regards,
Chimchimini