@@ -197,6 +197,7 @@ def ikine_LM(
197
197
198
198
.. note::
199
199
200
+ - See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
200
201
- Implements a Levenberg-Marquadt variable-step-size solver.
201
202
- The tolerance is computed on the norm of the error between
202
203
current and desired tool pose. This norm is computed from
@@ -443,6 +444,7 @@ def ikine_LMS(
443
444
444
445
.. note::
445
446
447
+ - See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
446
448
- Implements a modified Levenberg-Marquadt variable-step-size solver
447
449
which is quite robust in practice.
448
450
- 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,
712
714
:return: inverse kinematic solution
713
715
:rtype: named tuple
714
716
715
- ``sol = robot.ikine_unc (T)`` are the joint coordinates (n) corresponding
717
+ ``sol = robot.ikine_min (T)`` are the joint coordinates (n) corresponding
716
718
to the robot end-effector pose T which is an SE3 object. The
717
719
return value ``sol`` is a named tuple with elements:
718
720
@@ -760,6 +762,7 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
760
762
761
763
.. note::
762
764
765
+ - See `Toolbox kinematics wiki page <https://github.com/petercorke/robotics-toolbox-python/wiki/Kinematics>`_
763
766
- Uses ``SciPy.minimize`` with bounds.
764
767
- Joint limits are considered in this solution.
765
768
- Can be used for robots with arbitrary degrees of freedom.
@@ -858,6 +861,174 @@ def cost(q, T, weight, costfun, stiffness):
858
861
else :
859
862
return solutions
860
863
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
+
861
1032
# --------------------------------------------------------------------- #
862
1033
863
1034
# def ikine_min(self, T, q0=None, pweight=1.0, stiffness=0.0,
0 commit comments