Invalid IK solution

[zhouxian]

Hi, I wonder if there is a way to compute valid IK solution with calculateInverseKinematics. Now it could return IK solution that exceed joint limit(as in from getJointInfo()), or sometimes in self-collision. I tried specifying upperLimits and lowerLimits when calling it but no help.

Is there a way to calculate only valid IK or return False or something like that?

Thanks!

[erwincoumans]

Difficult to tell without simple repro case what the problem is. It could be that the joint limits are not active or properly specified. You may need to set the joint limits tighter. You could check yourself if any constraint is violated, and possibly call the IK solver again from a different starting point. You may need to do some debugging, the IK solver innerloop isn't very complicated.

[zhouxian]

A easy reproduce is:

import pybullet as p
import math
import pybullet_data

clid = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally

kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)

#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
ul = [.967, 2, 2.96, 2.29, 2.96, 2.09, 3.05]
#joint ranges for null space
jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

for i in range(numJoints):
  p.resetJointState(kukaId, i, rp[i])


pos = [-0.1, 0, 0.2]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])

jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul, jr, rp)

The returned jointPoses is

(2.740106077145573e-12,
 -0.3690549565852974,
 -7.501466373859642e-12,
 2.339617969438699,
 -1.4608018038690208e-11,
 -0.4466336838760028,
 5.174165072837143e-12)

Note that joint 3 has a value of 2.339, which exceeds both the limit specified by ll and ul, and the limit obtained from p.getJointInfo(kukaId, 3), which is [-2.09439510239, 2.09439510239].

[Steven89Liu]

i don't find any papers about the DLS with Nullspace, does any body can share the relative papers with me, thanks.
for this case, i change several points as below in bold. but it still can't work if i set the orientation of the end effector. i have a question, are you sure it can get what you expect with these limitions?

import pybullet as p
import math
import pybullet_data

clid = p.connect(p.GUI)
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally

kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)

#lower limits for null space
ll = [-.967, -2, -2.96, 0.19, -2.96, -2.09, -3.05]
#upper limits for null space
ul = [.967, 2, 2.96, 2.09, 2.96, 2.09, 3.05]
#joint ranges for null space
jr = [5.8, 4, 5.8, 4, 5.8, 4, 6]
#restposes for null space
rp = [0, 0, 0, 0.5 * math.pi, 0, -math.pi * 0.5 * 0.66, 0]
#joint damping coefficents
jd = [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

for i in range(numJoints):
p.resetJointState(kukaId, i, rp[i])

pos = [-0.1, 0, 0.2]
#end effector points down, not up (in case useOrientation==1)
orn = p.getQuaternionFromEuler([0, -math.pi, 0])

#jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, orn, ll, ul, jr, rp, maxNumIterations=100)
jointPoses = p.calculateInverseKinematics(kukaId, kukaEndEffectorIndex, pos, lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp, maxNumIterations=100)
print("jonintPoses = ", jointPoses)

//jonintPoses = (1.22050950923848e-11, -0.9361807246909419, -1.1822010390722412e-11, 2.0873899511548615, -4.9901012629469686e-12, -1.9240952471398638, 0.0)

[ghx0x0]

@Steven89Liu hi, have u solved this ik problem?
I've meet the same joint-limited problem, and also the joint-3 limits. Modify the 'll' and 'ul' can't solve it.

[Steven89Liu]

last time i had a suspicion that the model can't get what it expected with those constraints. if you can be sure your setting can work, Can you provide a similar sample that i can work with you to find the root cause?

[Steven89Liu]

i read the code and doc again and find a new clue, as i understand, the nullspace method is used to adjust the joint angule in the direct of the nullspace of Jacobian to meet the limitation. but current implementation the nullspace velocity is caculated based on the old position, in my view, we should calculate the nullspace velocity based on the newer position, that is first we should caculate the deta theta with DSL and then calcuate the nullspace and finally call the CalcDeltaThetasDLSwithNullspace