OMPL orientation constraint

[kat]

Hello,
I'm trying to path plan using OMPL whilst keeping the orientation of the end effector the same (eg carrying a cup of liquid task).

I'm not able to find how to set the contraint in the sim.ompl api.
I've tried using the validation callback to check the orientation of the end effector but it runs for very long times without finding a solution that satisfies the check.
Could you please let me know the best way to implement this?
Thanks,
Kat

[fferri]

OMPL constraints are not supported by the OMPL plugin.

Depending on what you want to achieve, you might be able to get a similar result with custom a state sampler, a custom state validator, or by CoppeliaSim's inverse kinematics, or any combination of these.

Difficult to go into more details than this without knowing what you are trying to do and your actual scene/code.

[kat]

Hello,
Ok, here is a link to the scene:

https://www.dropbox.com/scl/fi/xehlb2wj ... 3vacj&dl=0

its just a simple pick up a cup and move it, keeping it upright with an orientation constraint. Do you have a suggestion on how to best do that?

Best,
Kat

[fferri]

Maintaining orientation constant for the whole path is one of those situations where OMPL constraints are needed.

Currently, the default state interpolator performs interpolation between states in the joint space, not in the cartesian space; e.g. consider two states A and B, both with level orientation: a state interpolated between A and B it may tilt, because a straight-line trajectory in joint space can be curved in cartesian space if the robot has revolute joints.

But here's a workaround:

One can say that such state (i.e. tilting) is invalid, by overriding the state validation function (set via simOMPL.setStateValidationCallback), by checking that tip's X axis \(x_{tip}\) is aligned with world's \(z=\left(0,0,1\right)\), i.e. \(x_{tip} \cdot z \approx 1\) (note: vec:dot(Vector{0,0,1})==vec[3]):

Code: Select all

function validationCb(config)
    local retVal,origCfg=true,simOMPL.readState(task)
    simOMPL.writeState(task,config)
    for i=1,#collisionPairs/2,1 do
        local result,_=sim.checkCollision(collisionPairs[(i-1)*2+1],collisionPairs[(i-1)*2+2])
        if result>0 then retVal=false; break end
    end

    -- keep orientation up (check that tip's X is aligned with world Z):
    local m=Matrix(3,4,sim.getObjectMatrix(simTip))
    if m:col(1)[3]<0.95 then retVal=false end

    simOMPL.writeState(task, origCfg)
    return retVal
end

however, for the above implementation to work, there has to be a margin of error; if you impose that m:col(1)[3]==1 it will never work, because joint-space straight-line interpolation, and low probability of sampling a correct state.

To increase the probability of sampling good states, you should also define a valid state sampler (set via simOMPL.setValidStateSamplerCallback) to help the planner not waste time with states that are invalid by definition (i.e. tilting), and instead sample states that are always with the tip oriented correctly.

But in the end, this thing still won't work without a small margin of error.

[kat]

Hello,

Thanks for the information, I've implemented the validationCb and I have attempted the simOMPL.setValidStateSamplerCallback. I've found that depending on the OMPL algorithm used, it isn't always called? And when it is called, depending on the OMPL algorithm it either uses the simOMPL.validStateSamplerCallback() or the simOMPL.validStateSamplerCallbackNear(float[] state, float distance) or both.

Here is the new scene:
https://www.dropbox.com/scl/fi/onwo67la ... fzebe&dl=0

When doing the constrained motion planning, it usually can find a valid solution but often it takes a long time. I'm not sure if I've implemented the simOMPL.setValidStateSamplerCallback and the two callback functions correctly, could you suggest a better approach to take?

Best,
Kat

[fferri]

Yes, the valid state sampler callbacks need to be implemented both, as each is used in different situations.

More info in the OMPL docs as usual:

https://ompl.kavrakilab.org/samplers.html

[pr2001]

Hello
I encountered this threat because I was trying to do something similar to what you needed at the time.
With the help of your code I managed to make it work a little.
The problem is that it takes a long time to find a way and sometimes it doesn't even work.
I also noticed that the OMPL algorithm using simOMPL.validStateSamplerCallbackNear doesn't seem to work.
I was wondering if you made any progress on this task?
Or found another way?
Thanks.