Ruckig Trajectory change Velocity
Posted: 14 Nov 2022, 16:56
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
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
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
https://drive.google.com/file/d/1tjC5kS ... share_link
Thank you in advance and best regards
Chimchimini