Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions docs/source/conf.py
Original file line number Diff line number Diff line change
Expand Up @@ -198,6 +198,7 @@
:format: html
.. |BlockOptions| replace:: :raw-html:`<a href="https://petercorke.github.io/bdsim/internals.html?highlight=block%20__init__#bdsim.Block.__init__">common Block options</a>`
.. |GraphicsBlockOptions| replace:: :raw-html:`<a href="https://petercorke.github.io/bdsim/internals.html?highlight=graphicsblock%20__init__#bdsim.GraphicsBlock.__init__">common GraphicsBlock options</a>`
.. |ikargs| replace:: additional keyword arguments accepted by the underlying numerical IK solver -- ``ilimit``, ``slimit``, ``tol``, ``mask``, ``joint_limits``, ``seed``, ``k``, ``method``, ``kq``, ``km`` -- see :meth:`~roboticstoolbox.ETS.ikine_LM` for details of each
"""

# -------- Suppress common noisy warnings ----------------------------------------#
Expand Down
28 changes: 11 additions & 17 deletions examples/ikine_evaluate.py
Original file line number Diff line number Diff line change
Expand Up @@ -31,31 +31,25 @@
# - the statement to execute for timeit
ikfuncs = [
(
robot.ikine_LM, # Levenberg-Marquadt
lambda T, q0: robot.ikine_LM(T, q0=q0), # Levenberg-Marquadt
"ikine_LM",
"sol = robot.ikine_LM(T, q0)",
"sol = robot.ikine_LM(T, q0=q0)",
),
(
robot.ikine_LMS, # Levenberg-Marquadt (Sugihara)
"ikine_LMS",
"sol = robot.ikine_LMS(T, q0)",
lambda T, q0: robot.ikine_LM(T, q0=q0, method="sugihara", k=0.0001),
"ikine_LM (sugihara)",
"sol = robot.ikine_LM(T, q0=q0, method='sugihara', k=0.0001)",
),
(
robot.ikine_min, # numerical solution with no constraints
"ikine_min(qlim=False)",
"sol = robot.ikine_min(T, q0)",
lambda T, q0: robot.ikine_LM(T, q0=q0, joint_limits=False),
"ikine_LM(qlim=False)",
"sol = robot.ikine_LM(T, q0=q0, joint_limits=False)",
),
(
lambda T, q0: robot.ikine_min(
T, q0, qlim=True
), # numerical solution with constraints
"ikine_min(qlim=True)",
"sol = robot.ikine_min(T, q0, qlim=True)",
lambda T, q0: robot.ikine_LM(T, q0=q0, joint_limits=True),
"ikine_LM(qlim=True)",
"sol = robot.ikine_LM(T, q0=q0, joint_limits=True)",
),
# (robot.ikine_mmc, #numerical solution with no constraints
# "ikine_min(qlim=False)",
# "sol = robot.ikine_min(T, q0)"
# ),
]
if hasattr(robot, "ikine_a"):
a = (
Expand Down
4 changes: 3 additions & 1 deletion examples/readme.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@
# IK

T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1])
sol = robot.ikine_LMS(T) # solve IK, ignore additional outputs
sol = robot.ikine_LM(
T, method="sugihara", k=0.0001
) # solve IK, ignore additional outputs
print(sol.q) # display joint angles
# FK shows that desired end-effector pose was achieved
print(robot.fkine(sol.q))
Expand Down
33 changes: 17 additions & 16 deletions src/roboticstoolbox/robot/DHRobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -2526,23 +2526,24 @@ def ikine_LM(
self,
Tep: np.ndarray | SE3,
q0: ArrayLike | None = None,
ilimit: int = 30,
slimit: int = 100,
tol: float = 1e-6,
joint_limits: bool = False,
mask: ArrayLike | None = None,
seed: int | None = None,
**ikargs,
):
return self.ets().ikine_LM(
Tep=Tep,
q0=q0,
ilimit=ilimit,
slimit=slimit,
tol=tol,
joint_limits=joint_limits,
mask=mask,
seed=seed,
)
"""
Numerical inverse kinematics by Levenberg-Marquadt optimization

:param Tep: The desired end-effector pose
:param q0: The initial joint coordinate vector
:param ikargs: |ikargs|
:returns: An IKSolution containing joint coordinates ``q``, ``success`` flag, ``iterations``, ``searches``, ``residual`` error value, and ``reason`` string if applicable
:rtype: IKSolution

This is a thin wrapper around :meth:`ETS.ikine_LM` -- see that
method for the full description of the Levenberg-Marquadt solver
and its ``method``/``kq``/``km`` variants.

.. seealso:: :meth:`ikine_LM` (:class:`~roboticstoolbox.ETS`)
"""
return self.ets().ikine_LM(Tep=Tep, q0=q0, **ikargs)


class SerialLink(DHRobot):
Expand Down
16 changes: 10 additions & 6 deletions tests/test_DHRobot.py
Original file line number Diff line number Diff line change
Expand Up @@ -1029,14 +1029,18 @@ def test_ikine_LM(self):
self.assertTrue(sol.success)
self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=4)

# def test_ikine_LMS(self):
# puma = rp.models.DH.Puma560()
def test_ikine_LM_ikargs_forwarding(self):
# DHRobot.ikine_LM forwards **ikargs straight through to
# ETS.ikine_LM -- confirm a non-default method/k actually reaches
# the underlying solver (regression: DHRobot.ikine_LM used to have
# its own narrow signature that silently dropped method/k/kq/km).
puma = rp.models.DH.Puma560()

# T = puma.fkine(puma.qn)
T = puma.fkine(puma.qn)

# sol = puma.ikine_LM(T)
# self.assertTrue(sol.success)
# self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=6)
sol = puma.ikine_LM(T, method="sugihara", k=0.0001, tol=1e-10, seed=0)
self.assertTrue(sol.success)
self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=4)

# def test_ikine_unc(self):
# puma = rp.models.DH.Puma560()
Expand Down
Loading