@@ -1676,21 +1676,21 @@ def ikine(
1676
1676
"""
1677
1677
Inverse kinematics by optimization without joint limits
1678
1678
1679
- q, success, err = ikine(T) are the joint coordinates corresponding to
1680
- the robot end-effector pose T which is an SE3 object or homogenenous
1681
- transform matrix (4x4), and n is the number of robot joints.
1679
+ ``q, failure, reason = ikine(T)`` are the joint coordinates (n)
1680
+ corresponding to the robot end-effector pose ``T`` which is an ``SE3``
1681
+ instance. ``failure`` is True if the solver failed, and ``reason``
1682
+ contains details of the failure.
1682
1683
1683
1684
This method can be used for robots with any number of degrees of
1684
1685
freedom.
1685
1686
1686
1687
Trajectory operation:
1687
- In all cases if T is a vector of SE3 objects (m) or a homogeneous
1688
- transform sequence (4x4xm) then returns the joint coordinates
1689
- corresponding to each of the transforms in the sequence. q is nxm
1690
- where n is the number of robot joints. The initial estimate of q for
1688
+ If ``T`` contains multiple values, ie. a trajectory, then returns the joint coordinates
1689
+ corresponding to each of the pose values in ``T``. ``q`` is nxm
1690
+ where n is the number of robot joints. The initial estimate of ``q`` for
1691
1691
each time step is taken as the solution from the previous time step.
1692
- Retruns trajectory of joints q (nxm), list of success (m) and list of
1693
- errors (m)
1692
+ Returns trajectory of joints ``q`` (nxm), list of failure (m) and list of
1693
+ error reasons (m).
1694
1694
1695
1695
:param T: The desired end-effector pose
1696
1696
:type T: SE3 or SE3 trajectory
@@ -1717,12 +1717,12 @@ def ikine(
1717
1717
than Levenberg-Marquadt
1718
1718
:type transpose: float
1719
1719
1720
- :retrun q: The calculated joint values
1720
+ :return q: The calculated joint values
1721
1721
:rtype q: float ndarray(n)
1722
- :retrun success : IK solved (True) or failed (False)
1723
- :rtype success: bool
1724
- :retrun error: If failed, what went wrong
1725
- :rtype error: List of String
1722
+ :return failure : IK solver failed
1723
+ :rtype failure: bool or list of bool
1724
+ :return error: If failed, what went wrong
1725
+ :rtype error: List of str
1726
1726
1727
1727
Underactuated robots:
1728
1728
For the case where the manipulator has fewer than 6 DOF the
@@ -1880,6 +1880,7 @@ def ikine(
1880
1880
# Are we there yet
1881
1881
if np .linalg .norm (W @ e ) < tol :
1882
1882
# print(iterations)
1883
+ failed .append (False )
1883
1884
break
1884
1885
1885
1886
# Compute the Jacobian
@@ -1934,17 +1935,16 @@ def ikine(
1934
1935
qt [:, i ] = q
1935
1936
tcount += iterations
1936
1937
1937
- if failed :
1938
- err .append (
1939
- 'failed to converge: try a different '
1940
- 'initial value of joint coordinates' )
1941
- else :
1942
- failed .append (False )
1938
+ if any (failed ):
1939
+ err .append (
1940
+ 'failed to converge: try a different '
1941
+ 'initial value of joint coordinates' )
1943
1942
1944
1943
if trajn == 1 :
1945
1944
qt = qt [:, 0 ]
1945
+ failed = failed [0 ]
1946
1946
1947
- return qt , not failed , err
1947
+ return qt , failed , err
1948
1948
1949
1949
def ikine3 (self , T , left = True , elbow_up = True ):
1950
1950
"""
0 commit comments