def manipulability(self, q=None, J=None): """ Calculates the manipulability index (scalar) robot at the joint configuration q. It indicates dexterity, that is, how isotropic the robot's motion is with respect to the 6 degrees of Cartesian motion. The measure is high when the manipulator is capable of equal motion in all directions and low when the manipulator is close to a singularity. One of J or q is required. Supply J if already calculated to save computation time :param q: The joint coordinates of the robot :type q: float np.ndarray(n,) :param J: The manipulator Jacobian in any frame :type J: float np.ndarray(6,n) :return: The manipulability index :rtype: float References: Analysis and control of robot manipulators with redundancy, T. Yoshikawa, Robotics Research: The First International Symposium (M. Brady and R. Paul, eds.), pp. 735-747, The MIT press, 1984. """ if J is None: if q is not None: q = getvector(q, self.n) J = self.jacob0(q) else: raise ValueError('One of q or J must be supplied') else: verifymatrix(J, (6, self.n)) return np.sqrt(np.linalg.det(J @ np.transpose(J)))
def hessian0(self, q=None, J0=None, endlink=None, startlink=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) :param endlink: the final link which the Hessian represents :type endlink: str or ELink :param startlink: the first link which the Hessian represents :type startlink: str or ELink :return: The manipulator Hessian in 0 frame :rtype: float ndarray(6,n,n) H[i,j,k] is d2 u_i / dq_j dq_k where u = {t_x, t_y, t_z, r_x, r_y, r_z} J[i,j] is d u_i / dq_j where u = {t_x, t_y, t_z, ζ_x, ζ_y, ζ_z} v = J qd a = Jd qd + J qdd Jd = H qd :references: - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke """ endlink, startlink = self._get_limit_links(endlink, startlink) path, n = self.get_path(endlink, startlink) if J0 is None: q = getvector(q, n) J0 = self.jacob0(q, endlink=endlink) 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 _plot(robot, block, q, dt, limits=None, vellipse=False, fellipse=False, jointaxes=True, eeframe=True, shadow=True, name=True): # Make an empty 3D figure env = rp.backend.PyPlot() 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, jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, 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 jacobm(self, q=None, J=None, H=None, endlink=None, startlink=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) :param endlink: the final link which the Hessian represents :type endlink: str or ELink :param startlink: the first link which the Hessian represents :type startlink: str or ELink :return: The manipulability Jacobian :rtype: float ndarray(n) :references: - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke """ endlink, startlink = self._get_limit_links(endlink, startlink) path, n = self.get_path(endlink, startlink) if J is None: if q is None: q = np.copy(self.q) else: q = getvector(q, n) J = self.jacob0(q, startlink=startlink, endlink=endlink) else: verifymatrix(J, (6, n)) if H is None: H = self.hessian0(J0=J, startlink=startlink, endlink=endlink) else: verifymatrix(H, (6, n, n)) manipulability = self.manipulability(q, J=J, startlink=startlink, endlink=endlink) 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 gravload(self, q=None, grav=None): """ Compute gravity load :param q: The joint angles/configuration of the robot :type q: float ndarray(n) :param grav: The gravity vector (Optional, if not supplied will use the stored gravity values). :type grav: float ndarray(3) :return taug: The generalised joint force/torques due to gravity :rtype taug: float ndarray(n) ``taug = 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 SerialLink object. ``taug = gravload(q, grav)`` as above except the gravitational acceleration is explicitly specified as `grav``. 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. """ trajn = 1 if q is None: q = self.q if grav is None: grav = getvector(np.copy(self.gravity), 3, 'row') try: q = getvector(q, self.n, 'row') grav = getvector(grav, 3, 'row') except ValueError: trajn = q.shape[0] verifymatrix(q, (trajn, self.n)) if grav.shape[0] < trajn: grav = (grav.T @ np.ones((1, trajn))).T verifymatrix(grav, (trajn, 3)) taug = np.zeros((trajn, self.n)) for i in range(trajn): taug[i, :] = self.rne(q[i, :], np.zeros(self.n), np.zeros(self.n), grav[i, :]) if trajn == 1: return taug[0, :] else: return taug
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 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 itorque(self, q, qdd): """ Inertia torque :param qdd: The joint accelerations of the robot :type qdd: float ndarray(n) :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 taui: The inertia torque vector :rtype taui: float ndarray(n) ``tauI = 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. taui = inertia(q) * qdd. If q and qdd are matrices (nxk), each row is interpretted as a joint state vector, and the result is a matrix (nxk) where each row is the corresponding joint torques. :notes: - If the robot model contains non-zero motor inertia then this will included in the result. """ trajn = 1 try: q = getvector(q, self.n, 'row') qdd = getvector(qdd, self.n, 'row') except ValueError: trajn = q.shape[0] verifymatrix(q, (trajn, self.n)) verifymatrix(qdd, (trajn, self.n)) taui = np.zeros((trajn, self.n)) for i in range(trajn): taui[i, :] = self.rne(q[i, :], np.zeros(self.n), qdd[i, :], grav=[0, 0, 0]) if trajn == 1: return taui[0, :] else: return taui
def cinertia(self, q=None): """ M = cinertia(q) is the nxn Cartesian (operational space) inertia matrix which relates Cartesian force/torque to Cartesian acceleration at the joint configuration q. M = cinertia() 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 cinertia 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 M: The inertia matrix :rtype M: float ndarray(n,n) """ trajn = 1 if q is None: q = self.q try: q = getvector(q, self.n, 'row') except ValueError: trajn = q.shape[0] verifymatrix(q, (trajn, self.n)) Mt = np.zeros((trajn, self.n, self.n)) for i in range(trajn): J = self.jacob0(q[i, :]) Ji = np.linalg.pinv(J) M = self.inertia(q[i, :]) Mt[i, :, :] = Ji.T @ M @ Ji if trajn == 1: return Mt[0, :, :] else: return Mt
def manipulability(self, q=None, J=None, from_link=None, to_link=None): """ Calculates the manipulability index (scalar) robot at the joint configuration q. It indicates dexterity, that is, how isotropic the robot's motion is with respect to the 6 degrees of Cartesian motion. The measure is high when the manipulator is capable of equal motion in all directions and low when the manipulator is close to a singularity. One of J or q is required. Supply J 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) :return: The manipulability index :rtype: float :references: - Analysis and control of robot manipulators with redundancy, T. Yoshikawa, - Robotics Research: The First International Symposium (M. Brady and R. Paul, eds.), pp. 735-747, The MIT press, 1984. """ 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)) return np.sqrt(np.linalg.det(J @ np.transpose(J)))
def jacobm(self, q=None, J=None, H=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 coordinates of the robot :type q: float np.ndarray(n,) :param J: The manipulator Jacobian in any frame :type J: float np.ndarray(6,n) :param H: The manipulator Hessian in any frame :type H: float np.ndarray(6,n,n) :return: The manipulability Jacobian :rtype: float np.ndarray(n,1) References: Maximising Manipulability in Resolved-Rate Motion Control, J. Haviland and P. Corke """ if J is None: if q is not None: q = getvector(q, self.n) J = self.jacob0(q) else: raise ValueError('One of q or J must be supplied') else: verifymatrix(J, (6, self.n)) if H is None: H = self.hessian0(J0=J) else: verifymatrix(H, (6, self.n, self.n)) manipulability = self.manipulability(J=J) b = np.linalg.inv(J @ np.transpose(J)) Jm = np.zeros((self.n, 1)) for i in range(self.n): c = J @ np.transpose(H[:, :, i]) Jm[i, 0] = manipulability * \ np.transpose(c.flatten('F')) @ b.flatten('F') return Jm
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 hessian0(self, q=None, J0=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 coordinates of the robot :type q: float np.ndarray(n,) :param J0: The manipulator Jacobian in the 0 frame :type J0: float np.ndarray(6,n) :return: The manipulator Hessian in 0 frame :rtype: float np.ndarray(6,n,n) References: Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke """ if J0 is None: if q is not None: q = getvector(q, self.n) J0 = self.jacob0(q) else: raise ValueError('One of q or J0 must be supplied') else: verifymatrix(J0, (6, self.n)) H = np.zeros((6, self.n, self.n)) for j in range(self.n): for i in range(j, self.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 fkine(self, q=None): ''' Evaluates the forward kinematics of a robot based on its ETS and joint angles q. T = fkine(q) evaluates forward kinematics for the robot at joint configuration q. T = fkine() as above except uses the stored q value of the robot object. Trajectory operation: Calculates fkine for each point on a trajectory of joints q where q is (nxm) and the returning SE3 in (m) :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: The transformation matrix representing the pose of the end-effector :rtype: SE3 :notes: - The robot's base or tool transform, if present, are incorporated into the result. :references: - Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke ''' trajn = 1 if q is None: q = self.q try: q = getvector(q, self.n, 'col') except ValueError: trajn = q.shape[1] verifymatrix(q, (self.n, trajn)) for i in range(trajn): j = 0 tr = self.base for link in self._fkpath: if link.jtype == link.VARIABLE: T = link.A(q[j, i]) j += 1 else: T = link.A() tr = tr * T tr = tr * self.tool if i == 0: t = SE3(tr) else: t.append(tr) return t
def coriolis(self, q, qd): """ Coriolis and centripetal term ``C = coriolis(q, qd)`` calculates the Coriolis/centripetal matrix (nxn) for the robot in configuration q and velocity qd, where n is the number of joints. The product c*qd 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. If q and qd are matrices (nxk), each row is interpretted as a joint state vector, and the result (nxnxk) is a 3d-matrix where each plane corresponds to a row of q and qd. :notes: - Joint viscous friction is also a joint force proportional to velocity but it is eliminated in the computation of this value. - Computationally slow, involves n^2/2 invocations of RNE. """ trajn = 1 try: q = getvector(q, self.n, 'row') qd = getvector(qd, self.n, 'row') except ValueError: trajn = q.shape[0] verifymatrix(q, (trajn, self.n)) verifymatrix(qd, (trajn, self.n)) r1 = self.nofriction(True, True) C = np.zeros((trajn, self.n, self.n)) Csq = np.zeros((trajn, 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 j in range(trajn): for i in range(self.n): QD = np.zeros(self.n) QD[i] = 1 tau = r1.rne(q[j, :], QD, np.zeros(self.n), grav=[0, 0, 0]) Csq[j, :, i] = Csq[j, :, i] + tau # Find the torques that depend on a pair of finite joint speeds, # these are due to the product (Coridolis) terms # set QD = [1 1 0 ...] then resulting torque is due to # qd_1 qd_2 + qd_1^2 + qd_2^2 for k in range(trajn): 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(q[k, :], QD, np.zeros(self.n), grav=[0, 0, 0]) C[k, :, j] = C[k, :, j] + \ (tau - Csq[k, :, j] - Csq[k, :, i]) * qd[k, i] / 2 C[k, :, i] = C[k, :, i] + \ (tau - Csq[k, :, j] - Csq[k, :, i]) * qd[k, j] / 2 C[k, :, :] = C[k, :, :] + Csq[k, :, :] @ np.diag(qd[k, :]) if trajn == 1: return C[0, :, :] else: return C
def pay(self, W, q=None, J=None, frame=1): """ tau = pay(W, J) Returns the generalised joint force/torques due to a payload wrench W applied to the end-effector. Where the manipulator Jacobian is J (6xn), and n is the number of robot joints. tau = pay(W, q, frame) as above but the Jacobian is calculated at pose q in the frame given by frame which is 0 for base frame, 1 for end-effector frame. Uses the formula tau = J'W, where W is a wrench vector applied at the end effector, W = [Fx Fy Fz Mx My Mz]'. Trajectory operation: In the case q is nxm or J is 6xnxm then tau is nxm where each row is the generalised force/torque at the pose given by corresponding row of q. :param W: A wrench vector applied at the end effector, W = [Fx Fy Fz Mx My Mz] :type W: float ndarray(6) :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 (Optional, if not supplied will use the q value). :type J: float ndarray(6,n) :param frame: The frame in which to torques are expressed in when J is not supplied. 0 means base frame of the robot, 1 means end- effector frame :type frame: int :return tau: The joint forces/torques due to w :rtype tau: float ndarray(n) :notes: - Wrench vector and Jacobian must be from the same reference frame. - Tool transforms are taken into consideration when frame=1. - Must have a constant wrench - no trajectory support for this yet. """ try: W = getvector(W, 6) trajn = 0 except ValueError: trajn = W.shape[0] verifymatrix(W, (trajn, 6)) if trajn: # A trajectory if J is not None: # Jacobian supplied verifymatrix(J, (trajn, 6, self.n)) else: # Use q instead verifymatrix(q, (trajn, self.n)) J = np.zeros((trajn, 6, self.n)) for i in range(trajn): if frame: J[i, :, :] = self.jacobe(q[i, :]) else: J[i, :, :] = self.jacob0(q[i, :]) else: # Single configuration if J is not None: # Jacobian supplied verifymatrix(J, (6, self.n)) else: # Use q instead if q is None: q = np.copy(self.q) else: q = getvector(q, self.n) if frame: J = self.jacobe(q) else: J = self.jacob0(q) if trajn == 0: tau = -J.T @ W else: tau = np.zeros((trajn, self.n)) for i in range(trajn): tau[i, :] = -J[i, :, :].T @ W[i, :] return tau
def accel(self, q, qd, torque): """ Compute acceleration due to applied torque :param q: The joint angles/configuration of the robot :type q: float ndarray(n) :param qd: The joint velocities of the robot :type qd: float ndarray(n) :param torque: The joint torques of the robot :type torque: float ndarray(n) ``qdd = accel(q, qd, torque)`` calculates a vector (n) of joint accelerations that result from applying the actuator force/torque (n) to the manipulator in state q (n) and qd (n), and n is the number of robot joints. If q, qd, torque are matrices (nxk) then qdd is a matrix (nxk) where each row is the acceleration corresponding to the equivalent cols of q, qd, torque. :return qdd: The joint accelerations of the robot :rtype qdd: float ndarray(n) :notes: - Useful for simulation of manipulator dynamics, in conjunction with a numerical integration function. - Uses the method 1 of Walker and Orin to compute the forward dynamics. - Featherstone's method is more efficient for robots with large numbers of joints. - Joint friction is considered. :references: - Efficient dynamic computer simulation of robotic mechanisms, M. W. Walker and D. E. Orin, ASME Journa of Dynamic Systems, Measurement and Control, vol. 104, no. 3, pp. 205-211, 1982. """ trajn = 1 try: q = getvector(q, self.n, 'row') qd = getvector(qd, self.n, 'row') torque = getvector(torque, self.n, 'row') except ValueError: trajn = q.shape[0] verifymatrix(q, (trajn, self.n)) verifymatrix(qd, (trajn, self.n)) verifymatrix(torque, (trajn, self.n)) qdd = np.zeros((trajn, self.n)) for i in range(trajn): # Compute current manipulator inertia torques resulting from unit # acceleration of each joint with no gravity. qI = (np.c_[q[i, :]] @ np.ones((1, self.n))).T qdI = np.zeros((self.n, self.n)) qddI = np.eye(self.n) m = self.rne(qI, qdI, qddI, grav=[0, 0, 0]) # Compute gravity and coriolis torque torques resulting from zero # acceleration at given velocity & with gravity acting. tau = self.rne(q[i, :], qd[i, :], np.zeros((1, self.n))) inter = np.expand_dims((torque[i, :] - tau), axis=1) qdd[i, :] = (np.linalg.inv(m) @ inter).flatten() if trajn == 1: return qdd[0, :] else: return qdd
def paycap(self, w, tauR, frame=1, q=None): """ Static payload capacity of a robot :param w: The payload wrench :type w: float ndarray(n) :param tauR: Joint torque matrix minimum and maximums :type tauR: float ndarray(n,2) :param frame: The frame in which to torques are expressed in when J is not supplied. 'base' means base frame of the robot, 'ee' means end-effector frame :type frame: str :param q: The joint angles/configuration of the robot. :type q: float ndarray(n) :return wmax: The maximum permissible payload wrench :rtype wmax: float ndarray(6) :return joint: The joint index (zero indexed) which hits its force/torque limit :rtype joint: int ``wmax, joint = paycap(q, w, f, tauR)`` returns the maximum permissible payload wrench ``wmax`` (6) applied at the end-effector, and the index of the joint (zero indexed) which hits its force/torque limit at that wrench. ``q`` (n) is the manipulator pose, ``w`` the payload wrench (6), ``f`` the wrench reference frame and tauR (nx2) is a matrix of joint forces/torques (first col is maximum, second col minimum). Trajectory operation: In the case q is nxm then wmax is Mx6 and J is Mx1 where the rows are the results at the pose given by corresponding row of q. :notes: - Wrench vector and Jacobian must be from the same reference frame - Tool transforms are taken into consideration for frame=1. """ trajn = 1 if q is None: q = self.q try: q = getvector(q, self.n, 'row') w = getvector(w, 6, 'row') except ValueError: trajn = q.shape[1] verifymatrix(q, (trajn, self.n)) verifymatrix(w, (trajn, 6)) verifymatrix(tauR, (self.n, 2)) wmax = np.zeros((trajn, 6)) joint = np.zeros(trajn, dtype=np.int) for i in range(trajn): tauB = self.gravload(q[i, :]) # tauP = self.rne( # np.zeros(self.n), np.zeros(self.n), # q, grav=[0, 0, 0], fext=w/np.linalg.norm(w)) tauP = self.pay(w[i, :] / np.linalg.norm(w[i, :]), q=q[i, :], frame=frame) M = tauP > 0 m = tauP <= 0 TAUm = np.ones(self.n) TAUM = np.ones(self.n) for c in range(self.n): TAUM[c] = tauR[c, 0] TAUm[c] = tauR[c, 1] WM = np.zeros(self.n) WM[M] = (TAUM[M] - tauB[M]) / tauP[M] WM[m] = (TAUm[m] - tauB[m]) / tauP[m] WM[WM == np.NINF] = np.Inf wmax[i, :] = WM joint[i] = np.argmin(WM) if trajn == 1: return wmax[0, :], joint[0] else: return wmax, joint
def ikine(self, T, ilimit=500, rlimit=100, tol=1e-10, Y=0.1, Ymin=0, mask=None, q0=None, search=False, slimit=100, transpose=None): """ Numerical inverse kinematics by optimization (Robot superclass) :param T: The desired end-effector pose :type T: SE3 with 1 or ``m`` values :param ilimit: maximum number of iterations :type ilimit: int (default 500) :param rlimit: maximum number of consecutive step rejections :type rlimit: int (default 100) :param tol: final error tolerance :type tol: float (default 1e-10) :param Y: initial value of lambda :type Y: float (default 0.1) :param Ymin: minimum allowable value of lambda :type Ymin: float (default 0) :param mask: mask vector that correspond to translation in X, Y and Z and rotation about X, Y and Z respectively. :type mask: float ndarray(6) :param q0: initial joint configuration (default all zeros) :type q0: float ndarray(n) (default all zeros) :param search: search over all configurations :type search: bool :param slimit: maximum number of search attempts :type slimit: int (default 100) :param transpose: use Jacobian transpose with step size A, rather than Levenberg-Marquadt :type transpose: float :return: The calculated joint values :rtype: float ndarray(n) :return: IK solver failed :rtype: bool or list of bool :return: If failed, what went wrong :rtype: List of str ``q, failure, reason = ikine(T)`` are the joint coordinates (n) corresponding to the robot end-effector pose ``T`` which is an ``SE3`` instance. ``failure`` is True if the solver failed, and ``reason`` contains details of the failure. This method can be used for robots with any number of degrees of freedom. **Trajectory operation:** If ``T`` contains ``m`` > 1 values, ie. a trajectory, then returns the joint coordinates corresponding to each of the pose values in ``T``. ``q`` (m,n) where ``n`` is the number of robot joints. The initial estimate of ``q`` for each time step is taken as the solution from the previous time step. Returns trajectory of joints ``q`` (m,n), list of failure (m) and list of error reasons (m). **Underactuated robots:** For the case where the manipulator has fewer than 6 DOF the solution space has more dimensions than can be spanned by the manipulator joint coordinates. In this case we specify the ``mask`` option where the ``mask`` vector (6) specifies the Cartesian DOF (in the wrist coordinate frame) that will be ignored in reaching a solution. The mask vector has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value should be 0 (for ignore) or 1. The number of non-zero elements should equal the number of manipulator DOF. For example when using a 3 DOF manipulator rotation orientation might be unimportant in which case use the option: mask = [1 1 1 0 0 0]. For robots with 4 or 5 DOF this method is very difficult to use since orientation is specified by T in world coordinates and the achievable orientations are a function of the tool position. .. note:: - Solution is computed iteratively. - Implements a Levenberg-Marquadt variable step size solver. - The tolerance is computed on the norm of the error between current and desired tool pose. This norm is computed from distances and angles without any kind of weighting. - The inverse kinematic solution is generally not unique, and depends on the initial guess q0 (defaults to 0). - The default value of q0 is zero which is a poor choice for most manipulators (eg. puma560, twolink) since it corresponds to a kinematic singularity. - Such a solution is completely general, though much less efficient than specific inverse kinematic solutions derived symbolically, like ikine6s or ikine3. - This approach allows a solution to be obtained at a singularity, but the joint angles within the null space are arbitrarily assigned. - Joint offsets, if defined, are added to the inverse kinematics to generate q. - Joint limits are not considered in this solution. - The 'search' option peforms a brute-force search with initial conditions chosen from the entire configuration space. - If the search option is used any prismatic joint must have joint limits defined. :references: - Robotics, Vision & Control, P. Corke, Springer 2011, Section 8.4. """ if not isinstance(T, SE3): T = SE3(T) trajn = len(T) err = [] try: if q0 is not None: if trajn == 1: q0 = getvector(q0, self.n, 'row') else: verifymatrix(q0, (trajn, self.n)) else: q0 = np.zeros((trajn, self.n)) except ValueError: verifymatrix(q0, (trajn, self.n)) if mask is not None: mask = getvector(mask, 6) else: mask = np.ones(6) if search: # Randomised search for a starting point search = False # quiet = True qlim = self.qlim qspan = qlim[1] - qlim[0] # range of joint motion for k in range(slimit): q0n = np.random.rand(self.n) * qspan + qlim[0, :] # fprintf('Trying q = %s\n', num2str(q)) q, _, _ = self.ikine(T, ilimit, rlimit, tol, Y, Ymin, mask, q0n, search, slimit, transpose) if not np.sum(np.abs(q)) == 0: return q, True, err q = np.array([]) return q, False, err if not self.n >= np.sum(mask): raise ValueError('Number of robot DOF must be >= the same number ' 'of 1s in the mask matrix') W = np.diag(mask) # Preallocate space for results qt = np.zeros((len(T), self.n)) # Total iteration count tcount = 0 # Rejected step count rejcount = 0 failed = [] nm = 0 revolutes = [] for i in range(self.n): revolutes.append(not self.links[i].sigma) for i in range(len(T)): iterations = 0 q = np.copy(q0[i, :]) Yl = Y while True: # Update the count and test against iteration limit iterations += 1 if iterations > ilimit: err.append('ikine: iteration limit {0} exceeded ' ' (pose {1}), final err {2}'.format( ilimit, i, nm)) failed.append(True) break e = tr2delta(self.fkine(q).A, T[i].A) # Are we there yet if np.linalg.norm(W @ e) < tol: # print(iterations) failed.append(False) break # Compute the Jacobian J = self.jacobe(q) JtJ = J.T @ W @ J if transpose is not None: # Do the simple Jacobian transpose with constant gain dq = transpose * J.T @ e # lgtm [py/multiple-definition] else: # Do the damped inverse Gauss-Newton with # Levenberg-Marquadt dq = np.linalg.inv(JtJ + ( (Yl + Ymin) * np.eye(self.n))) @ J.T @ W @ e # Compute possible new value of qnew = q + dq # And figure out the new error enew = tr2delta(self.fkine(qnew).A, T[i].A) # Was it a good update? if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e): # Step is accepted q = qnew e = enew Yl = Yl / 2 rejcount = 0 else: # Step is rejected, increase the damping and retry Yl = Yl * 2 rejcount += 1 if rejcount > rlimit: err.append( 'ikine: rejected-step limit {0} exceeded ' '(pose {1}), final err {2}'.format( rlimit, i, np.linalg.norm(W @ enew))) failed.append(True) break # Wrap angles for revolute joints k = (q > np.pi) & revolutes q[k] -= 2 * np.pi k = (q < -np.pi) & revolutes q[k] += +2 * np.pi nm = np.linalg.norm(W @ e) qt[i, :] = q tcount += iterations if any(failed): err.append('failed to converge: try a different ' 'initial value of joint coordinates') if trajn == 1: qt = qt[0, :] failed = failed[0] return qt, failed, err
def fkine(self, q=None, from_link=None, to_link=None): ''' Evaluates the forward kinematics of a robot based on its ETS and joint angles q. T = fkine(q) evaluates forward kinematics for the robot at joint configuration q. T = fkine() as above except uses the stored q value of the robot object. Trajectory operation: Calculates fkine for each point on a trajectory of joints q where q is (nxm) and the returning SE3 in (m) :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: The transformation matrix representing the pose of the end-effector :rtype: SE3 :notes: - The robot's base or tool transform, if present, are incorporated into the result. :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] trajn = 1 if q is None: q = self.q path, n = self.get_path(from_link, to_link) use_jindex = True try: q = getvector(q, self.n, 'col') except ValueError: try: q = getvector(q, n, 'col') use_jindex = False j = 0 except ValueError: trajn = q.shape[1] verifymatrix(q, (self.n, trajn)) for i in range(trajn): tr = self.base.A for link in path: if link.isjoint: if use_jindex: T = link.A(q[link.jindex, i], fast=True) else: T = link.A(q[j, i], fast=True) j += 1 else: T = link.A(fast=True) tr = tr @ T if i == 0: t = SE3(tr) else: t.append(SE3(tr)) return t
def ikunc(self, T, q0=None, ilimit=1000): """ Inverse manipulator by optimization without joint limits (Robot superclass) q, success, err = ikunc(T) are the joint coordinates (n) corresponding to the robot end-effector pose T which is an SE3 object or homogenenous transform matrix (4x4), and n is the number of robot joints. Also returns success and err which is the scalar final value of the objective function. q, success, err = robot.ikunc(T, q0, ilimit) as above but specify the initial joint coordinates q0 used for the minimisation. Trajectory operation: In all cases if T is a vector of SE3 objects (m) or a homogeneous transform sequence (4x4xm) then returns the joint coordinates corresponding to each of the transforms in the sequence. q is mxn where n is the number of robot joints. The initial estimate of q for each time step is taken as the solution from the previous time step. :param T: The desired end-effector pose :type T: SE3 or SE3 trajectory :param ilimit: Iteration limit (default 1000) :type ilimit: bool :retrun q: The calculated joint values :rtype q: float ndarray(n) :retrun success: IK solved (True) or failed (False) :rtype success: bool :retrun error: Final pose error :rtype error: float .. note:: - Joint limits are not considered in this solution. - Can be used for robots with arbitrary degrees of freedom. - In the case of multiple feasible solutions, the solution returned depends on the initial choice of q0 - Works by minimizing the error between the forward kinematics of the joint angle solution and the end-effector frame as an optimisation. - The objective function (error) is described as: sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega ) Where omega is some gain matrix, currently not modifiable. """ if not isinstance(T, SE3): T = SE3(T) trajn = len(T) if q0 is None: q0 = np.zeros((trajn, self.n)) verifymatrix(q0, (trajn, self.n)) qt = np.zeros((trajn, self.n)) success = [] err = [] omega = np.diag([1, 1, 1, 3 / self.reach]) def sumsqr(arr): return np.sum(np.power(arr, 2)) for i in range(trajn): Ti = T[i] res = minimize(lambda q: sumsqr( ((np.linalg.inv(Ti.A) @ self.fkine(q).A) - np.eye(4)) @ omega), q0[i, :], options={ 'gtol': 1e-6, 'maxiter': ilimit }) qt[i, :] = res.x success.append(res.success) err.append(res.fun) if trajn == 1: return qt[0, :], success[0], err[0] else: return qt, success, err
def base(self, base_new): if isinstance(base_new, sp.SE3): self._base = base_new.A else: verifymatrix(base_new, (4, 4)) self._base = base_new
def _plot(robot, block, q, dt, limits=None, vellipse=False, fellipse=False, jointaxes=True, eeframe=True, shadow=True, name=True, movie=None): # Make an empty 3D figure env = rp.backend.PyPlot() 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, jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, name=name) if vellipse: vell = robot.vellipse(centre='ee') env.add(vell) if fellipse: fell = robot.fellipse(centre='ee') env.add(fell) if movie is not None: if not _pil_exists: raise RuntimeError( 'to save movies PIL must be installed:\npip3 install PIL') images = [] # list of images saved from each plot # make the background white, looks better than grey stipple env.ax.w_xaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) env.ax.w_yaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) env.ax.w_zaxis.set_pane_color((1.0, 1.0, 1.0, 1.0)) if trajn != 1: for i in range(trajn): robot.q = q[:, i] env.step() time.sleep(dt / 1000) if movie is not None: # render the frame and save as a PIL image in the list canvas = env.fig.canvas img = PIL.Image.frombytes('RGB', canvas.get_width_height(), canvas.tostring_rgb()) images.append(img) if movie is not None: # save it as an animated GIF images[0].save(movie, save_all=True, append_images=images[1:], optimize=False, duration=dt, loop=0) # Keep the plot open if block: # pragma: no cover env.hold() return env
def ikcon(self, T, q0=None): """ Inverse kinematics by optimization with joint limits (Robot superclass) q, success, err = ikcon(T, q0) calculates the joint coordinates (1xn) corresponding to the robot end-effector pose T which is an SE3 object or homogenenous transform matrix (4x4), and N is the number of robot joints. Initial joint coordinates Q0 used for the minimisation. q, success, err = ikcon(T) as above but q0 is set to 0. Trajectory operation: In all cases if T is a vector of SE3 objects or a homogeneous transform sequence (4x4xm) then returns the joint coordinates corresponding to each of the transforms in the sequence. q is mxn where n is the number of robot joints. The initial estimate of q for each time step is taken as the solution from the previous time step. Retruns trajectory of joints q (mxn), list of success (m) and list of errors (m) :param T: The desired end-effector pose :type T: SE3 or SE3 trajectory :param q0: initial joint configuration (default all zeros) :type q0: float ndarray(n) (default all zeros) :retrun q: The calculated joint values :rtype q: float ndarray(n) :retrun success: IK solved (True) or failed (False) :rtype success: bool :retrun error: Final pose error :rtype error: float .. note:: - Joint limits are considered in this solution. - Can be used for robots with arbitrary degrees of freedom. - In the case of multiple feasible solutions, the solution returned depends on the initial choice of q0. - Works by minimizing the error between the forward kinematics of the joint angle solution and the end-effector frame as an optimisation. - The objective function (error) is described as: sumsqr( (inv(T)*robot.fkine(q) - eye(4)) * omega ) Where omega is some gain matrix, currently not modifiable. """ if not isinstance(T, SE3): T = SE3(T) trajn = len(T) try: if q0 is not None: q0 = getvector(q0, self.n, 'row') else: q0 = np.zeros((trajn, self.n)) except ValueError: verifymatrix(q0, (trajn, self.n)) # create output variables qstar = np.zeros((trajn, self.n)) error = [] exitflag = [] omega = np.diag([1, 1, 1, 3 / self.reach]) def cost(q, T, omega): return np.sum(((np.linalg.pinv(T.A) @ self.fkine(q).A - np.eye(4)) @ omega)**2) bnds = Bounds(self.qlim[0, :], self.qlim[1, :]) for i in range(trajn): Ti = T[i] res = minimize(lambda q: cost(q, Ti, omega), q0[i, :], bounds=bnds, options={'gtol': 1e-6}) qstar[i, :] = res.x error.append(res.fun) exitflag.append(res.success) if trajn > 1: return qstar, exitflag, error else: return qstar[0, :], exitflag[0], error[0]