8000 fixup error with failure status and tweak doco · ctc-eng/robotics-toolbox-python@755d968 · GitHub
[go: up one dir, main page]

Skip to content

Commit 755d968

Browse files
committed
fixup error with failure status and tweak doco
1 parent f85ee77 commit 755d968

File tree

1 file changed

+21
-21
lines changed

1 file changed

+21
-21
lines changed

roboticstoolbox/robot/SerialLink.py

Lines changed: 21 additions & 21 deletions
Original file line numberDiff line numberDiff line change
@@ -1676,21 +1676,21 @@ def ikine(
16761676
"""
16771677
Inverse kinematics by optimization without joint limits
16781678
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.
16821683
16831684
This method can be used for robots with any number of degrees of
16841685
freedom.
16851686
16861687
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
16911691
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).
16941694
16951695
:param T: The desired end-effector pose
16961696
:type T: SE3 or SE3 trajectory
@@ -1717,12 +1717,12 @@ def ikine(
17171717
than Levenberg-Marquadt
17181718
:type transpose: float
17191719
1720-
:retrun q: The calculated joint values
1720+
:return q: The calculated joint values
17211721
: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
17261726
17271727
Underactuated robots:
17281728
For the case where the manipulator has fewer than 6 DOF the
@@ -1880,6 +1880,7 @@ def ikine(
18801880
# Are we there yet
18811881
if np.linalg.norm(W @ e) < tol:
18821882
# print(iterations)
1883+
failed.append(False)
18831884
break
18841885

18851886
# Compute the Jacobian
@@ -1934,17 +1935,16 @@ def ikine(
19341935
qt[:, i] = q
19351936
tcount += iterations
19361937

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')
19431942

19441943
if trajn == 1:
19451944
qt = qt[:, 0]
1945+
failed = failed[0]
19461946

1947-
return qt, not failed, err
1947+
return qt, failed, err
19481948

19491949
def ikine3(self, T, left=True, elbow_up=True):
19501950
"""

0 commit comments

Comments
 (0)
0