Skip to content

Commit 13e4e55

Browse files
committed
clean up
1 parent 5da75f1 commit 13e4e55

File tree

1 file changed

+69
-60
lines changed
  • roboticstoolbox/robot

1 file changed

+69
-60
lines changed

roboticstoolbox/robot/IK.py

Lines changed: 69 additions & 60 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ def ikine_mmc(
5151
# bad update
5252
# gain = gain / 2
5353
dt = dt / 2
54-
# q = q_last
54+
q = q_last
5555
# print('Down')
5656

5757
e_prev = e
@@ -204,7 +204,7 @@ def ikine_LM(
204204
distances and angles without any kind of weighting.
205205
- The inverse kinematic solution is generally not unique, and
206206
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
208208
most manipulators since it often corresponds to a
209209
kinematic singularity.
210210
- Such a solution is completely general, though much less
@@ -395,9 +395,9 @@ def ikine_LMS(
395395
and rotation about X, Y and Z respectively.
396396
:type mask: ndarray(6)
397397
:param ilimit: maximum number of iterations (default 500)
398-
:type ilimit: int
398+
:type ilimit: int
399399
:param tol: final error tolerance (default 1e-10)
400-
:type tol: float
400+
:type tol: float
401401
:param ωN: damping coefficient
402402
:type ωN: float (default 1e-3)
403403
:return: inverse kinematic solution
@@ -452,7 +452,7 @@ def ikine_LMS(
452452
distances and angles without any kind of weighting.
453453
- The inverse kinematic solution is generally not unique, and
454454
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
456456
most manipulators since it often corresponds to a
457457
kinematic singularity.
458458
- Such a solution is completely general, though much less
@@ -466,7 +466,7 @@ def ikine_LMS(
466466
467467
:references:
468468
- "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),
470470
October 2011, pp. 984-991.
471471
472472
:seealso: :func:`ikine_LM`, :func:`ikine_unc`, :func:`ikine_con`, :func:`ikine_min`
@@ -551,7 +551,6 @@ def ikine_LMS(
551551
else:
552552
return solutions
553553

554-
555554
# --------------------------------------------------------------------- #
556555

557556
# def ikine_unc(self, T, q0=None, ilimit=1000, tol=1e-16, stiffness=0, costfun=None):
@@ -617,19 +616,19 @@ def ikine_LMS(
617616
# - Can be used for robots with arbitrary degrees of freedom.
618617
# - The inverse kinematic solution is generally not unique, and
619618
# 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
621620
# most manipulators since it often corresponds to a
622621
# kinematic singularity.
623622
# - Such a solution is completely general, though much less
624623
# efficient than analytic inverse kinematic solutions derived
625624
# symbolically.
626-
# - The objective function (error) is
625+
# - The objective function (error) is
627626
# :math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
628627
# where :math:`\mat{\Omega}` is a diagonal matrix.
629628
# - Joint offsets, if defined, are accounted for in the solution.
630629

631-
# .. warning::
632-
630+
# .. warning::
631+
633632
# - The objective function is rather uncommon.
634633
# - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
635634
# uses a scalar cost-function and does not provide a Jacobian.
@@ -690,7 +689,9 @@ def ikine_LMS(
690689

691690
# --------------------------------------------------------------------- #
692691

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={}):
694695
r"""
695696
Inverse kinematics by optimization with joint limits (Robot superclass)
696697
@@ -739,8 +740,8 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
739740
``L-BFGS-B`` (Broyden–Fletcher–Goldfarb–Shanno) but for redundant
740741
robots can sometimes give poor results, pushing against the joint
741742
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
744745
vector :math:`[d,a]` with components respectively the
745746
translation error and rotation error in Euler vector form, between the
746747
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,
749750
750751
This method supports two additional costs:
751752
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
754755
arm straight
755756
- ``costfun`` add a cost given by a user-specified function ``costfun(q)``
756757
@@ -769,19 +770,19 @@ def ikine_min(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=None,
769770
- Can be used for robots with arbitrary degrees of freedom.
770771
- The inverse kinematic solution is generally not unique, and
771772
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
773774
most manipulators since it often corresponds to a
774775
kinematic singularity.
775776
- Such a solution is completely general, though much less
776777
efficient than analytic inverse kinematic solutions derived
777778
symbolically.
778-
- The objective function (error) is
779+
- The objective function (error) is
779780
:math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2`
780781
where :math:`\mat{\Omega}` is a diagonal matrix.
781782
- Joint offsets, if defined, are accounted for in the solution.
782783
783-
.. warning::
784-
784+
.. warning::
785+
785786
- The objective function is rather uncommon.
786787
- Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
787788
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,
816817
bounds = opt.Bounds(self.qlim[0, :], self.qlim[1, :])
817818

818819
if method is None:
819-
method='trust-constr'
820+
method = 'trust-constr'
820821
else:
821822
# no joint limits
822823
if method is None:
@@ -840,7 +841,7 @@ def cost(q, T, weight, costfun, stiffness):
840841
for Tk in T:
841842
res = opt.minimize(
842843
cost,
843-
q0,
844+
q0,
844845
args=(Tk.A, weight, costfun, stiffness),
845846
bounds=bounds,
846847
method=method,
@@ -849,7 +850,7 @@ def cost(q, T, weight, costfun, stiffness):
849850
)
850851

851852
# 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
853854
# error.
854855
# but 'truts-constr' is 5x slower
855856

@@ -864,7 +865,9 @@ def cost(q, T, weight, costfun, stiffness):
864865

865866
# --------------------------------------------------------------------- #
866867

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={}):
868871
r"""
869872
.. warning:: Experimental code for using SciPy global optimizers.
870873
@@ -889,20 +892,19 @@ def ikine_global(self, T, q0=None, qlim=False, ilimit=1000, tol=1e-16, method=No
889892

890893
solutions = []
891894

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]
894897

895898
optdict = {}
896899

897900
if method is None:
898-
method='differential-evolution'
901+
method = 'differential-evolution'
899902

900903
if method == 'brute':
901904
# 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])
903906
else:
904-
optdict['bounds'] = tuple([tuple(l.qlim) for l in self])
905-
907+
optdict['bounds'] = tuple([tuple(li.qlim) for li in self])
906908

907909
if method not in ['basinhopping', 'brute', 'differential_evolution',
908910
'shgo', 'dual_annealing']:
@@ -915,11 +917,10 @@ def cost(q, T, weight):
915917
e = _angle_axis(self.fkine(q).A, T) * weight
916918
return (e**2).sum()
917919

918-
for Tk in T:
920+
for _ in T:
919921
res = global_minimizer(
920922
cost,
921923
**optdict)
922-
923924

924925
solution = iksol(res.x, res.success, res.message, res.nit, res.fun)
925926
solutions.append(solution)
@@ -929,7 +930,7 @@ def cost(q, T, weight):
929930
return solutions[0]
930931
else:
931932
return solutions
932-
933+
933934
# --------------------------------------------------------------------- #
934935

935936
# def ikine_min(self, T, q0=None, pweight=1.0, stiffness=0.0,
@@ -983,12 +984,12 @@ def cost(q, T, weight):
983984
# - The inverse kinematic solution is generally not unique, and
984985
# depends on the initial guess ``q0``.
985986
# - 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
987988
# with rotation error norm.
988989
# - For a highly redundant robot ``stiffness`` can be used to impose
989990
# a smoothness contraint on joint angles, tending toward solutions
990991
# 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
992993
# most manipulators since it often corresponds to a
993994
# kinematic singularity.
994995
# - Such a solution is completely general, though much less
@@ -1000,8 +1001,8 @@ def cost(q, T, weight):
10001001
# - Joint offsets, if defined, are accounted for in the solution.
10011002
# - Joint limits become explicit bounds if 'qlimits' is set.
10021003

1003-
# .. warning::
1004-
1004+
# .. warning::
1005+
10051006
# - The objective function is rather uncommon.
10061007
# - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it
10071008
# uses a scalar cost-function and does not provide a Jacobian.
@@ -1070,7 +1071,6 @@ def cost(q, T, weight):
10701071
# else:
10711072
# return solutions
10721073

1073-
10741074
# def qmincon(self, q=None):
10751075
# """
10761076
# Move away from joint limits
@@ -1145,11 +1145,13 @@ def cost(q, T, weight):
11451145
# else:
11461146
# return qstar, success, error
11471147

1148+
11481149
def _angle_axis(T, Td):
11491150
d = base.transl(Td) - base.transl(T)
11501151
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):
11531155
# diagonal matrix case
11541156
if np.trace(R) > 0:
11551157
# (1,1,1) case
@@ -1158,16 +1160,18 @@ def _angle_axis(T, Td):
11581160
a = np.pi / 2 * (np.diag(R) + 1)
11591161
else:
11601162
# 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+
11641166
return np.r_[d, a]
11651167

1168+
11661169
def _angle_axis_sekiguchi(T, Td):
11671170
d = base.transl(Td) - base.transl(T)
11681171
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):
11711175
# diagonal matrix case
11721176
if np.trace(R) > 0:
11731177
# (1,1,1) case
@@ -1176,29 +1180,33 @@ def _angle_axis_sekiguchi(T, Td):
11761180
# (1, -1, -1), (-1, 1, -1) or (-1, -1, 1) case
11771181
a = np.pi / 2 * (np.diag(R) + 1)
11781182
# 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)
11871194
else:
11881195
# 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
11911198

11921199
return np.r_[d, a]
11931200

11941201

11951202
if __name__ == "__main__": # pragma nocover
11961203

11971204
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
12011206

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}"})
12021210

12031211
robot = rtb.models.DH.Panda()
12041212

@@ -1210,9 +1218,10 @@ def _angle_axis_sekiguchi(T, Td):
12101218
# print(robot.fkine(sol.q))
12111219
# robot.plot(sol.q)
12121220

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])
12141223
# print(sol)
12151224
# print(robot.fkine(sol.q))
12161225
# robot.plot(sol.q)
12171226

1218-
sol = robot.ikine_global(T, method='brute')
1227+
sol = robot.ikine_global(T, method='brute')

0 commit comments

Comments
 (0)