def jacobm(self, q=None, J=None, H=None, from_link=None, to_link=None):
"""
Calculates the manipulability Jacobian. This measure relates the rate
of change of the manipulability to the joint velocities of the robot.
One of J or q is required. Supply J and H if already calculated to
save computation time
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:param J: The manipulator Jacobian in any frame
:type J: float ndarray(6,n)
:param H: The manipulator Hessian in any frame
:type H: float ndarray(6,n,n)
:return: The manipulability Jacobian
:rtype: float ndarray(n)
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
if from_link is None:
from_link = self.base_link
if to_link is None:
to_link = self.ee_links[0]
path, n = self.get_path(from_link, to_link)
if J is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, n)
J = self.jacob0(q, from_link, to_link)
else:
verifymatrix(J, (6, n))
if H is None:
H = self.hessian0(J0=J, from_link=from_link, to_link=to_link)
else:
verifymatrix(H, (6, n, n))
manipulability = self.manipulability(J=J,
from_link=from_link,
to_link=to_link)
b = np.linalg.inv(J @ np.transpose(J))
Jm = np.zeros((n, 1))
for i in range(n):
c = J @ np.transpose(H[:, :, i])
Jm[i, 0] = manipulability * \
np.transpose(c.flatten('F')) @ b.flatten('F')
return Jm
def friction(self, qd):
r"""
Manipulator joint friction (Robot superclass)
:param qd: The joint velocities of the robot
:type qd: ndarray(n)
:return: The joint friction forces/torques for the robot
:rtype: ndarray(n,)
``robot.friction(qd)`` is a vector of joint friction
forces/torques for the robot moving with joint velocities ``qd``.
The friction model includes:
- Viscous friction which is a linear function of velocity.
- Coulomb friction which is proportional to sign(qd).
.. math::
\tau_j = G^2 B \dot{q}_j + |G_j| \left\{ \begin{array}{ll}
\tau_{C,j}^+ & \mbox{if $\dot{q}_j > 0$} \\
\tau_{C,j}^- & \mbox{if $\dot{q}_j < 0$} \end{array} \right.
.. note::
- The friction value should be added to the motor output torque to
determine the nett torque. It has a negative value when qd > 0.
- The returned friction value is referred to the output of the
gearbox.
- The friction parameters in the Link object are referred to the
motor.
- Motor viscous friction is scaled up by :math:`G^2`.
- Motor Coulomb friction is scaled up by math:`G`.
- The appropriate Coulomb friction value to use in the
non-symmetric case depends on the sign of the joint velocity,
not the motor velocity.
- Coulomb friction is zero for zero joint velocity, stiction is
not modeled.
- The absolute value of the gear ratio is used. Negative gear
ratios are tricky: the Puma560 robot has negative gear ratio for
joints 1 and 3.
- The absolute value of the gear ratio is used. Negative gear
ratios are tricky: the Puma560 has negative gear ratio for
joints 1 and 3.
:seealso: :func:`Robot.nofriction`, :func:`Link.friction`
"""
qd = getvector(qd, self.n)
tau = np.zeros(self.n)
for i in range(self.n):
tau[i] = self.links[i].friction(qd[i])
return tau
def Planes(pi1, pi2):
"""
Plucker.planes Create Plucker line from two planes
P = Plucker.planes(PI1, PI2) is a Plucker object that represents
the line formed by the intersection of two planes PI1, PI2 (each 4x1).
Notes::
- Planes are given by the 4-vector [a b c d] to represent ax+by+cz+d=0.
"""
if not isinstance(p11, Plane):
pi1 = Plane(arg.getvector(pi1, 4))
if not isinstance(p12, Plane):
pi2 = Plane(arg.getvector(pi2, 4))
w = np.cross(pi1.n, pi2.n)
v = pi2.d * pi1.n - pi1.d * pi2.n
return Plucker(np.r_[v, w])
def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'):
"""
Create an SO(3) rotation matrix from roll-pitch-yaw angles
:param roll: roll angle
:type roll: float
:param pitch: pitch angle
:type pitch: float
:param yaw: yaw angle
:type yaw: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 3x3 rotation matrix
:rtype: numpdy.ndarray, shape=(3,3)
- ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
(3x3) equivalent to the specified roll, pitch, yaw angles angles.
These correspond to successive rotations about the axes specified by ``order``:
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. Convention for a camera with z-axis parallel
to the optic axis and x-axis parallel to the pixel rows.
- ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
from ``RPY`` which is a 3-vector (array_like) with values
(ROLL, PITCH, YAW).
:seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy`
"""
if np.isscalar(roll):
angles = [roll, pitch, yaw]
else:
angles = argcheck.getvector(roll, 3)
angles = argcheck.getunit(angles, unit)
if order == 'xyz' or order == 'arm':
R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
elif order == 'zyx' or order == 'vehicle':
R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
elif order == 'yxz' or order == 'camera':
R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])
else:
raise ValueError('Invalid angle order')
return R
def __init__(self, s: Any = None, v=None, check=True, norm=True):
"""
A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
A quaternion can be considered as a rotation about a vector in space where
q = cos (theta/2) sin(theta/2) <vx vy vz>
where <vx vy vz> is a unit vector.
:param s: scalar
:param v: vector
"""
super().__init__()
if s is None and v is None:
self.data = [np.array([0, 0, 0, 0])]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
self.data = [np.r_[s, argcheck.getvector(v)]]
elif argcheck.isvector(s, 4):
self.data = [argcheck.getvector(s)]
elif isinstance(s, list):
if isinstance(s[0], np.ndarray):
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, self.__class__),
s)), 'all elements of list must have same type'
self.data = [x._A for x in s]
else:
raise ValueError('incorrect list')
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
self.data = [x for x in s]
elif isinstance(s, Quaternion):
self.data = s.data
else:
raise ValueError('bad argument to Quaternion constructor')
def point(L, lam):
"""
Plucker.point Generate point on line
``P = self.point(LAMBDA)`` is a point on the line, where LAMBDA is the parametric
distance along the line from the principal point of the line P = PP + self.UW*LAMBDA.
See also Plucker.pp, Plucker.closest.
"""
lam = arg.getvector(lam, out='row')
return L.pp.reshape((3, 1)) + L.uw.reshape((3, 1)) * lam
def I(self, I_new): # noqa
# Try for Inertia Matrix
try:
verifymatrix(I_new, (3, 3))
except (ValueError, TypeError):
# Try for the moments and products of inertia
# [Ixx Iyy Izz Ixy Iyz Ixz]
try:
Ia = getvector(I_new, 6)
I_new = np.array([[Ia[0], Ia[3], Ia[5]], [Ia[3], Ia[1], Ia[4]],
[Ia[5], Ia[4], Ia[2]]])
except ValueError:
# Try for the moments of inertia
# [Ixx Iyy Izz]
Ia = getvector(I_new, 3)
I_new = np.diag(Ia)
self._I = I_new
def R(cls, a, q, p=None):
"""
Construct a new rotational 3D twist
:param a: Twist axis or line of action
:type a: 3-element array_like
:param q: Point on the line of action
:type q: 3-element array_like
:param p: pitch, defaults to None
:type p: float, optional
:return: a rotational or helical twist
:rtype: Twist instance
"""
w = tr.unitvec(argcheck.getvector(a, 3))
v = -np.cross(w, argcheck.getvector(q, 3))
if p is not None:
pitch = argcheck.getvector(p, 3)
v = v + pitch * w
return cls(v, w)
def PointDir(point, dir):
"""
Create Plucker line from point and direction
:param point: A 3D point
:type point: 3-element array_like
:param dir: Direction vector
:type dir: 3-element array_like
:return: Plucker line
:rtype: Plucker
``L = Plucker.pointdir(P, W)`` is a Plucker object that represents the
line containing the point ``P`` and parallel to the direction vector ``W``.
:seealso: Plucker, Plucker.Planes, Plucker.PQ
"""
point = arg.getvector(point, 3)
dir = arg.getvector(dir, 3)
return Plucker(np.r_[np.cross(dir, point), dir])
def inner(q1, q2):
"""
Quaternion innert product
:arg q0: quaternion as a 4-vector
:type q0: : array_like
:arg q1: uaternion as a 4-vector
:type q1: array_like
:return: inner product
:rtype: numpy.ndarray, shape=(4,)
This is the inner or dot product of two quaternions, it is the sum of the element-wise
product.
:seealso: qvmul
"""
q1 = argcheck.getvector(q1, 4)
q2 = argcheck.getvector(q2, 4)
return np.dot(q1, q2)
def kcircle(self, r, hw=None):
"""
Circular structuring element
:param r: radius of circle structuring element, or 2-vector (see below)
:type r: float, 2-tuple or 2-element vector of floats
:param hw: half-width of kernel
:type hw: integer
:return k: kernel
:rtype: numpy array (2 * 3 * sigma + 1, 2 * 3 * sigma + 1)
- ``IM.kcircle(r)`` is a square matrix ``(w,w)`` where ``w=2r+1`` of
zeros with a maximal centred circular region of radius ``r`` pixels
set to one.
- ``IM.kcircle(r,w)`` as above but the dimension of the kernel is
explicitly specified.
Example:
.. autorun:: pycon
.. note::
- If ``r`` is a 2-element vector the result is an annulus of ones,
and the two numbers are interpretted as inner and outer radii.
"""
# check valid input:
if not argcheck.isscalar(r): # r.shape[1] > 1:
r = argcheck.getvector(r)
rmax = r.max()
rmin = r.min()
else:
rmax = r
if hw is not None:
w = hw * 2 + 1
elif hw is None:
w = 2 * rmax + 1
s = np.zeros((np.int(w), np.int(w)))
c = np.floor(w / 2.0)
if not argcheck.isscalar(r):
s = self.kcircle(rmax, w) - self.kcircle(rmin, w)
else:
x, y = self.meshgrid(s)
x = x - c
y = y - c
ll = np.where(np.round((x**2 + y**2 - r**2) <= 0))
s[ll] = 1
return s
def inertia(self, q=None):
"""
SerialLink.INERTIA Manipulator inertia matrix
I = inertia(q) is the symmetric joint inertia matrix (nxn) which
relates joint torque to joint acceleration for the robot at joint
configuration q.
I = inertia() as above except uses the stored q value of the robot
object.
If q is a matrix (nxk), 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.
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:return I: The inertia matrix
:rtype I: float ndarray(n,n)
:notes:
- 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.
"""
trajn = 1
try:
q = getvector(q, self.n, 'row')
except ValueError:
trajn = q.shape[0]
verifymatrix(q, (trajn, self.n))
In = np.zeros((trajn, self.n, self.n))
for i in range(trajn):
In[i, :, :] = self.rne((np.c_[q[i, :]] @ np.ones((1, self.n))).T,
np.zeros((self.n, self.n)),
np.eye(self.n),
grav=[0, 0, 0])
if trajn == 1:
return In[0, :, :]
else:
return In
def _plot2(robot,
block,
q,
dt,
limits=None,
vellipse=False,
fellipse=False,
eeframe=True,
name=True):
# Make an empty 2D figure
env = rp.backend.PyPlot2()
trajn = 1
if q is None:
q = robot.q
try:
q = getvector(q, robot.n, 'col')
robot.q = q
except ValueError:
trajn = q.shape[1]
verifymatrix(q, (robot.n, trajn))
# Add the robot to the figure in readonly mode
if trajn == 1:
env.launch(robot.name + ' Plot', limits)
else:
env.launch(robot.name + ' Trajectory Plot', limits)
env.add(robot, readonly=True, eeframe=eeframe, name=name)
if vellipse:
vell = robot.vellipse(centre='ee')
env.add(vell)
if fellipse:
fell = robot.fellipse(centre='ee')
env.add(fell)
if trajn != 1:
for i in range(trajn):
robot.q = q[:, i]
env.step()
time.sleep(dt / 1000)
# Keep the plot open
if block: # pragma: no cover
env.hold()
return env
def oa2r(o, a=None):
"""
Create SO(3) rotation matrix from two vectors
:param o: 3-vector parallel to Y- axis
:type o: array_like
:param a: 3-vector parallel to the Z-axis
:type o: array_like
:return: 3x3 rotation matrix
:rtype: numpy.ndarray, shape=(3,3)
``T = oa2tr(O, A)`` is an SO(3) orthonormal rotation matrix for a frame defined in terms of
vectors parallel to its Y- and Z-axes with respect to a reference frame. In robotics these axes are
respectively called the orientation and approach vectors defined such that
R = [N O A] and N = O x A.
Steps:
1. N' = O x A
2. O' = A x N
3. normalize N', O', A
4. stack horizontally into rotation matrix
Notes:
- The A vector is the only guaranteed to have the same direction in the resulting
rotation matrix
- O and A do not have to be unit-length, they are normalized
- O and A do not have to be orthogonal, so long as they are not parallel
- The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
:seealso: :func:`~oa2tr`
"""
o = argcheck.getvector(o, 3, out='array')
a = argcheck.getvector(a, 3, out='array')
n = np.cross(o, a)
o = np.cross(a, n)
R = np.stack((vec.unitvec(n), vec.unitvec(o), vec.unitvec(a)), axis=1)
return R
def qvmul(q, v):
"""
Vector rotation
:arg q: input unit-quaternion
:type q: 4-vector: list, tuple, numpy.ndarray
:arg v: 3-vector to be rotated
:type v: list, tuple, numpy.ndarray
:return: rotated 3-vector
:rtype: numpy.ndarray, shape=(3,)
The vector `v` is rotated about the origin by the SO(3) equivalent of the unit
quaternion.
.. warning:: There is no check that the passed value is a unit-quaternions.
:seealso: qvmul
"""
q = check.getvector(q, 4)
v = check.getvector(v, 3)
qv = qqmul(q, qqmul(pure(v), conj(q)))
return qv[1:4]
def dotb(q, w):
"""
Rate of change of unit-quaternion
:arg q0: unit-quaternion
:type q0: 4-vector: list, tuple, numpy.ndarray
:arg w: angular velocity in body frame
:type w: 3-vector: list, tuple, numpy.ndarray
:return: rate of change of unit quaternion
:rtype: numpy.ndarray, shape=(4,)
``dot(q, w)`` is the rate of change of the elements of the unit quaternion ``q``
which represents the orientation of a body frame with angular velocity ``w`` in
the body frame.
.. warning:: There is no check that the passed values are unit-quaternions.
"""
q = check.getvector(q, 4)
w = check.getvector(w, 3)
E = q[0] * (np.eye(3, 3)) + tr.skew(q[1:4])
return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
def ctraj(T0, T1, s):
"""
Cartesian trajectory between two poses
:param T0: initial pose
:type T0: SE3
:param T1: final pose
:type T1: SE3
:return T0: smooth path from ``T0`` to ``T1``
:rtype: SE3
``ctraj(T0, T1, n)`` is a Cartesian trajectory from SE3 pose ``T0`` to
``T1`` with ``n`` points that follow a trapezoidal velocity profile along
the path. The Cartesian trajectory is an SE3 instance containing ``n``
values.
``ctraj(T0, T1, s)`` as above but the elements of ``s`` specify the
fractional distance along the path, and these values are in the
range [0 1]. The i'th point corresponds to a distance ``s[i]`` along
the path.
Examples::
>>> tg = ctraj(SE3.Rand(), SE3.Rand(), 20)
>>> len(tg)
20
Notes:
- In the second case ``s`` could be generated by a scalar trajectory
generator such as ``tpoly`` or ``lspb`` (default).
- Orientation interpolation is performed using unit-quaternion
interpolation.
Reference:
- Robotics, Vision & Control, Sec 3.1.5,
Peter Corke, Springer 2011
:seealso: :func:`~roboticstoolbox.trajectory.lspb`,
:func:`~spatialmath.unitquaternion.interp`
"""
if isinstance(s, int):
s = lspb(0, 1, s).y
elif isvector(s):
s = getvector(s)
else:
raise TypeError('bad argument for time, must be int or vector')
return T0.interp(T1, s)
def __init__(self, arg = None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if arg is None:
# empty constructor
self.data = [np.eye(2)]
elif argcheck.isvector(arg):
# SO2(value)
# SO2(list of values)
self.data = [transforms.rot2(x, unit) for x in argcheck.getvector(arg)]
else:
super().arghandler(arg)
def P(cls, a):
"""
Construct a new prismatic 3D twist
:param a: Twist axis or line of action
:type a: 3-element array_like
:return: a prismatic twist
:rtype: Twist instance
"""
w = np.r_[0, 0, 0]
v = tr.unitvec(argcheck.getvector(a, 3))
return cls(v, w)
def intersect_plane(line, plane):
r"""
Line intersection with a plane
:param line: A line
:type line: Plucker
:param plane: A plane
:type plane: 4-element array_like or Plane
:return: Intersection point
:rtype: collections.namedtuple
- ``line.intersect_plane(plane).p`` is the point where the line
intersects the plane, or None if no intersection.
- ``line.intersect_plane(plane).lam`` is the `lambda` value for the point on the line
that intersects the plane.
The plane can be specified as:
- a 4-vector :math:`[a, b, c, d]` which describes the plane :math:`\pi: ax + by + cz + d=0`.
- a ``Plane`` object
The return value is a named tuple with elements:
- ``.p`` for the point on the line as a numpy.ndarray, shape=(3,)
- ``.lam`` the `lambda` value for the point on the line.
See also Plucker.point.
"""
# Line U, V
# Plane N n
# (VxN-nU:U.N)
# Note that this is in homogeneous coordinates.
# intersection of plane (n,p) with the line (v,p)
# returns point and line parameter
if not isinstance(plane, Plane):
plane = Plane(arg.getvector(plane, 4))
den = np.dot(line.w, plane.n)
if abs(den) > (100 * _eps):
# P = -(np.cross(line.v, plane.n) + plane.d * line.w) / den
p = (np.cross(line.v, plane.n) - plane.d * line.w) / den
t = np.dot(line.pp - p, plane.n)
return namedtuple('intersect_plane', 'p lam')(p, t)
else:
return None
def conj(q):
"""
Quaternion conjugate
:arg q: quaternion as a 4-vector
:type v: array_like
:return: conjugate of input quaternion
:rtype: numpy.ndarray, shape=(4,)
Conjugate of quaternion, the vector part is negated.
"""
q = argcheck.getvector(q, 4)
return np.r_[q[0], -q[1:4]]
def PQ(P=None, Q=None):
"""
Create Plucker line object from two 3D points
:param P: First 3D point
:type P: 3-element array_like
:param Q: Second 3D point
:type Q: 3-element array_like
:return: Plucker line
:rtype: Plucker
``L = Plucker(P, Q)`` create a Plucker object that represents
the line joining the 3D points ``P`` (3-vector) and ``Q`` (3-vector). The direction
is from ``Q`` to ``P``.
:seealso: Plucker, Plucker.Planes, Plucker.PointDir
"""
P = arg.getvector(P, 3)
Q = arg.getvector(Q, 3)
# compute direction and moment
w = P - Q
v = np.cross(P - Q, P)
return Plucker(np.r_[v, w])
def stretch(self, max=1, r=None):
"""
Image normalisation
:param max: M pixels are mapped to the r 0 to M
:type max: scalar integer or float
:param r: r[0] is mapped to 0, r[1] is mapped to 1 (or max value)
:type r: 2-tuple or numpy array (2,1)
:return: Image with pixel values stretched to M across r
:rtype: Image instance
- ``IM.stretch()`` is a normalised image in which all pixel values lie
in the r range of 0 to 1. That is, a linear mapping where the minimum
value of ``im`` is mapped to 0 and the maximum value of ``im`` is
mapped to 1.
Example:
.. runblock:: pycon
.. note::
- For an integer image the result is a float image in the range 0
to max value
:references:
- Robotics, Vision & Control, Section 12.1, P. Corke,
Springer 2011.
"""
# TODO make all infinity values = None?
out = []
for im in [img.image for img in self]:
if r is None:
mn = np.min(im)
mx = np.max(im)
else:
r = argcheck.getvector(r)
mn = r[0]
mx = r[1]
zs = (im - mn) / (mx - mn) * max
if r is not None:
zs = np.maximum(0, np.minimum(max, zs))
out.append(zs)
return self.__class__(out)
def hessian0(self, q=None, J0=None, from_link=None, to_link=None):
"""
The manipulator Hessian tensor maps joint acceleration to end-effector
spatial acceleration, expressed in the world-coordinate frame. This
function calulcates this based on the ETS of the robot. One of J0 or q
is required. Supply J0 if already calculated to save computation time
:param q: The joint angles/configuration of the robot (Optional,
if not supplied will use the stored q values).
:type q: float ndarray(n)
:param J0: The manipulator Jacobian in the 0 frame
:type J0: float ndarray(6,n)
:return: The manipulator Hessian in 0 frame
:rtype: float ndarray(6,n,n)
:references:
- Kinematic Derivatives using the Elementary Transform
Sequence, J. Haviland and P. Corke
"""
if from_link is None:
from_link = self.base_link
if to_link is None:
to_link = self.ee_links[0]
path, n = self.get_path(from_link, to_link)
if J0 is None:
if q is None:
q = np.copy(self.q)
else:
q = getvector(q, n)
J0 = self.jacob0(q, from_link, to_link)
else:
verifymatrix(J0, (6, n))
H = np.zeros((6, n, n))
for j in range(n):
for i in range(j, n):
H[:3, i, j] = np.cross(J0[3:, j], J0[:3, i])
H[3:, i, j] = np.cross(J0[3:, j], J0[3:, i])
if i != j:
H[:3, j, i] = H[:3, i, j]
return H
def conj(q):
"""
Quaternion conjugate
:arg q: input quaternion
:type v: 4-vector: list, tuple, numpy.ndarray
:return: conjugate of input quaternion
:rtype: numpy.ndarray, shape=(4,)
Conjugate of quaternion, the vector part is negated.
"""
q = check.getvector(q, 4)
return np.r_[q[0], -q[1:4]]
def test_humoments(self):
im = np.array([[0, 0, 0, 0, 0, 0, 0], [0, 1, 1, 1, 0, 0, 0],
[0, 0, 1, 1, 0, 0, 0], [0, 1, 1, 1, 1, 1, 0],
[0, 1, 1, 1, 1, 1, 0], [0, 0, 0, 1, 1, 0, 0],
[0, 0, 0, 0, 0, 0, 0]])
out = np.array([
0.184815794830043, 0.004035534812971, 0.000533013844814,
0.000035606641461, 0.000000003474073, 0.000000189873096,
-0.000000003463063
])
im = Image(im)
hu = im.humoments()
nt.assert_array_almost_equal(argcheck.getvector(hu[0]), out, decimal=7)
def P(cls, a):
"""
Construct a new 2D primsmatic Twist object
:param a: displacment
:type a: 2-element array-like
:return: 2D prismatic twist
:rtype: Twist2 instance
- ``Twist.P(q)`` is a 2D Twist object representing 2D-translation in the direction ``a``.
"""
w = 0
v = tr.unitvec(argcheck.getvector(a, 2))
return cls(v, w)
def publish_transforms(self) -> None:
"""[summary]
"""
if not self.is_publishing_transforms:
return
for robot in self.robots:
joint_positions = getvector(robot.q, robot.n)
for link in robot.elinks:
if link.parent is None:
continue
if link.isjoint:
transform = link.A(joint_positions[link.jindex])
else:
transform = link.A()
self.broadcaster.sendTransform(
populate_transform_stamped(link.parent.name, link.name,
transform))
for gripper in robot.grippers:
joint_positions = getvector(gripper.q, gripper.n)
for link in gripper.links:
if link.parent is None:
continue
if link.isjoint:
transform = link.A(joint_positions[link.jindex])
else:
transform = link.A()
self.broadcaster.sendTransform(
populate_transform_stamped(link.parent.name, link.name,
transform))
def __init__(self, x = None, y = None, theta = None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if x is None:
# empty constructor
self.data = [np.eye(3)]
elif all(map(lambda x: isinstance(x, (int,float)), [x, y, theta])):
# SE2(x, y, theta)
self.data = [transforms.trot2(theta, t=[x,y], unit=unit)]
elif argcheck.isvector(x) and argcheck.isvector(y) and argcheck.isvector(theta):
# SE2(xvec, yvec, tvec)
xvec = argcheck.getvector(x)
yvec = argcheck.getvector(y, dim=len(xvec))
tvec = argcheck.getvector(theta, dim=len(xvec))
self.data = [transforms.trot2(_t, t=[_x, _y]) for (_x, _y, _t) in zip(xvec, yvec, argcheck.getunit(tvec, unit))]
elif isinstance(x, np.ndarray) and y is None and theta is None:
assert x.shape[1] == 3, 'array argument must be Nx3'
self.data = [transforms.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x]
else:
super().arghandler(x)
def pure(v):
"""
Create a pure quaternion
:arg v: vector
:type v: 3-vector: list, tuple, numpy.ndarray
:return: pure quaternion
:rtype: numpy.ndarray, shape=(4,)
Creates a pure quaternion, with a zero scalar value and the vector part
equal to the passed vector value.
"""
v = check.getvector(v, 3)
return np.r_[0, v]
