Changing ETS revolute joint constant does not apply to inverse kinematics

[lucasbardis]

Hello,

Describe the bug
I tried to change the value of a constant ETS revolute joint and it works, but it seems to not take it into account when computing inverse kinematics, either with "ikine_LM" or "ik_LM".

Version information
Did you install from PyPI or GitHub?
PyPI : 1.1.0

To Reproduce
Here is a code for reproduction of the behavior :

import roboticstoolbox as rtb
import numpy as np

if __name__ == "__main__":
    ets_1 = rtb.ET.Rx(0.1607) * rtb.ET.Rz(0.1072) * rtb.ET.Ry() * rtb.ET.ty(-0.3113) * rtb.ET.Rx() * rtb.ET.ty(-0.254) * rtb.ET.Ry() * rtb.ET.Rx() * rtb.ET.Rz() * rtb.ET.tx(0.03387) * rtb.ET.ty(-0.07897) * rtb.ET.tz(-0.01757)
    ets_2 = rtb.ET.Rx(0) * rtb.ET.Rz(0) * rtb.ET.Ry() * rtb.ET.ty(-0.3113) * rtb.ET.Rx() * rtb.ET.ty(-0.254) * rtb.ET.Ry() * rtb.ET.Rx() * rtb.ET.Rz() * rtb.ET.tx(0.03387) * rtb.ET.ty(-0.07897) * rtb.ET.tz(-0.01757)
    
    print("ets_1 : ", ets_1)
    print("ets_2 : ", ets_2)

    T = np.asarray([[0.9823761, 0.15612919, -0.1027661, 0.0158],
                [-0.01447417, 0.61169345,0.79096247, -0.472],
                [0.18635368, -0.77553517, 0.60317286, 0.237],
                [0, 0, 0, 1]])
    q0 = np.array([0.16219589, -1.26615422, -0.30561781, 0.2537811, -0.26293779])

    sol = ets_1.ikine_LM(T, q0 = q0)
    print(sol)

    # del ets_2[0:2]
    # ets_2.insert(rtb.ET.Rx(0.1607), 0)
    # ets_2.insert(rtb.ET.Rz(0.1072), 1)

    ets_2[0] = rtb.ET.Rx(0.1607)
    ets_2[1] = rtb.ET.Rz(0.1072)
    print("new ets_2 : ", ets_2)

    sol = ets_2.ikine_LM(T, q0 = q0)
    print(sol)

and the associated output :

ets_1 :  Rx(9.207°) ⊕ Rz(6.142°) ⊕ Ry(q0) ⊕ ty(-0.3113) ⊕ Rx(q1) ⊕ ty(-0.254) ⊕ Ry(q2) ⊕ Rx(q3) ⊕ Rz(q4) ⊕ tx(0.03387) ⊕ ty(-0.07897) ⊕ tz(-0.01757)
ets_2 :  Rx(0°) ⊕ Rz(0°) ⊕ Ry(q0) ⊕ ty(-0.3113) ⊕ Rx(q1) ⊕ ty(-0.254) ⊕ Ry(q2) ⊕ Rx(q3) ⊕ Rz(q4) ⊕ tx(0.03387) ⊕ ty(-0.07897) ⊕ tz(-0.01757)
IKSolution: q=[-0.1911, -1.329, 0.08336, 0.2394, -0.01916], success=True, iterations=4, searches=1, residual=6.53e-07
new ets_2 :  Rx(9.207°) ⊕ Rz(6.142°) ⊕ Ry(q0) ⊕ ty(-0.3113) ⊕ Rx(q1) ⊕ ty(-0.254) ⊕ Ry(q2) ⊕ Rx(q3) ⊕ Rz(q4) ⊕ tx(0.03387) ⊕ ty(-0.07897) ⊕ tz(-0.01757)
IKSolution: q=[-0.2903, -3.376, -0.4712, -2.065, -2.331], success=False, reason=iteration and search limit reached, iterations=3000, searches=100, residual=4.68

Expected behavior
I expected the ets_2 to find a solution as it as the exact same parameters as ets_1. Maybe the attribution of the new values of the constant joint are not the right ones ?

Environment:

• Your OS : Windows
• Your Python version : 3.10.9

I thank you in advance for your response.

[jhavl]

Thanks for finding this issue, I've made a fix on the future branch which will be incorporated into the next release.