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]