Skip to content

Commit f1a7b82

Browse files
committed
add global solver method
update doco
1 parent ec48bc1 commit f1a7b82

File tree

1 file changed

+172
-1
lines changed
  • roboticstoolbox/robot

1 file changed

+172
-1
lines changed

roboticstoolbox/robot/IK.py

Lines changed: 172 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -197,6 +197,7 @@ def ikine_LM(
197197
198198
.. note::
199199
200+
- See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
200201
- Implements a Levenberg-Marquadt variable-step-size solver.
201202
- The tolerance is computed on the norm of the error between
202203
current and desired tool pose. This norm is computed from
@@ -443,6 +444,7 @@ def ikine_LMS(
443444
444445
.. note::
445446
447+
- See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
446448
- Implements a modified Levenberg-Marquadt variable-step-size solver
447449
which is quite robust in practice.
448450
- The tolerance is computed on the norm of the error between
@@ -712,7 +714,7 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
712714
:return: inverse kinematic solution
713715
:rtype: named tuple
714716
715-
``sol = robot.ikine_unc(T)`` are the joint coordinates (n) corresponding
717+
``sol = robot.ikine_min(T)`` are the joint coordinates (n) corresponding
716718
to the robot end-effector pose T which is an SE3 object. The
717719
return value ``sol`` is a named tuple with elements:
718720
@@ -760,6 +762,7 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
760762
761763
.. note::
762764
765+
- See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
763766
- Uses ``SciPy.minimize`` with bounds.
764767
- Joint limits are considered in this solution.
765768
- Can be used for robots with arbitrary degrees of freedom.
@@ -858,6 +861,174 @@ def cost(q, T, weight, costfun, stiffness):
858861
else:
859862
return solutions
860863

864+
# --------------------------------------------------------------------- #
865+
866+
def ikine_global(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None, options={}):
867+
r"""
868+
Inverse kinematics by optimization with joint limits (Robot superclass)
869+
870+
:param T: The desired end-effector pose or pose trajectory
871+
:type T: SE3
872+
:param q0: initial joint configuration (default all zeros)
873+
:type q0: ndarray(n)
874+
:param qlim: enforce joint limits
875+
:type qlim: bool
876+
:param ilimit: Iteration limit (default 1000)
877+
:type ilimit: int
878+
:param tol: Tolerance (default 1e-16)
879+
:type tol: tol
880+
:param method: minimization method to use
881+
:type method: str
882+
:param stiffness: Stiffness used to impose a smoothness contraint on
883+
joint angles, useful when n is large (default 0)
884+
:type stiffness: float
885+
:param costfun: User supplied cost term, optional
886+
:type costfun: callable
887+
:return: inverse kinematic solution
888+
:rtype: named tuple
889+
890+
``sol = robot.ikine_min(T)`` are the joint coordinates (n) corresponding
891+
to the robot end-effector pose T which is an SE3 object. The
892+
return value ``sol`` is a named tuple with elements:
893+
894+
============ ========== ============================================================
895+
Element Type Description
896+
============ ========== ============================================================
897+
``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None``
898+
``success`` bool whether a solution was found
899+
``reason`` str reason for the failure
900+
``iterations`` int number of iterations
901+
``residual`` float final value of cost function
902+
============ ========== ============================================================
903+
904+
**Minimization method**:
905+
906+
By default this method uses:
907+
908+
- the Scipy ``SLSQP`` minimizer for the case of no joint limits
909+
- the Scipy ``trust-constr`` minimizer for the case with joint limits.
910+
This gives good results but is very slow. An alternative is
911+
``L-BFGS-B`` (Broyden–Fletcher–Goldfarb–Shanno) but for redundant
912+
robots can sometimes give poor results, pushing against the joint
913+
limits when there is no need to.
914+
915+
In both case the function to be minimized is the squared norm of a
916+
vector :math:`[d,a]` with components respectively the
917+
translation error and rotation error in Euler vector form, between the
918+
desired pose and the current estimate obtained by inverse kinematics.
919+
920+
**Additional cost terms**:
921+
922+
This method supports two additional costs:
923+
924+
- ``stiffness`` imposes a penalty on joint variation
925+
:math:`\sum_{j=1}^N (q_j - q_{j-1})^2` which tends to keep the
926+
arm straight
927+
- ``costfun`` add a cost given by a user-specified function ``costfun(q)``
928+
929+
**Trajectory operation**:
930+
931+
If ``len(T) > 1`` it is considered to be a trajectory, and the result is
932+
a list of named tuples such that ``sol[k]`` corresponds to ``T[k]``. The
933+
initial estimate of q for each time step is taken as the solution from
934+
the previous time step.
935+
936+
.. note::
937+
938+
- Uses ``SciPy.minimize`` with bounds.
939+
- Joint limits are considered in this solution.
940+
- Can be used for robots with arbitrary degrees of freedom.
941+
- The inverse kinematic solution is generally not unique, and
942+
depends on the initial guess ``q0``.
943+
- The default value of ``q0`` is zero which is a poor choice for
944+
most manipulators since it often corresponds to a
945+
kinematic singularity.
946+
- Such a solution is completely general, though much less
947+
efficient than analytic inverse kinematic solutions derived
948+
symbolically.
949+
- The objective function (error) is
950+
:math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
951+
where :math:`\mat{\Omega}` is a diagonal matrix.
952+
- Joint offsets, if defined, are accounted for in the solution.
953+
954+
.. warning::
955+
956+
- The objective function is rather uncommon.
957+
- Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
958+
uses a scalar cost-function and does not provide a Jacobian.
959+
960+
:author: Bryan Moutrie, for RTB-MATLAB
961+
962+
:seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_min`
963+
964+
"""
965+
966+
if not isinstance(T, SE3):
967+
T = SE3(T)
968+
969+
if q0 is None:
970+
q0 = np.zeros((self.n))
971+
else:
972+
q0 = base.getvector(q0, self.n)
973+
974+
solutions = []
975+
976+
wr = 1 / self.reach
977+
weight = np.r_[wr, wr, wr, 1, 1, 1]
978+
979+
optdict = {'maxiter': ilimit}
980+
if options is not None and isinstance(options, dict):
981+
optdict.update(options)
982+
else:
983+
raise ValueError('options must be a dict')
984+
985+
if qlim:
986+
# dealing with joint limits
987+
988+
989+
if method is None:
990+
method='differential-evolution'
991+
992+
if method == 'brute':
993+
# requires a tuple of tuples
994+
opt['ranges'] = tuple([tuple(l.qlim) for l in self])
995+
else:
996+
opt['bounds'] = Bounds(self.qlim[0, :], self.qlim[1, :])
997+
else:
998+
# no joint limits
999+
if method is None:
1000+
method = 'basinhopping'
1001+
bounds = None
1002+
1003+
def cost(q, T, weight):
1004+
# T, weight, costfun, stiffness = args
1005+
e = _angle_axis(self.fkine(q).A, T) * weight
1006+
return (e**2).sum()
1007+
1008+
for Tk in T:
1009+
res = minimize(
1010+
cost,
1011+
q0,
1012+
args=(Tk.A,),
1013+
method=method,
1014+
tol=tol,
1015+
options=options
1016+
)
1017+
1018+
# trust-constr seems to work better than L-BFGS-B which often
1019+
# runs a joint up against its limit and terminates with position
1020+
# error.
1021+
# but 'truts-constr' is 5x slower
1022+
1023+
solution = iksol(res.x, res.success, res.message, res.nit, res.fun)
1024+
solutions.append(solution)
1025+
q0 = res.x # use this solution as initial estimate for next time
1026+
1027+
if len(T) == 1:
1028+
return solutions[0]
1029+
else:
1030+
return solutions
1031+
8611032
# --------------------------------------------------------------------- #
8621033

8631034
# def ikine_min(self, T, q0=None, pweight=1.0, stiffness=0.0,

0 commit comments

Comments
 (0)