@@ -51,7 +51,7 @@ def ikine_mmc(
51
51
# bad update
52
52
# gain = gain / 2
53
53
dt = dt / 2
54
- # q = q_last
54
+ q = q_last
55
55
# print('Down')
56
56
57
57
e_prev = e
@@ -204,7 +204,7 @@ def ikine_LM(
204
204
distances and angles without any kind of weighting.
205
205
- The inverse kinematic solution is generally not unique, and
206
206
depends on the initial guess ``q0``.
207
- - The default value of ``q0`` is zero which is a poor choice for
207
+ - The default value of ``q0`` is zero which is a poor choice for
208
208
most manipulators since it often corresponds to a
209
209
kinematic singularity.
210
210
- Such a solution is completely general, though much less
@@ -395,9 +395,9 @@ def ikine_LMS(
395
395
and rotation about X, Y and Z respectively.
396
396
:type mask: ndarray(6)
397
397
:param ilimit: maximum number of iterations (default 500)
398
- :type ilimit: int
398
+ :type ilimit: int
399
399
:param tol: final error tolerance (default 1e-10)
400
- :type tol: float
400
+ :type tol: float
401
401
:param ωN: damping coefficient
402
402
:type ωN: float (default 1e-3)
403
403
:return: inverse kinematic solution
@@ -452,7 +452,7 @@ def ikine_LMS(
452
452
distances and angles without any kind of weighting.
453
453
- The inverse kinematic solution is generally not unique, and
454
454
depends on the initial guess ``q0``.
455
- - The default value of ``q0`` is zero which is a poor choice for
455
+ - The default value of ``q0`` is zero which is a poor choice for
456
456
most manipulators since it often corresponds to a
457
457
kinematic singularity.
458
458
- Such a solution is completely general, though much less
@@ -466,7 +466,7 @@ def ikine_LMS(
466
466
467
467
:references:
468
468
- "Solvability-Unconcerned Inverse Kinematics by the
469
- Levenberg–Marquardt Method", T. Sugihara, IEEE T-RO, 27(5),
469
+ Levenberg–Marquardt Method", T. Sugihara, IEEE T-RO, 27(5),
470
470
October 2011, pp. 984-991.
471
471
472
472
:seealso: :func:`ikine_LM`, :func:`ikine_unc`, :func:`ikine_con`, :func:`ikine_min`
@@ -551,7 +551,6 @@ def ikine_LMS(
551
551
else :
552
552
return solutions
553
553
554
-
555
554
# --------------------------------------------------------------------- #
556
555
557
556
# def ikine_unc(self, T, q0=None, ilimit=1000, tol=1e-16, stiffness=0, costfun=None):
@@ -617,19 +616,19 @@ def ikine_LMS(
617
616
# - Can be used for robots with arbitrary degrees of freedom.
618
617
# - The inverse kinematic solution is generally not unique, and
619
618
# depends on the initial guess ``q0``.
620
- # - The default value of ``q0`` is zero which is a poor choice for
619
+ # - The default value of ``q0`` is zero which is a poor choice for
621
620
# most manipulators since it often corresponds to a
622
621
# kinematic singularity.
623
622
# - Such a solution is completely general, though much less
624
623
# efficient than analytic inverse kinematic solutions derived
625
624
# symbolically.
626
- # - The objective function (error) is
625
+ # - The objective function (error) is
627
626
# :math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
628
627
# where :math:`\mat{\Omega}` is a diagonal matrix.
629
628
# - Joint offsets, if defined, are accounted for in the solution.
630
629
631
- # .. warning::
632
-
630
+ # .. warning::
631
+
633
632
# - The objective function is rather uncommon.
634
633
# - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
635
634
# uses a scalar cost-function and does not provide a Jacobian.
@@ -690,7 +689,9 @@ def ikine_LMS(
690
689
691
690
# --------------------------------------------------------------------- #
692
691
693
- def ikine_min (self , T , q0 = None , qlim = False , ilimit = 1000 , tol = 1e-16 , method = None , stiffness = 0 , costfun = None , options = {}):
692
+ def ikine_min (
693
+ self , T , q0 = None , qlim = False , ilimit = 1000 ,
694
+ tol = 1e-16 , method = None , stiffness = 0 , costfun = None , options = {}):
694
695
r"""
695
696
Inverse kinematics by optimization with joint limits (Robot superclass)
696
697
@@ -739,8 +740,8 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
739
740
``L-BFGS-B`` (Broyden–Fletcher–Goldfarb–Shanno) but for redundant
740
741
robots can sometimes give poor results, pushing against the joint
741
742
limits when there is no need to.
742
-
743
- In both case the function to be minimized is the squared norm of a
743
+
744
+ In both case the function to be minimized is the squared norm of a
744
745
vector :math:`[d,a]` with components respectively the
745
746
translation error and rotation error in Euler vector form, between the
746
747
desired pose and the current estimate obtained by inverse kinematics.
@@ -749,8 +750,8 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
749
750
750
751
This method supports two additional costs:
751
752
752
- - ``stiffness`` imposes a penalty on joint variation
753
- :math:`\sum_{j=1}^N (q_j - q_{j-1})^2` which tends to keep the
753
+ - ``stiffness`` imposes a penalty on joint variation
754
+ :math:`\sum_{j=1}^N (q_j - q_{j-1})^2` which tends to keep the
754
755
arm straight
755
756
- ``costfun`` add a cost given by a user-specified function ``costfun(q)``
756
757
@@ -769,19 +770,19 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
769
770
- Can be used for robots with arbitrary degrees of freedom.
770
771
- The inverse kinematic solution is generally not unique, and
771
772
depends on the initial guess ``q0``.
772
- - The default value of ``q0`` is zero which is a poor choice for
773
+ - The default value of ``q0`` is zero which is a poor choice for
773
774
most manipulators since it often corresponds to a
774
775
kinematic singularity.
775
776
- Such a solution is completely general, though much less
776
777
efficient than analytic inverse kinematic solutions derived
777
778
symbolically.
778
- - The objective function (error) is
779
+ - The objective function (error) is
779
780
:math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
780
781
where :math:`\mat{\Omega}` is a diagonal matrix.
781
782
- Joint offsets, if defined, are accounted for in the solution.
782
783
783
- .. warning::
784
-
784
+ .. warning::
785
+
785
786
- The objective function is rather uncommon.
786
787
- Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
787
788
uses a scalar cost-function and does not provide a Jacobian.
@@ -816,7 +817,7 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
816
817
bounds = opt .Bounds (self .qlim [0 , :], self .qlim [1 , :])
817
818
818
819
if method is None :
819
- method = 'trust-constr'
820
+ method = 'trust-constr'
820
821
else :
821
822
# no joint limits
822
823
if method is None :
@@ -840,7 +841,7 @@ def cost(q, T, weight, costfun, stiffness):
840
841
for Tk in T :
841
842
res = opt .minimize (
842
843
cost ,
843
- q0 ,
844
+ q0 ,
844
845
args = (Tk .A , weight , costfun , stiffness ),
845
846
bounds = bounds ,
846
847
method = method ,
@@ -849,7 +850,7 @@ def cost(q, T, weight, costfun, stiffness):
849
850
)
850
851
851
852
# trust-constr seems to work better than L-BFGS-B which often
852
- # runs a joint up against its limit and terminates with position
853
+ # runs a joint up against its limit and terminates with position
853
854
# error.
854
855
# but 'truts-constr' is 5x slower
855
856
@@ -864,7 +865,9 @@ def cost(q, T, weight, costfun, stiffness):
864
865
865
866
# --------------------------------------------------------------------- #
866
867
867
- def ikine_global (self , T , q0 = None , qlim = False , ilimit = 1000 , tol = 1e-16 , method = None , options = {}):
868
+ def ikine_global (
869
+ self , T , q0 = None , qlim = False , ilimit = 1000 ,
870
+ tol = 1e-16 , method = None , options = {}):
868
871
r"""
869
872
.. warning:: Experimental code for using SciPy global optimizers.
870
873
@@ -889,20 +892,19 @@ def ikine_global(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=No
889
892
890
893
solutions = []
891
894
892
- wr = 1 / self .reach
893
- weight = np .r_ [wr , wr , wr , 1 , 1 , 1 ]
895
+ # wr = 1 / self.reach
896
+ # weight = np.r_[wr, wr, wr, 1, 1, 1]
894
897
895
898
optdict = {}
896
899
897
900
if method is None :
898
- method = 'differential-evolution'
901
+ method = 'differential-evolution'
899
902
900
903
if method == 'brute' :
901
904
# requires a tuple of tuples
902
- optdict ['ranges' ] = tuple ([tuple (l .qlim ) for l in self ])
905
+ optdict ['ranges' ] = tuple ([tuple (li .qlim ) for li in self ])
903
906
else :
904
- optdict ['bounds' ] = tuple ([tuple (l .qlim ) for l in self ])
905
-
907
+ optdict ['bounds' ] = tuple ([tuple (li .qlim ) for li in self ])
906
908
907
909
if method not in ['basinhopping' , 'brute' , 'differential_evolution' ,
908
910
'shgo' , 'dual_annealing' ]:
@@ -915,11 +917,10 @@ def cost(q, T, weight):
915
917
e = _angle_axis (self .fkine (q ).A , T ) * weight
916
918
return (e ** 2 ).sum ()
917
919
918
- for Tk in T :
920
+ for _ in T :
919
921
res = global_minimizer (
920
922
cost ,
921
923
** optdict )
922
-
923
924
924
925
solution = iksol (res .x , res .success , res .message , res .nit , res .fun )
925
926
solutions .append (solution )
@@ -929,7 +930,7 @@ def cost(q, T, weight):
929
930
return solutions [0 ]
930
931
else :
931
932
return solutions
932
-
933
+
933
934
# --------------------------------------------------------------------- #
934
935
935
936
# def ikine_min(self, T, q0=None, pweight=1.0, stiffness=0.0,
@@ -983,12 +984,12 @@ def cost(q, T, weight):
983
984
# - The inverse kinematic solution is generally not unique, and
984
985
# depends on the initial guess ``q0``.
985
986
# - This norm is computed from distances and angles and ``pweight``
986
- # can be used to scale the position error norm to be congruent
987
+ # can be used to scale the position error norm to be congruent
987
988
# with rotation error norm.
988
989
# - For a highly redundant robot ``stiffness`` can be used to impose
989
990
# a smoothness contraint on joint angles, tending toward solutions
990
991
# with are smooth curves.
991
- # - The default value of ``q0`` is zero which is a poor choice for
992
+ # - The default value of ``q0`` is zero which is a poor choice for
992
993
# most manipulators since it often corresponds to a
993
994
# kinematic singularity.
994
995
# - Such a solution is completely general, though much less
@@ -1000,8 +1001,8 @@ def cost(q, T, weight):
1000
1001
# - Joint offsets, if defined, are accounted for in the solution.
1001
1002
# - Joint limits become explicit bounds if 'qlimits' is set.
1002
1003
1003
- # .. warning::
1004
-
1004
+ # .. warning::
1005
+
1005
1006
# - The objective function is rather uncommon.
1006
1007
# - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
1007
1008
# uses a scalar cost-function and does not provide a Jacobian.
@@ -1070,7 +1071,6 @@ def cost(q, T, weight):
1070
1071
# else:
1071
1072
# return solutions
1072
1073
1073
-
1074
1074
# def qmincon(self, q=None):
1075
1075
# """
1076
1076
# Move away from joint limits
@@ -1145,11 +1145,13 @@ def cost(q, T, weight):
1145
1145
# else:
1146
1146
# return qstar, success, error
1147
1147
1148
+
1148
1149
def _angle_axis (T , Td ):
1149
1150
d = base .transl (Td ) - base .transl (T )
1150
1151
R = base .t2r (Td ) @ base .t2r (T ).T
1151
- l = np .r_ [R [2 ,1 ]- R [1 ,2 ], R [0 ,2 ]- R [2 ,0 ], R [1 ,0 ]- R [0 ,1 ]]
1152
- if base .iszerovec (l ):
1152
+ li = np .r_ [R [2 , 1 ] - R [1 , 2 ], R [0 , 2 ] - R [2 , 0 ], R [1 , 0 ] - R [0 , 1 ]]
1153
+
1154
+ if base .iszerovec (li ):
1153
1155
# diagonal matrix case
1154
1156
if np .trace (R ) > 0 :
1155
1157
# (1,1,1) case
@@ -1158,16 +1160,18 @@ def _angle_axis(T, Td):
1158
1160
a = np .pi / 2 * (np .diag (R ) + 1 )
1159
1161
else :
1160
1162
# non-diagonal matrix case
1161
- ln = base .norm (l )
1162
- a = math .atan2 (ln , np .trace (R ) - 1 ) * l / ln
1163
-
1163
+ ln = base .norm (li )
1164
+ a = math .atan2 (ln , np .trace (R ) - 1 ) * li / ln
1165
+
1164
1166
return np .r_ [d , a ]
1165
1167
1168
+
1166
1169
def _angle_axis_sekiguchi (T , Td ):
1167
1170
d = base .transl (Td ) - base .transl (T )
1168
1171
R = base .t2r (Td ) @ base .t2r (T ).T
1169
- l = np .r_ [R [2 ,1 ]- R [1 ,2 ], R [0 ,2 ]- R [2 ,0 ], R [1 ,0 ]- R [0 ,1 ]]
1170
- if base .iszerovec (l ):
1172
+ li = np .r_ [R [2 , 1 ] - R [1 , 2 ], R [0 , 2 ] - R [2 , 0 ], R [1 , 0 ] - R [0 , 1 ]]
1173
+
1174
+ if base .iszerovec (li ):
1171
1175
# diagonal matrix case
1172
1176
if np .trace (R ) > 0 :
1173
1177
# (1,1,1) case
@@ -1176,29 +1180,33 @@ def _angle_axis_sekiguchi(T, Td):
1176
1180
# (1, -1, -1), (-1, 1, -1) or (-1, -1, 1) case
1177
1181
a = np .pi / 2 * (np .diag (R ) + 1 )
1178
1182
# as per Sekiguchi paper
1179
- if R [1 ,0 ] > 0 and R [2 ,1 ] > 0 and R [0 ,2 ] > 0 :
1180
- a = np .pi / np .sqrt (2 ) * np .sqrt (n .diag (R ) + 1 )
1181
- elif R [1 ,0 ] > 0 : # (2)
1182
- a = np .pi / np .sqrt (2 ) * np .sqrt (n .diag (R ) @ np .r_ [1 ,1 ,- 1 ] + 1 )
1183
- elif R [0 ,2 ] > 0 : # (3)
1184
- a = np .pi / np .sqrt (2 ) * np .sqrt (n .diag (R ) @ np .r_ [1 ,- 1 ,1 ] + 1 )
1185
- elif R [2 ,1 ] > 0 : # (4)
1186
- a = np .pi / np .sqrt (2 ) * np .sqrt (n .diag (R ) @ np .r_ [- 1 ,1 ,1 ] + 1 )
1183
+ if R [1 , 0 ] > 0 and R [2 , 1 ] > 0 and R [0 , 2 ] > 0 :
1184
+ a = np .pi / np .sqrt (2 ) * np .sqrt (np .diag (R ) + 1 )
1185
+ elif R [1 , 0 ] > 0 : # (2)
1186
+ a = np .pi / np .sqrt (2 ) * np .sqrt (
1187
+ np .diag (R ) @ np .r_ [1 , 1 , - 1 ] + 1 )
1188
+ elif R [0 , 2 ] > 0 : # (3)
1189
+ a = np .pi / np .sqrt (2 ) * np .sqrt (
1190
+ np .diag (R ) @ np .r_ [1 , - 1 , 1 ] + 1 )
1191
+ elif R [2 , 1 ] > 0 : # (4)
1192
+ a = np .pi / np .sqrt (2 ) * np .sqrt (
1193
+ np .diag (R ) @ np .r_ [- 1 , 1 , 1 ] + 1 )
1187
1194
else :
1188
1195
# non-diagonal matrix case
1189
- ln = base .norm (l )
1190
- a = math .atan2 (ln , np .trace (R ) - 1 ) * l / ln
1196
+ ln = base .norm (li )
1197
+ a = math .atan2 (ln , np .trace (R ) - 1 ) * li / ln
1191
1198
1192
1199
return np .r_ [d , a ]
1193
1200
1194
1201
1195
1202
if __name__ == "__main__" : # pragma nocover
1196
1203
1197
1204
import roboticstoolbox as rtb
1198
- from spatialmath import SE3
1199
-
1200
- # np.set_printoptions(linewidth=120, formatter={'float': lambda x: f"{x:9.5g}" if abs(x) > 1e-10 else f"{0:9.5g}"})
1205
+ # from spatialmath import SE3
1201
1206
1207
+ # np.set_printoptions(
1208
+ # linewidth=120, formatter={'float': lambda x: f"{x:9.5g}"
1209
+ # if abs(x) > 1e-10 else f"{0:9.5g}"})
1202
1210
1203
1211
robot = rtb .models .DH .Panda ()
1204
1212
@@ -1210,9 +1218,10 @@ def _angle_axis_sekiguchi(T, Td):
1210
1218
# print(robot.fkine(sol.q))
1211
1219
# robot.plot(sol.q)
1212
1220
1213
- # sol = robot.ikine_unc(T, costfun=lambda q: q[1] * 1e-6 if q[1] > 0 else -q[1])
1221
+ # sol = robot.ikine_unc(
1222
+ # T, costfun=lambda q: q[1] * 1e-6 if q[1] > 0 else -q[1])
1214
1223
# print(sol)
1215
1224
# print(robot.fkine(sol.q))
1216
1225
# robot.plot(sol.q)
1217
1226
1218
- sol = robot .ikine_global (T , method = 'brute' )
1227
+ sol = robot .ikine_global (T , method = 'brute' )
0 commit comments