8000 add back jacobm · naren200/robotics-toolbox-python@867d64d · GitHub
[go: up one dir, main page]
Skip to content

Commit 867d64d

Browse files
jhavljhavl
committed
add back jacobm
1 parent 4cacde2 commit 867d64d

File tree

1 file changed

+219
-124
lines changed

1 file changed

+219
-124
lines changed

‎roboticstoolbox/robot/Robot.py

Lines changed: 219 additions & 124 deletions
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@
2323

2424
import numpy as np
2525

26-
from spatialmath import SE3
26+
from spatialmath import SE3, SE2
2727
import spatialmath.base as smb
2828
from spatialmath.base.argcheck import (
2929
isvector,
@@ -57,8 +57,8 @@
5757
import roboticstoolbox as rtb
5858
from roboticstoolbox.robot.RobotKinematics import RobotKinematicsMixin
5959
from roboticstoolbox.robot.Gripper import Gripper
60-
from roboticstoolbox.robot.Link import BaseLink, Link
61-
from roboticstoolbox.robot.ETS import ETS
60+
from roboticstoolbox.robot.Link import BaseLink, Link, Link2
61+
from roboticstoolbox.robot.ETS import ETS, ETS2
6262
from roboticstoolbox.robot.ET import ET
6363
from roboticstoolbox.robot.Dynamics import DynamicsMixin
6464
from roboticstoolbox.tools import xacro
@@ -88,6 +88,10 @@
8888
# A generic type variable representing any subclass of BaseLink
8989
LinkType = TypeVar("LinkType", bound=BaseLink)
9090

91+
# ============================================================================================= #
92+
# ================= BaseRobot Class =========================================================== #
93+
# ============================================================================================= #
94+
9195

9296
class BaseRobot(SceneNode, DynamicsMixin, ABC, Generic[LinkType]):
9397
def __init__(
@@ -2178,6 +2182,11 @@ def get_link_scene_node():
21782182
# --------------------------------------------------------------------- #
21792183

21802184

2185+
# ============================================================================================= #
2186+
# ================= Robot Class =============================================================== #
2187+
# ============================================================================================= #
2188+
2189+
21812190
class Robot(BaseRobot[Link], RobotKinematicsMixin):
21822191

21832192
_color = True
@@ -3161,127 +3170,128 @@ def jacob0_dot(
31613170

31623171
return np.tensordot(H, qd, (0, 0))
31633172

3164-
# @overload
3165-
# def jacobm(
3166-
# self,
3167-
# q: ArrayLike = ...,
3168-
# J: None = None,
3169-
# H: None = None,
3170-
# end: Union[str, Link, Gripper, None] = None,
3171-
# start: Union[str, Link, Gripper, None] = None,
3172-
# axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3173-
# ):
3174-
# ...
3175-
3176-
# @overload
3177-
# def jacobm(
3178-
# self,
3179-
# q: None = None,
3180-
# J: NDArray = ...,
3181-
# H: NDArray = ...,
3182-
# end: Union[str, Link, Gripper, None] = None,
3183-
# start: Union[str, Link, Gripper, None] = None,
3184-
# axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3185-
# ):
3186-
# ...
3187-
3188-
# def jacobm(
3189-
# self,
3190-
# q=None,
3191-
# J=None,
3192-
# H=None,
3193-
# end: Union[str, Link, Gripper, None] = None,
3194-
# start: Union[str, Link, Gripper, None] = None,
3195-
# axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3196-
# ):
3197-
# r"""
3198-
# The manipulability Jacobian
3199-
3200-
# This measure relates the rate of change of the manipulability to the
3201-
# joint velocities of the robot. One of J or q is required. Supply J
3202-
# and H if already calculated to save computation time
3203-
3204-
# Parameters
3205-
# ----------
3206-
# q
3207-
# The joint angles/configuration of the robot (Optional,
3208-
# if not supplied will use the stored q values).
3209-
# J
3210-
# The manipulator Jacobian in any frame
3211-
# H
3212-
# The manipulator Hessian in any frame
3213-
# end
3214-
# the final link or Gripper which the Hessian represents
3215-
# start
3216-
# the first link which the Hessian represents
3217-
3218-
# Returns
3219-
# -------
3220-
# jacobm
3221-
# The manipulability Jacobian
3222-
3223-
# Synopsis
3224-
# --------
3225-
# Yoshikawa's manipulability measure
3226-
3227-
# .. math::
3228-
3229-
# m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
3230-
3231-
# This method returns its Jacobian with respect to configuration
3232-
3233-
# .. math::
3234-
3235-
# \frac{\partial m(\vec{q})}{\partial \vec{q}}
3236-
3237-
# References
3238-
# ----------
3239-
# - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
3240-
# Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
3241-
3242-
# """
3243-
3244-
# end, start, _ = self._get_limit_links(end, start)
3245-
3246-
# if axes == "all":
3247-
# axes = [True, True, True, True, True, True]
3248-
# elif axes.startswith("trans"):
3249-
# axes = [True, True, True, False, False, False]
3250-
# elif axes.startswith("rot"):
3251-
# axes = [False, False, False, True, True, True]
3252-
# else:
3253-
# raise ValueError("axes must be all, trans or rot")
3254-
3255-
# if J is None:
3256-
# if q is None:
3257-
# q = np.copy(self.q)
3258-
# else:
3259-
# q = getvector(q, self.n)
3260-
3261-
# J = self.jacob0(q, start=start, end=end)
3262-
# else:
3263-
# verifymatrix(J, (6, self.n))
3264-
3265-
# n = J.shape[1]
3266-
3267-
# if H is None:
3268-
# H = self.hessian0(J0=J, start=start, end=end)
3269-
# else:
3270-
# verifymatrix(H, (6, self.n, self.n))
3271-
3272-
# manipulability = self.manipulability(q, J=J, start=start, end=end, axes=axes)
3273-
3274-
# J = J[axes, :]
3275-
# H = H[:, axes, :]
3276-
3277-
# b = np.linalg.inv(J @ np.transpose(J))
3278-
# Jm = np.zeros((n, 1))
3279-
3280-
# for i in range(n):
3281-
# c = J @ np.transpose(H[i, :, :])
3282-
# Jm[i, 0] = manipulability * np.transpose(c.flatten("F")) @ b.flatten("F")
3283-
3284-
# return Jm
3173+
@overload
3174+
def jacobm(
3175+
self,
3176+
q: ArrayLike = ...,
3177+
J: None = None,
3178+
H: None = None,
3179+
end: Union[str, Link, Gripper, None] = None,
3180+
start: Union[str, Link, Gripper, None] = None,
3181+
axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3182+
) -> NDArray:
3183+
...
3184+
3185+
@overload
3186+
def jacobm(
3187+
self,
3188+
q: None = None,
3189+
J: NDArray = ...,
3190+
H: NDArray = ...,
3191+
end: Union[str, Link, Gripper, None] = None,
3192+
start: Union[str, Link, Gripper, None] = None,
3193+
axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3194+
) -> NDArray:
3195+
...
3196+
3197+
def jacobm(
3198+
s F42D elf,
3199+
q=None,
3200+
J=None,
3201+
H=None,
3202+
end: Union[str, Link, Gripper, None] = None,
3203+
start: Union[str, Link, Gripper, None] = None,
3204+
axes: Union[L["all", "trans", "rot"], List[bool]] = "all",
3205+
) -> NDArray:
3206+
r"""
3207+
The manipulability Jacobian
3208+
3209+
This measure relates the rate of change of the manipulability to the
3210+
joint velocities of the robot. One of J or q is required. Supply J
3211+
and H if already calculated to save computation time
3212+
3213+
Parameters
3214+
----------
3215+
q
3216+
The joint angles/configuration of the robot (Optional,
3217+
if not supplied will use the stored q values).
3218+
J
3219+
The manipulator Jacobian in any frame
3220+
H
3221+
The manipulator Hessian in any frame
3222+
end
3223+
the final link or Gripper which the Hessian represents
3224+
start
3225+
the first link which the Hessian represents
3226+
3227+
Returns
3228+
-------
3229+
jacobm
3230+
The manipulability Jacobian
3231+
3232+
Synopsis
3233+
--------
3234+
Yoshikawa's manipulability measure
3235+
3236+
.. math::
3237+
3238+
m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}
3239+
3240+
This method returns its Jacobian with respect to configuration
3241+
3242+
.. math::
3243+
3244+
\frac{\partial m(\vec{q})}{\partial \vec{q}}
3245+
3246+
References
3247+
----------
3248+
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I:
3249+
Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022).
3250+
3251+
"""
3252+
3253+
end, start, _ = self._get_limit_links(end, start)
3254+
3255+
if not isinstance(axes, list):
3256+
if axes == "all":
3257+
axes = [True, True, True, True, True, True]
3258+
elif axes.startswith("trans"):
3259+
axes = [True, True, True, False, False, False]
3260+
elif axes.startswith("rot"):
3261+
axes = [False, False, False, True, True, True]
3262+
else:
3263+
raise ValueError("axes must be all, trans or rot")
3264+
3265+
if J is None:
3266+
if q is None:
3267+
q = np.copy(self.q)
3268+
else:
3269+
q = getvector(q, self.n)
3270+
3271+
J = self.jacob0(q, start=start, end=end)
3272+
else:
3273+
verifymatrix(J, (6, self.n))
3274+
3275+
n = J.shape[1]
3276+
3277+
if H is None:
3278+
H = self.hessian0(J0=J, start=start, end=end)
3279+
else:
3280+
verifymatrix(H, (6, self.n, self.n))
3281+
3282+
manipulability = self.manipulability(q, J=J, start=start, end=end, axes=axes) # type: ignore
3283+
3284+
J = J[axes, :] # type: ignore
3285+
H = H[:, axes, :] # type: ignore
3286+
3287+
b = np.linalg.inv(J @ np.transpose(J))
3288+
Jm = np.zeros((n, 1))
3289+
3290+
for i in range(n):
3291+
c = J @ np.transpose(H[i, :, :])
3292+
Jm[i, 0] = manipulability * np.transpose(c.flatten("F")) @ b.flatten("F")
3293+
3294+
return Jm
32853295

32863296
# --------------------------------------------------------------------- #
32873297
# --------- PyPlot Methods -------------------------------------------- #
@@ -4545,3 +4555,88 @@ def rne(
45454555
f[jp] = f[jp] + Xup[j] * f[j]
45464556

45474557
return Q
4558+
4559+
4560+
# ============================================================================================= #
4561+
# ================= Robot2 Class ============================================================== #
4562+
# ============================================================================================= #
4563+
4564+
4565+
class Robot2(BaseRobot[Link2]):
4566+
def __init__(self, arg, **kwargs):
4567+
4568+
if isinstance(arg, ETS2):
4569+
# we're passed an ETS string
4570+
links = []
4571+
# chop it up into segments, a link frame after every joint
4572+
parent = None
4573+
for j, ets_j in enumerate(arg.split()):
4574+
elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}")
4575+
parent = elink
4576+
if (
4577+
elink.qlim is None
4578+
and elink.v is not None
4579+
and elink.v.qlim is not None
4580+
):
4581+
elink.qlim = elink.v.qlim
4582+
links.append(elink)
4583+
4584+
elif smb.islistof(arg, Link2):
4585+
links = arg
4586+
4587+
else:
4588+
raise TypeError("constructor argument must be ETS2 or list of Link2")
4589+
4590+
super().__init__(links, **kwargs)
4591+
4592+
# Should just set it to None
4593+
self.base = SE2() # override superclass
4594+
4595+
@property
4596+
def base(self) -> SE2:
4597+
"""
4598+
Get/set robot base transform (Robot superclass)
4599+
4600+
``robot.base`` is the robot base transform
4601+
4602+
Returns
4603+
-------
4604+
base
4605+
robot tool transform
4606+
4607+
- ``robot.base = ...`` checks and sets the robot base transform
4608+
4609+
Notes
4610+
-----
4611+
- The private attribute ``_base`` will be None in the case of
4612+
no base transform, but this property will return ``SE3()`` which
4613+
is an identity matrix.
4614+
"""
4615+
if self._base is None:
4616+
self._base = SE2()
4617+
4618+
# return a copy, otherwise somebody with
4619+
# reference to the base can change it
4620+
return self._base.copy()
4621+
4622+
@base.setter
4623+
def base(self, T):
4624+
if T is None:
4625+
self._base = T
4626+
elif isinstance(self, Robot2):
4627+
# 2D robot
4628+
if isinstance(T, SE2):
4629+
self._base = T
4630+
elif SE2.isvalid(T):
4631+
self._tool = SE2(T, check=True)
4632+
else:
4633+
raise ValueError("base must be set to None (no tool) or SE2")
4634+
4635+
def jacob0(self, q, start=None, end=None):
4636+
return self.ets(start, end).jacob0(q)
4637+
4638+
def jacobe(self, q, start=None, end=None):
4639+
return self.ets(start, end).jacobe(q)
4640+
4641+
def fkine(self, q, end=None, start=None):
4642+
return self.ets(start, end).fkine(q)

0 commit comments

Comments
 (0)
0