Changing ETS revolute joint constant does not apply to inverse kinematics #377
Description
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.