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.