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