def itorque(self, q, qdd):
r"""
Inertia torque
:param q: Joint coordinates
:type q: ndarray(n)
:param qdd: Joint acceleration
:type qdd: ndarray(n)
:return: The inertia torque vector
:rtype: ndarray(n)
``itorque(q, qdd)`` is the inertia force/torque vector (n) at
the specified joint configuration q (n) and acceleration qdd (n), and
``n`` is the number of robot joints.
It is :math:`\mathbf{I}(q) \ddot{q}`.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.itorque(puma.qz, 0.5 * np.ones((6,)))
**Trajectory operation**
If ``q`` and ``qdd`` are matrices (m,n), each row is interpretted as a
joint configuration, and the result is a matrix (m,n) where each row is
the corresponding joint torques.
.. note:: If the robot model contains non-zero motor inertia then this
will be included in the result.
:seealso: :func:`inertia`
"""
q = getmatrix(q, (None, self.n))
qdd = getmatrix(qdd, (None, self.n))
if q.shape[0] != qdd.shape[0]:
raise ValueError('q and qdd must have the same number of rows')
taui = np.zeros((q.shape[0], self.n))
for k, (qk, qddk) in enumerate(zip(q, qdd)):
taui[k, :] = self.rne(qk,
np.zeros(self.n),
qddk,
gravity=[0, 0, 0])
if q.shape[0] == 1:
return taui[0, :]
else:
return taui
def plot_sphere(radius, centre=(0, 0, 0), pose=None, resolution=50, ax=None, **kwargs):
"""
Plot a sphere using matplotlib
:param centre: centre of sphere, defaults to (0, 0, 0)
:type centre: array_like(3), ndarray(3,N), optional
:param radius: radius of sphere, defaults to 1
:type radius: float, optional
:param resolution: number of points on circumferece, defaults to 50
:type resolution: int, optional
:param pose: pose of sphere, defaults to None
:type pose: SE3, optional
:param ax: axes to draw into, defaults to None
:type ax: Axes3D, optional
:param filled: draw filled polygon, else wireframe, defaults to False
:type filled: bool, optional
:param kwargs: arguments passed to ``plot_wireframe`` or ``plot_surface``
:return: matplotlib collection
:rtype: list of Line3DCollection or Poly3DCollection
Plot one or more spheres. If ``centre`` is a 3xN array, then each column is
taken as the centre of a sphere. All spheres have the same radius, color
etc.
Example:
.. runblock:: pycon
>>> from spatialmath.base import plot_sphere
>>> plot_sphere(1, 'r') # red sphere wireframe
>>> plot_sphere(1, centre=(1,1,1), filled=True, facecolor='b')
:seealso: :func:`~matplotlib.pyplot.plot_surface`, :func:`~matplotlib.pyplot.plot_wireframe`
"""
ax = axes_logic(ax, 3)
centre = base.getmatrix(centre, (3, None))
handles = []
for c in centre.T:
X, Y, Z = sphere(centre=c, radius=radius, resolution=resolution)
if pose is not None:
xc = X.reshape((-1,))
yc = Y.reshape((-1,))
zc = Z.reshape((-1,))
xyz = np.array((xc, yc, zc))
xyz = pose * xyz
X = xyz[0, :].reshape(x.shape)
Y = xyz[1, :].reshape(y.shape)
Z = xyz[2, :].reshape(z.shape)
handles.append(_render3D(ax, X, Y, Z, **kwargs))
return handles
def inertia(self, q=None):
"""
DHRobot.INERTIA Manipulator inertia matrix
:param q: Joint coordinates
:type q: ndarray(n)
:return: The inertia matrix
:rtype: ndarray(n,n)
``inertia(q)`` is the symmetric joint inertia matrix (n,n) which
relates joint torque to joint acceleration for the robot at joint
configuration q.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.inertia(puma.qz)
**Trajectory operation**
If ``q`` is a matrix (m,n), each row is interpretted as a joint state
vector, and the result is a 3d-matrix (nxnxk) where each plane
corresponds to the inertia for the corresponding row of q.
.. note::
- The diagonal elements ``I[j,j]`` are the inertia seen by joint
actuator ``j``.
- The off-diagonal elements ``I[j,k]`` are coupling inertias that
relate acceleration on joint ``j`` to force/torque on
joint ``k``.
- The diagonal terms include the motor inertia reflected through
the gear ratio.
:seealso: :func:`cinertia`
"""
q = getmatrix(q, (None, self.n))
In = np.zeros((q.shape[0], self.n, self.n))
for k, qk in enumerate(q):
In[k, :, :] = self.rne(
(np.c_[qk] @ np.ones((1, self.n))).T,
np.zeros((self.n, self.n)),
np.eye(self.n),
gravity=[0, 0, 0])
if q.shape[0] == 1:
return In[0, :, :]
else:
return In
def __init__(self, m=None, r=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param r: centre of mass relative to link frame
:type r: 3-element array_like
:param I: inertia about the centre of mass, axes aligned with link frame
:type I: numpy.array, shape=(6,6)
- ``SpatialInertia(m, r I)`` is a spatial inertia object for a rigid-body
with mass ``m``, centre of mass at ``r`` relative to the link frame, and an
inertia matrix ``I`` (3x3) about the centre of mass.
- ``SpatialInertia(I)`` is a spatial inertia object with a value equal
to ``I`` (6x6).
:SymPy: supported
"""
super().__init__()
if m is None and r is None and I is None:
# no arguments
I = SpatialInertia._identity()
elif m is not None and r is None and I is None and base.ismatrix(
m, (6, 6)):
I = base.getmatrix(m, (6, 6))
elif m is not None and r is not None:
r = base.getvector(r, 3)
if I is None:
I = np.zeros((3, 3))
else:
I = base.getmatrix(I, (3, 3))
C = base.skew(r)
M = np.diag((m, ) * 3) # sym friendly
I = np.block([[M, m * C.T], [m * C, I + m * C @ C.T]])
else:
raise ValueError('bad values')
self.data = [I]
def colorconvert(image, src, dst):
flag = _convertflag(src, dst)
if isinstance(image, np.ndarray) and image.ndim == 3:
# its a color image
return cv.cvtColor(image, flag)
elif base.ismatrix(image, (None, 3)):
# not an image, see if it's Nx3
image = base.getmatrix(image, (None, 3), dtype=np.float32)
image = image.reshape((-1, 1, 3))
return cv.cvtColor(image, flag).reshape((-1, 3))
def gravload(self, q=None, gravity=None):
"""
Compute gravity load
:param q: Joint coordinates
:type q: ndarray(n)
:param gravity: Gravitational acceleration (Optional, if not supplied will
use the stored gravity values).
:type gravity: ndarray(3)
:return: The generalised joint force/torques due to gravity
:rtype: ndarray(n)
``robot.gravload(q)`` calculates the joint gravity loading (n) for
the robot in the joint configuration ``q`` and using the default
gravitational acceleration specified in the DHRobot object.
``robot.gravload(q, gravity=g)`` as above except the gravitational
acceleration is explicitly specified as ``g``.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.gravload(puma.qz)
**Trajectory operation**
If q is a matrix (nxm) each column is interpreted as a joint
configuration vector, and the result is a matrix (nxm) each column
being the corresponding joint torques.
"""
q = getmatrix(q, (None, self.n))
if gravity is None:
gravity = self.gravity
else:
gravity = getvector(gravity, 3)
taug = np.zeros((q.shape[0], self.n))
z = np.zeros(self.n)
for k, qk in enumerate(q):
taug[k, :] = self.rne(qk, z, z, gravity=gravity)
if q.shape[0] == 1:
return taug[0, :]
else:
return taug
def cinertia(self, q=None):
r"""
Cartesian manipulator inertia matrix
:param q: Joint coordinates
:type q: ndarray(n)
:return: The inertia matrix
:rtype: ndarray(6,6)
``robot.cinertia(q)`` is the Cartesian (operational space) inertia
matrix which relates Cartesian force/torque to Cartesian
acceleration at the joint configuration q.
:math:`\mathbf{M} = {\mathbf{J}(q)^+}^T \mathbf{I}(q) \mathbf{J}(q)^+
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.cinertia(puma.qz)
**Trajectory operation**
If ``q`` is a matrix (m,n), each row is interpretted as a joint state
vector, and the result is a 3d-matrix (n,n,m) where each plane
corresponds to the Cartesian inertia for the corresponding
row of ``q``.
.. warning:: Assumes that the operational space has 6 DOF.
:seealso: :func:`inertia`
"""
q = getmatrix(q, (None, self.n))
Mt = np.zeros((q.shape[0], 6, 6))
for k, qk in enumerate(q):
J = self.jacob0(qk)
Ji = np.linalg.pinv(J)
M = self.inertia(qk)
Mt[k, :, :] = Ji.T @ M @ Ji
if q.shape[0] == 1:
return Mt[0, :, :]
else:
return Mt
def plot_homline(lines, *args, ax=None, **kwargs):
"""
Plot a homogeneous line using matplotlib
:param lines: homgeneous lines
:type lines: array_like(3), ndarray(3,N)
:param ax: axes to plot in, defaults to ``gca()``
:type ax: Axis, optional
:param kwargs: arguments passed to ``plot``
:return: matplotlib object
:rtype: list of Line2D instances
Draws the 2D line given in homogeneous form :math:`\ell[0] x + \ell[1] y + \ell[2] = 0` in the current
2D axes.
.. warning: A set of 2D axes must exist in order that the axis limits can
be obtained. The line is drawn from edge to edge.
If ``lines`` is a 3xN array then ``N`` lines are drawn, one per column.
Example:
.. runblock:: pycon
>>> from spatialmath.base import plotvol2, plot_homline
>>> plotvol2(5)
>>> plot_homline((1, -2, 3))
>>> plot_homline((1, -2, 3), 'k--') # dashed black line
"""
ax = axes_logic(ax, 2)
# get plot limits from current graph
xlim = np.r_[ax.get_xlim()]
ylim = np.r_[ax.get_ylim()]
lines = base.getmatrix(lines, (None, 3))
handles = []
for line in lines:
if abs(line[1]) > abs(line[0]):
y = (-line[2] - line[0] * xlim) / line[1]
ax.plot(xlim, y, *args, **kwargs)
else:
x = (-line[2] - line[1] * ylim) / line[0]
handles.append(ax.plot(x, ylim, *args, **kwargs))
return handles
def project(self, P, pose=None, objpose=None):
"""
Central projection for now
P world points or image plane points in column vectors only
"""
P = getmatrix(P, (3,None))
if P.shape[0] == 3:
# for 3D world points
if pose is None:
C = self.C
else:
C = self.getC(SE3(pose))
ip = h2e(C @ e2h(P))
elif P.shape[0] == 2:
# for 2D imageplane points
ip = P
return ip
def plot_circle(
radius, *fmt, centre=(0, 0), resolution=50, ax=None, filled=False, **kwargs
):
"""
Plot a circle using matplotlib
:param centre: centre of circle, defaults to (0,0)
:type centre: array_like(2), optional
:param args:
:param radius: radius of circle
:type radius: float
:param resolution: number of points on circumferece, defaults to 50
:type resolution: int, optional
:return: the matplotlib object
:rtype: list of Line2D or Patch.Polygon
Plot or more circles. If ``centre`` is a 3xN array, then each column is
taken as the centre of a circle. All circles have the same radius, color
etc.
Example:
.. runblock:: pycon
>>> from spatialmath.base import plotvol2, plot_circle
>>> plotvol2(5)
>>> plot_circle(1, 'r') # red circle
>>> plot_circle(2, 'b--') # blue dashed circle
>>> plot_circle(0.5, filled=True, facecolor='y') # yellow filled circle
"""
centres = base.getmatrix(centre, (2, None))
ax = axes_logic(ax, 2)
handles = []
for centre in centres.T:
xy = circle(centre, radius, resolution)
if filled:
patch = plt.Polygon(xy.T, **kwargs)
handles.append(ax.add_patch(patch))
else:
handles.append(ax.plot(xy[0, :], xy[1, :], *fmt, **kwargs))
return handles
def visjac_p(self, uv, Z):
'''
Image Jacobian for point features (interaction matrix)
Returns a 2Nx6 matrix of stacked Jacobians, one per image-plane point.
uv is a 2xN matrix of image plane points
Z is the depth of the corresponding world points. Can be scalar, same distance to every
point, or a vector or list of length N.
References:
* A tutorial on Visual Servo Control", Hutchinson, Hager & Corke,
IEEE Trans. R&A, Vol 12(5), Oct, 1996, pp 651-670.
* Robotics, Vision & Control, Corke, Springer 2017, Chap 15.
'''
uv = base.getmatrix(uv, (2, None))
Z = base.getvector(Z)
if len(Z) == 1:
Z = np.repeat(Z, uv.shape[1])
elif len(Z) != uv.shape[1]:
raise ValueError('Z must be a scalar or have same number of columns as uv')
L = np.empty((0, 6)) # empty matrix
K = self.K
Kinv = np.linalg.inv(K)
for z, p in zip(Z, uv.T): # iterate over each column (point)
# convert to normalized image-plane coordinates
x, y, _ = Kinv @ base.e2h(p)
# 2x6 Jacobian for this point
Lp = K[:2,:2] @ np.array(
[ [-1/z, 0, x/z, x * y, -(1 + x**2), y],
[ 0, -1/z, y/z, (1 + y**2), -x*y, -x] ])
# stack them vertically
L = np.vstack([L, Lp])
return L
def rne_dh(self,
Q,
QD=None,
QDD=None,
grav=None,
fext=None,
debug=False,
basewrench=False):
"""
Compute inverse dynamics via recursive Newton-Euler formulation
:param Q: Joint coordinates
:param QD: Joint velocity
:param QDD: Joint acceleration
:param grav: [description], defaults to None
:type grav: [type], optional
:param fext: end-effector wrench, defaults to None
:type fext: 6-element array-like, optional
:param debug: print debug information to console, defaults to False
:type debug: bool, optional
:param basewrench: compute the base wrench, defaults to False
:type basewrench: bool, optional
:raises ValueError: for misshaped inputs
:return: Joint force/torques
:rtype: NumPy array
Recursive Newton-Euler for standard Denavit-Hartenberg notation.
- ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is the
number of robot joints. The result has shape (n,).
- ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n is the
number of robot joints and where m is the number of steps in the joint
trajectory. The result has shape (m,n).
- ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with
shape (3n,), and the result has shape (n,).
- ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with
shape (m,3n) and the result has shape (m,n).
.. notes::
- this is a pure Python implementation and slower than the .rne() which
is written in C.
- this version supports symbolic model parameters
- verified against MATLAB code
"""
def removesmall(x):
return x
n = self.n
if self.symbolic:
dtype = 'O'
else:
dtype = None
z0 = np.array([0, 0, 1], dtype=dtype)
if grav is None:
grav = self.gravity # default gravity from the object
else:
grav = getvector(grav, 3)
if fext is None:
fext = np.zeros((6, ), dtype=dtype)
else:
fext = getvector(fext, 6)
if debug:
print('grav', grav)
print('fext', fext)
# unpack the joint coordinates and derivatives
if Q is not None and QD is None and QDD is None:
# single argument case
Q = getmatrix(Q, (None, self.n * 3))
q = Q[:, 0:n]
qd = Q[:, n:2 * n]
qdd = Q[:, 2 * n:]
else:
# 3 argument case
q = getmatrix(Q, (None, self.n))
qd = getmatrix(QD, (None, self.n))
qdd = getmatrix(QDD, (None, self.n))
nk = q.shape[0]
tau = np.zeros((nk, n), dtype=dtype)
if basewrench:
wbase = np.zeros((nk, n), dtype=dtype)
for k in range(nk):
# take the k'th row of data
q_k = q[k, :]
qd_k = qd[k, :]
qdd_k = qdd[k, :]
if debug:
print('q_k', q_k)
print('qd_k', qd_k)
print('qdd_k', qdd_k)
print()
# joint vector quantities stored columwise in matrix
# m suffix for matrix
Fm = np.zeros((3, n), dtype=dtype)
Nm = np.zeros((3, n), dtype=dtype)
# if robot.issym
# pstarm = sym([]);
# else
# pstarm = [];
pstarm = np.zeros((3, n), dtype=dtype)
Rm = []
# rotate base velocity and acceleration into L1 frame
Rb = t2r(self.base.A).T
w = Rb @ np.zeros(
(3, ), dtype=dtype) # base has zero angular velocity
wd = Rb @ np.zeros(
(3, ), dtype=dtype) # base has zero angular acceleration
vd = Rb @ grav
# ---------------- initialize some variables -------------------- #
for j in range(n):
link = self.links[j]
# compute the link rotation matrix
if link.sigma == 0:
# revolute axis
Tj = link.A(q_k[j]).A
d = link.d
else:
# prismatic
Tj = link.A(link.theta).A
d = q_k[j]
Rm.append(t2r(Tj))
# compute pstar:
# O_{j-1} to O_j in {j}, negative inverse of link xform
alpha = link.alpha
pstarm[:, j] = np.r_[link.a, d * sym.sin(alpha),
d * sym.cos(alpha)]
# ----------------- the forward recursion ----------------------- #
for j, link in enumerate(self.links):
Rt = Rm[j].T # transpose!!
pstar = pstarm[:, j]
r = link.r
# statement order is important here
if link.isrevolute() == 0:
# revolute axis
wd = Rt @ (wd + z0 * qdd_k[j] \
+ _cross(w, z0 * qd_k[j]))
w = Rt @ (w + z0 * qd_k[j])
vd = _cross(wd, pstar) \
+ _cross(w, _cross(w, pstar)) \
+ Rt @ vd
else:
# prismatic axis
w = Rt @ w
wd = Rt @ wd
vd = Rt @ (z0 * qdd_k[j] + vd) \
+ _cross(wd, pstar) \
+ 2 * _cross(w, Rt @ z0 * qd_k[j]) \
+ _cross(w, _cross(w, pstar))
vhat = _cross(wd, r) \
+ _cross(w, _cross(w, r)) \
+ vd
Fm[:, j] = link.m * vhat
Nm[:, j] = link.I @ wd + _cross(w, link.I @ w)
if debug:
print('w: ', removesmall(w))
print('wd: ', removesmall(wd))
print('vd: ', removesmall(vd))
print('vdbar: ', removesmall(vhat))
print()
if debug:
print('Fm\n', Fm)
print('Nm\n', Nm)
# ----------------- the backward recursion ----------------------- #
f = fext[:3] # force/moments on end of arm
nn = fext[3:]
for j in range(n - 1, -1, -1):
link = self.links[j]
pstar = pstarm[:, j]
#
# order of these statements is important, since both
# nn and f are functions of previous f.
#
if j == (n - 1):
R = np.eye(3, dtype=dtype)
else:
R = Rm[j + 1]
r = link.r
nn = R @ (nn + _cross(R.T @ pstar, f)) \
+ _cross(pstar + r, Fm[:,j]) \
+ Nm[:,j]
f = R @ f + Fm[:, j]
if debug:
print('f: ', removesmall(f))
print('n: ', removesmall(nn))
R = Rm[j]
if link.isrevolute():
# revolute axis
t = nn @ (R.T @ z0)
else:
# prismatic
t = f @ (R.T @ z0)
# add joint inertia and friction
# no Coulomb friction if model is symbolic
tau[k, j] = t \
+ link.G ** 2 * link.Jm * qdd_k[j] \
- link.friction(qd_k[j], coulomb=not self.symbolic)
if debug:
print(
f'j={j:}, G={link.G:}, Jm={link.Jm:}, friction={link.friction(qd_k[j], coulomb=False):}'
)
print()
# compute the base wrench and save it
if basewrench:
R = Rm[0]
nn = R @ nn
f = R @ f
wbase[k, :] = np.r_[f, nn]
if self.symbolic:
# simplify symbolic expressions
print('start simplification')
t0 = time()
from sympy import trigsimp
tau = trigsimp(tau)
# consider using multiprocessing to spread over cores
# https://stackoverflow.com/questions/33844085/using-multiprocessing-with-sympy
print(f'done simplification after {time() - t0:} sec')
if tau.shape[0] == 1:
return tau.flatten()
else:
return tau
if tau.shape[0] == 1:
return tau #.flatten()
else:
return tau
def coriolis(self, q, qd):
r"""
Coriolis and centripetal term
:param q: Joint coordinates
:type q: ndarray(n)
:param qd: Joint velocity
:type qd: ndarray(n)
:return: Velocity matrix
:rtype: ndarray(n,n)
``coriolis(q, qd)`` calculates the Coriolis/centripetal matrix (n,n)
for the robot in configuration ``q`` and velocity ``qd``, where ``n``
is the number of joints.
The product :math:`\mathbf{C} \dot{q}` is the vector of joint
force/torque due to velocity coupling. The diagonal elements are due to
centripetal effects and the off-diagonal elements are due to Coriolis
effects. This matrix is also known as the velocity coupling matrix,
since it describes the disturbance forces on any joint due to
velocity of all other joints.
Example:
.. runblock:: pycon
>>> import roboticstoolbox as rtb
>>> puma = rtb.models.DH.Puma560()
>>> puma.coriolis(puma.qz, 0.5 * np.ones((6,)))
**Trajectory operation**
If ``q`` and `qd` are matrices (m,n), each row is interpretted as a
joint configuration, and the result (n,n,m) is a 3d-matrix where
each plane corresponds to a row of ``q`` and ``qd``.
.. note::
- Joint viscous friction is also a joint force proportional to
velocity but it is eliminated in the computation of this value.
- Computationally slow, involves :math:`n^2/2` invocations of RNE.
"""
q = getmatrix(q, (None, self.n))
qd = getmatrix(qd, (None, self.n))
if q.shape[0] != qd.shape[0]:
raise ValueError('q and qd must have the same number of rows')
# ensure that friction doesn't enter the mix, it's also a velocity
# dependent force/torque
r1 = self.nofriction(True, True)
C = np.zeros((q.shape[0], self.n, self.n))
Csq = np.zeros((q.shape[0], self.n, self.n))
# Find the torques that depend on a single finite joint speed,
# these are due to the squared (centripetal) terms
# set QD = [1 0 0 ...] then resulting torque is due to qd_1^2
for k, qk in enumerate(q):
for i in range(self.n):
QD = np.zeros(self.n)
QD[i] = 1
tau = r1.rne(qk, QD, np.zeros(self.n), grav=[0, 0, 0])
Csq[k, :, i] = Csq[k, :, i] + tau
# Find the torques that depend on a pair of finite joint speeds,
# these are due to the product (Coriolis) terms
# set QD = [1 1 0 ...] then resulting torque is due to
# qd_1 qd_2 + qd_1^2 + qd_2^2
for k, (qk, qdk) in enumerate(zip(q, qd)):
for i in range(self.n):
for j in range(i + 1, self.n):
# Find a product term qd_i * qd_j
QD = np.zeros(self.n)
QD[i] = 1
QD[j] = 1
tau = r1.rne(qk, QD, np.zeros(self.n), grav=[0, 0, 0])
C[k, :, j] = C[k, :, j] + \
(tau - Csq[k, :, j] - Csq[k, :, i]) * qdk[i] / 2
C[k, :, i] = C[k, :, i] + \
(tau - Csq[k, :, j] - Csq[k, :, i]) * qdk[j] / 2
C[k, :, :] = C[k, :, :] + Csq[k, :, :] @ np.diag(qdk)
if q.shape[0] == 1:
return C[0, :, :]
else:
return C
def project(self, P, pose=None, objpose=None, visibility=False):
"""
Project world points to image plane
:param P: 3D points to project into camera image plane
:type P: array_like(3), array_like(3,n)
:param pose: camera pose with respect to the world frame, defaults to
camera's ``pose`` attribute
:type pose: SE3, optional
:param objpose: 3D point reference frame, defaults to world frame
:type objpose: SE3, optional
:param visibility: test if points are visible, default False
:type visibility: bool
:raises ValueError: [description]
:return: image plane points
:rtype: ndarray(2,n)
If ``pose`` is specified it is used for the camera pose instead of the
attribute ``pose``. The objects attribute is not updated.
The points ``P`` are by default with respect to the world frame, but
they can be transformed
If points are behind the camera, the image plane points are set to
NaN.
if ``visibility`` is True then check whether the projected point lies in
the bounds of the image plane. Return two values: the image plane
coordinates and an array of booleans indicating if the corresponding
point is visible.
If ``P`` is a Plucker object, then each value is projected into a
2D line in homogeneous form :math:`p[0] u + p[1] v + p[2] = 0`.
"""
P = base.getmatrix(P, (3,None))
if pose is None:
pose = self.pose
C = self.getC(pose)
if isinstance(P, np.ndarray):
# project 3D points
if objpose is not None:
P = objpose * P
x = C @ base.e2h(P)
x[2,x[2,:]<0] = np.nan # points behind the camera are set to NaN
x = base.h2e(x)
# if self._distortion is not None:
# x = self.distort(x)
# if self._noise is not None:
# # add Gaussian noise with specified standard deviation
# x += np.diag(self._noise) * np.random.randn(x.shape)
# do visibility check if required
if visibility:
visible = ~np.isnan(x[0,:]) \
& (x[0,:] >= 0) \
& (x[1,:] >= 0) \
& (x[0,:] < self.nu) \
& (x[1,:] < self.nv)
return x, visibility
else:
return x
elif isinstance(P, Plucker):
# project Plucker lines
x = np.empty(shape=(3, 0))
for p in P:
l = base.vex( C * p.skew * C.T)
x = np.c_[x, l / np.max(np.abs(l))] # normalize by largest element
return x
