diff --git a/packages/Python/modern_robotics/core.py b/packages/Python/modern_robotics/core.py index 7f1f2e9..925751c 100644 --- a/packages/Python/modern_robotics/core.py +++ b/packages/Python/modern_robotics/core.py @@ -4,7 +4,7 @@ from __future__ import print_function Modern Robotics: Mechanics, Planning, and Control. Code Library *************************************************************************** -Author: Huan Weng, Bill Hunt, Jarvis Schultz, Mikhail Todes, +Author: Huan Weng, Bill Hunt, Jarvis Schultz, Mikhail Todes, Email: huanweng@u.northwestern.edu Date: July 2018 *************************************************************************** @@ -27,24 +27,24 @@ import numpy as np def NearZero(z): """Determines whether a scalar is small enough to be treated as zero - + :param z: A scalar input to check :return: True if z is close to zero, false otherwise - + Example Input: z = -1e-7 Output: True """ return abs(z) < 1e-6 - + def Normalize(V): """Normalizes a vector - + :param V: A vector :return: A unit vector pointing in the same direction as z - - Example Input: + + Example Input: V = np.array([1, 2, 3]) Output: np.array([0.26726124, 0.53452248, 0.80178373]) @@ -57,16 +57,16 @@ def Normalize(V): def RotInv(R): """Inverts a rotation matrix - + :param R: A rotation matrix :return: The inverse of R - - Example Input: + + Example Input: R = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) Output: - np.array([[0, 1, 0], + np.array([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) """ @@ -74,28 +74,28 @@ def RotInv(R): def VecToso3(omg): """Converts a 3-vector to an so(3) representation - + :param omg: A 3-vector :return: The skew symmetric representation of omg - - Example Input: + + Example Input: omg = np.array([1, 2, 3]) Output: np.array([[ 0, -3, 2], [ 3, 0, -1], [-2, 1, 0]]) """ - return np.array([[0, -omg[2], omg[1]], - [omg[2], 0, -omg[0]], - [-omg[1], omg[0], 0]]) + return np.array([[0, -omg[2], omg[1]], + [omg[2], 0, -omg[0]], + [-omg[1], omg[0], 0]]) def so3ToVec(so3mat): """Converts an so(3) representation to a 3-vector - + :param so3mat: A 3x3 skew-symmetric matrix :return: The 3-vector corresponding to so3mat - - Example Input: + + Example Input: so3mat = np.array([[ 0, -3, 2], [ 3, 0, -1], [-2, 1, 0]]) @@ -105,14 +105,14 @@ def so3ToVec(so3mat): return np.array([so3mat[2][1], so3mat[0][2], so3mat[1][0]]) def AxisAng3(expc3): - """Converts a 3-vector of exponential coordinates for rotation into + """Converts a 3-vector of exponential coordinates for rotation into axis-angle form - + :param expc3: A 3-vector of exponential coordinates for rotation - :return omghat: A unit rotation axis + :return omghat: A unit rotation axis :return theta: The corresponding rotation angle - - Example Input: + + Example Input: expc3 = np.array([1, 2, 3]) Output: (np.array([0.26726124, 0.53452248, 0.80178373]), 3.7416573867739413) @@ -121,10 +121,10 @@ def AxisAng3(expc3): def MatrixExp3(so3mat): """Computes the matrix exponential of a matrix in so(3) - + :param so3mat: A 3x3 skew-symmetric matrix :return: The matrix exponential of so3mat - + Example Input: so3mat = np.array([[ 0, -3, 2], [ 3, 0, -1], @@ -145,11 +145,11 @@ def MatrixExp3(so3mat): def MatrixLog3(R): """Computes the matrix logarithm of a rotation matrix - + :param R: A 3x3 rotation matrix :return: The matrix logarithm of R - - Example Input: + + Example Input: R = np.array([[0, 0, 1], [1, 0, 0], [0, 1, 0]]) @@ -164,7 +164,7 @@ def MatrixLog3(R): if not NearZero(1 + R[2][2]): omg = (1.0 / np.sqrt(2 * (1 + R[2][2]))) \ * np.array([R[0][2], R[1][2], 1 + R[2][2]]) - elif not NearZero(1 + R[1][1]): + elif not NearZero(1 + R[1][1]): omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \ * np.array([R[0][1], 1 + R[1][1], R[2][1]]) else: @@ -182,15 +182,15 @@ def MatrixLog3(R): def RpToTrans(R, p): """Converts a rotation matrix and a position vector into homogeneous - tranformation matrix - + transformation matrix + :param R: A 3x3 rotation matrix :param p: A 3-vector :return: A homogeneous transformation matrix corresponding to the inputs - - Example Input: - R = np.array([[1, 0, 0], - [0, 0, -1], + + Example Input: + R = np.array([[1, 0, 0], + [0, 0, -1], [0, 1, 0]]) p = np.array([1, 2, 5]) Output: @@ -199,25 +199,25 @@ def RpToTrans(R, p): [0, 1, 0, 5], [0, 0, 0, 1]]) """ - return np.r_[np.c_[R, p], [[0, 0, 0, 1]]] + return np.r_[np.c_[R, p], [[0, 0, 0, 1]]] def TransToRp(T): - """Converts a homogeneous transformation matrix into a rotation matrix + """Converts a homogeneous transformation matrix into a rotation matrix and position vector - :param T: A homogeneous transformation matrix + :param T: A homogeneous transformation matrix :return R: The corresponding rotation matrix, :return p: The corresponding position vector. - - Example Input: + + Example Input: T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]]) Output: - (np.array([[1, 0, 0], - [0, 0, -1], - [0, 1, 0]]), + (np.array([[1, 0, 0], + [0, 0, -1], + [0, 1, 0]]), np.array([0, 0, 3])) """ T = np.array(T) @@ -225,12 +225,12 @@ def TransToRp(T): def TransInv(T): """Inverts a homogeneous transformation matrix - + :param T: A homogeneous transformation matrix :return: The inverse of T Uses the structure of transformation matrices to avoid taking a matrix inverse, for efficiency. - + Example input: T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], @@ -245,19 +245,19 @@ def TransInv(T): R, p = TransToRp(T) Rt = np.array(R).T return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]] - + def VecTose3(V): """Converts a spatial velocity vector into a 4x4 matrix in se3 - + :param V: A 6-vector representing a spatial velocity :return: The 4x4 se3 representation of V - - Example Input: + + Example Input: V = np.array([1, 2, 3, 4, 5, 6]) Output: - np.array([[ 0, -3, 2, 4], - [ 3, 0, -1, 5], - [-2, 1, 0, 6], + np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], [ 0, 0, 0, 0]]) """ return np.r_[np.c_[VecToso3([V[0], V[1], V[2]]), [V[3], V[4], V[5]]], @@ -265,14 +265,14 @@ def VecTose3(V): def se3ToVec(se3mat): """ Converts an se3 matrix into a spatial velocity vector - + :param se3mat: A 4x4 matrix in se3 :return: The spatial velocity 6-vector corresponding to se3mat - - Example Input: - se3mat = np.array([[ 0, -3, 2, 4], - [ 3, 0, -1, 5], - [-2, 1, 0, 6], + + Example Input: + se3mat = np.array([[ 0, -3, 2, 4], + [ 3, 0, -1, 5], + [-2, 1, 0, 6], [ 0, 0, 0, 0]]) Output: np.array([1, 2, 3, 4, 5, 6]) @@ -281,16 +281,16 @@ def se3ToVec(se3mat): [se3mat[0][3], se3mat[1][3], se3mat[2][3]]] def Adjoint(T): - """Computes the adjoint representation of a homogeneous transformation + """Computes the adjoint representation of a homogeneous transformation matrix :param T: A homogeneous transformation matrix :return: The 6x6 adjoint representation [AdT] of T - - Example Input: - T = np.array([[1, 0, 0, 0], - [0, 0, -1, 0], - [0, 1, 0, 3], + + Example Input: + T = np.array([[1, 0, 0, 0], + [0, 0, -1, 0], + [0, 1, 0, 3], [0, 0, 0, 1]]) Output: np.array([[1, 0, 0, 0, 0, 0], @@ -305,15 +305,15 @@ def Adjoint(T): np.c_[np.dot(VecToso3(p), R), R]] def ScrewToAxis(q, s, h): - """Takes a parametric description of a scre axis and converts it to a + """Takes a parametric description of a screw axis and converts it to a normalized screw axis - + :param q: A point lying on the screw axis :param s: A unit vector in the direction of the screw axis :param h: The pitch of the screw axis :return: A normalized screw axis described by the inputs - - Example Input: + + Example Input: q = np.array([3, 0, 0]) s = np.array([0, 0, 1]) h = 2 @@ -323,15 +323,15 @@ def ScrewToAxis(q, s, h): return np.r_[s, np.cross(q, s) + np.dot(h, s)] def AxisAng6(expc6): - """Converts a 6-vector of exponenation coordinates into screw axis-angle + """Converts a 6-vector of exponential coordinates into screw axis-angle form - - :param expc6: A 6-vector of exponential corrdinates for rigid-body motion - S*theta + + :param expc6: A 6-vector of exponential coordinates for rigid-body motion + S*theta :return S: The corresponding normalized screw axis :return theta: The distance traveled along/about S - - Example Input: + + Example Input: expc6 = np.array([1, 0, 0, 1, 2, 3]) Output: (np.array([1.0, 0.0, 0.0, 1.0, 2.0, 3.0]), 1.0) @@ -342,13 +342,13 @@ def AxisAng6(expc6): return (np.array(expc6 / theta), theta) def MatrixExp6(se3mat): - """Computes the matrix exponential of an se3 representation of + """Computes the matrix exponential of an se3 representation of exponential coordinates - + :param se3mat: A matrix in se3 :return: The matrix exponential of se3mat - - Example Input: + + Example Input: se3mat = np.array([[0, 0, 0, 0], [0, 0, -1.57079632, 2.35619449], [0, 1.57079632, 0, 2.35619449], @@ -357,7 +357,7 @@ def MatrixExp6(se3mat): np.array([[1.0, 0.0, 0.0, 0.0], [0.0, 0.0, -1.0, 0.0], [0.0, 1.0, 0.0, 3.0], - [ 0, 0, 0, 1]]) + [ 0, 0, 0, 1]]) """ se3mat = np.array(se3mat) omgtheta = so3ToVec(se3mat[0: 3, 0: 3]) @@ -380,8 +380,8 @@ def MatrixLog6(T): :param R: A matrix in SE3 :return: The matrix logarithm of R - Example Input: - T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], + Example Input: + T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]]) Output: np.array([[0, 0, 0, 0] @@ -394,34 +394,34 @@ def MatrixLog6(T): return np.r_[np.c_[np.zeros((3, 3)), [T[0][3], T[1][3], T[2][3]]], [[0, 0, 0, 0]]] - else: + else: acosinput = (np.trace(R) - 1) / 2.0 if acosinput > 1: acosinput = 1 elif acosinput < -1: - acosinput = -1 - theta = np.arccos(acosinput) - omgmat = MatrixLog3(R) - return np.r_[np.c_[omgmat, + acosinput = -1 + theta = np.arccos(acosinput) + omgmat = MatrixLog3(R) + return np.r_[np.c_[omgmat, np.dot(np.eye(3) - omgmat / 2.0 \ + (1.0 / theta - 1.0 / np.tan(theta / 2.0) / 2) \ - * np.dot(omgmat,omgmat) / theta,[T[0][3], - T[1][3], - T[2][3]])], + * np.dot(omgmat,omgmat) / theta,[T[0][3], + T[1][3], + T[2][3]])], [[0, 0, 0, 0]]] - - + + def ProjectToSO3(mat): """Returns a projection of mat into SO(3) - + :param mat: A matrix near SO(3) to project to SO(3) :return: The closest matrix to R that is in SO(3) Projects a matrix mat to the closest matrix in SO(3) using singular-value decomposition (see http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). This function is only appropriate for matrices close to SO(3). - - Example Input: + + Example Input: mat = np.array([[ 0.675, 0.150, 0.720], [ 0.370, 0.771, -0.511], [-0.630, 0.619, 0.472]]) @@ -430,13 +430,13 @@ def ProjectToSO3(mat): [ 0.37320708, 0.77319584, -0.51272279], [-0.63218672, 0.61642804, 0.46942137]]) """ - U, s, Vh = np.linalg.svd(mat) - R = np.dot(U, Vh) + U, s, Vh = np.linalg.svd(mat) + R = np.dot(U, Vh) if np.linalg.det(R) < 0: # In this case the result may be far from mat. - R[:, s[2, 2]] = -R[:, s[2, 2]] + R[:, s[2, 2]] = -R[:, s[2, 2]] return R - + def ProjectToSE3(mat): """Returns a projection of mat into SE(3) @@ -447,7 +447,7 @@ def ProjectToSE3(mat): http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review). This function is only appropriate for matrices close to SE(3). - Example Input: + Example Input: mat = np.array([[ 0.675, 0.150, 0.720, 1.2], [ 0.370, 0.771, -0.511, 5.4], [-0.630, 0.619, 0.472, 3.6], @@ -457,50 +457,50 @@ def ProjectToSE3(mat): [ 0.37320708, 0.77319584, -0.51272279, 5.4 ], [-0.63218672, 0.61642804, 0.46942137, 3.6 ], [ 0. , 0. , 0. , 1. ]]) - """ - mat = np.array(mat) + """ + mat = np.array(mat) return RpToTrans(ProjectToSO3(mat[:3, :3]), mat[:3, 3]) - + def DistanceToSO3(mat): """Returns the Frobenius norm to describe the distance of mat from the SO(3) manifold :param mat: A 3x3 matrix :return: A quantity describing the distance of mat from the SO(3) - manifold - Computes the distance from mat to the SO(3) manifold using the following - method: - If det(mat) <= 0, return a large number. + manifold + Computes the distance from mat to the SO(3) manifold using the following + method: + If det(mat) <= 0, return a large number. If det(mat) > 0, return norm(mat^T.mat - I). - - Example Input: + + Example Input: mat = np.array([[ 1.0, 0.0, 0.0 ], [ 0.0, 0.1, -0.95], [ 0.0, 1.0, 0.1 ]]) Output: - 0.08835 + 0.08835 """ if np.linalg.det(mat) > 0: return np.linalg.norm(np.dot(np.array(mat).T, mat) - np.eye(3)) else: return 1e+9 - + def DistanceToSE3(mat): """Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold :param mat: A 4x4 matrix :return: A quantity describing the distance of mat from the SE(3) - manifold - Computes the distance from mat to the SE(3) manifold using the following + manifold + Computes the distance from mat to the SE(3) manifold using the following method: - Compute the determinant of matR, the top 3x3 submatrix of mat. + Compute the determinant of matR, the top 3x3 submatrix of mat. If det(matR) <= 0, return a large number. If det(matR) > 0, replace the top 3x3 submatrix of mat with matR^T.matR, - and set the first three entries of the fourth column of mat to zero. Then + and set the first three entries of the fourth column of mat to zero. Then return norm(mat - I). - - Example Input: + + Example Input: mat = np.array([[ 1.0, 0.0, 0.0, 1.2 ], [ 0.0, 0.1, -0.95, 1.5 ], [ 0.0, 1.0, 0.1, -0.9 ], @@ -509,25 +509,25 @@ def DistanceToSE3(mat): 0.134931 """ matR = np.array(mat)[0: 3, 0: 3] - if np.linalg.det(matR) > 0: - return np.linalg.norm(np.r_[np.c_[np.dot(np.transpose(matR), matR), - np.zeros((3, 1))], - [np.array(mat)[3, :]]] - np.eye(4)) + if np.linalg.det(matR) > 0: + return np.linalg.norm(np.r_[np.c_[np.dot(np.transpose(matR), matR), + np.zeros((3, 1))], + [np.array(mat)[3, :]]] - np.eye(4)) else: return 1e+9 - + def TestIfSO3(mat): """Returns true if mat is close to or on the manifold SO(3) - + :param mat: A 3x3 matrix :return: True if mat is very close to or in SO(3), false otherwise - Computes the distance d from mat to the SO(3) manifold using the + Computes the distance d from mat to the SO(3) manifold using the following method: - If det(mat) <= 0, d = a large number. + If det(mat) <= 0, d = a large number. If det(mat) > 0, d = norm(mat^T.mat - I). If d is close to zero, return true. Otherwise, return false. - - Example Input: + + Example Input: mat = np.array([[1.0, 0.0, 0.0 ], [0.0, 0.1, -0.95], [0.0, 1.0, 0.1 ]]) @@ -535,84 +535,84 @@ def TestIfSO3(mat): False """ return abs(DistanceToSO3(mat)) < 1e-3 - + def TestIfSE3(mat): """Returns true if mat is close to or on the manifold SE(3) - + :param mat: A 3x3 matrix :return: True if mat is very close to or in SE(3), false otherwise - Computes the distance d from mat to the SE(3) manifold using the + Computes the distance d from mat to the SE(3) manifold using the following method: - Compute the determinant of the top 3x3 submatrix of mat. + Compute the determinant of the top 3x3 submatrix of mat. If det(mat) <= 0, d = a large number. If det(mat) > 0, replace the top 3x3 submatrix of mat with mat^T.mat, and - set the first three entries of the fourth column of mat to zero. - Then d = norm(T - I). + set the first three entries of the fourth column of mat to zero. + Then d = norm(T - I). If d is close to zero, return true. Otherwise, return false. - - Example Input: + + Example Input: mat = np.array([[1.0, 0.0, 0.0, 1.2], [0.0, 0.1, -0.95, 1.5], [0.0, 1.0, 0.1, -0.9], [0.0, 0.0, 0.1, 0.98]]) Output: - False + False """ return abs(DistanceToSE3(mat)) < 1e-3 - + ''' *** CHAPTER 4: FORWARD KINEMATICS *** ''' def FKinBody(M, Blist, thetalist): """Computes forward kinematics in the body frame for an open chain robot - + :param M: The home configuration (position and orientation) of the end- effector - :param Blist: The joint screw axes in the end-effector frame when the - manipulator is at the home position, in the format of a + :param Blist: The joint screw axes in the end-effector frame when the + manipulator is at the home position, in the format of a matrix with axes as the columns - :param thetalist: A list of joint coordinates + :param thetalist: A list of joint coordinates :return: A homogeneous transformation matrix representing the end- effector frame when the joints are at the specified coordinates (i.t.o Body Frame) - - Example Input: - M = np.array([[-1, 0, 0, 0], [0, 1, 0, 6], [0, 0, -1, 2], + + Example Input: + M = np.array([[-1, 0, 0, 0], [0, 1, 0, 6], [0, 0, -1, 2], [0, 0, 0, 1]]) Blist = np.array([[0, 0, -1, 2, 0, 0], - [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0.1]]).T thetalist = np.array([np.pi / 2.0, 3, np.pi]) Output: np.array([[0, 1, 0, -5], [1, 0, 0, 4], [0, 0, -1, 1.68584073], - [0, 0, 0, 1]]) + [0, 0, 0, 1]]) """ T = np.array(M) for i in range(len(thetalist)): T = np.dot(T, MatrixExp6(VecTose3(np.array(Blist)[:, i] \ - * thetalist[i]))) + * thetalist[i]))) return T def FKinSpace(M, Slist, thetalist): """Computes forward kinematics in the space frame for an open chain robot - + :param M: The home configuration (position and orientation) of the end- effector - :param Slist: The joint screw axes in the space frame when the - manipulator is at the home position, in the format of a + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a matrix with axes as the columns :param thetalist: A list of joint coordinates :return: A homogeneous transformation matrix representing the end- effector frame when the joints are at the specified coordinates (i.t.o Space Frame) - - Example Input: - M = np.array([[-1, 0, 0, 0], - [ 0, 1, 0, 6], - [ 0, 0, -1, 2], + + Example Input: + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], [ 0, 0, 0, 1]]) Slist = np.array([[0, 0, 1, 4, 0, 0], [0, 0, 0, 0, 1, 0], @@ -636,18 +636,18 @@ def FKinSpace(M, Slist, thetalist): def JacobianBody(Blist, thetalist): """Computes the body Jacobian for an open chain robot - + :param Blist: The joint screw axes in the end-effector frame when the - manipulator is at the home position, in the format of a + manipulator is at the home position, in the format of a matrix with axes as the columns :param thetalist: A list of joint coordinates - :return: The body Jacobian corresponding to the inputs (6xn real + :return: The body Jacobian corresponding to the inputs (6xn real numbers) - - Example Input: - Blist = np.array([[0, 0, 1, 0, 0.2, 0.2], - [1, 0, 0, 2, 0, 3], - [0, 1, 0, 0, 2, 1], + + Example Input: + Blist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], [1, 0, 0, 0.2, 0.3, 0.4]]).T thetalist = np.array([0.2, 1.1, 0.1, 1.2]) Output: @@ -663,25 +663,25 @@ def JacobianBody(Blist, thetalist): for i in range(len(thetalist) - 2, -1, -1): T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i + 1] \ * -thetalist[i + 1]))) - Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) + Jb[:, i] = np.dot(Adjoint(T), np.array(Blist)[:, i]) return Jb def JacobianSpace(Slist, thetalist): """Computes the space Jacobian for an open chain robot - - :param Slist: The joint screw axes in the space frame when the - manipulator is at the home position, in the format of a + + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a matrix with axes as the columns :param thetalist: A list of joint coordinates - :return: The space Jacobian corresponding to the inputs (6xn real + :return: The space Jacobian corresponding to the inputs (6xn real numbers) - - Example Input: - Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], - [1, 0, 0, 2, 0, 3], - [0, 1, 0, 0, 2, 1], + + Example Input: + Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], + [1, 0, 0, 2, 0, 3], + [0, 1, 0, 0, 2, 1], [1, 0, 0, 0.2, 0.3, 0.4]]).T - thetalist = np.array([0.2, 1.1, 0.1, 1.2]) + thetalist = np.array([0.2, 1.1, 0.1, 1.2]) Output: np.array([[ 0, 0.98006658, -0.09011564, 0.95749426] [ 0, 0.19866933, 0.4445544, 0.28487557] @@ -695,51 +695,51 @@ def JacobianSpace(Slist, thetalist): for i in range(1, len(thetalist)): T = np.dot(T, MatrixExp6(VecTose3(np.array(Slist)[:, i - 1] \ * thetalist[i - 1]))) - Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i]) + Js[:, i] = np.dot(Adjoint(T), np.array(Slist)[:, i]) return Js - + ''' -*** CHAPTER 6: INVERSE KINEMATICS *** +*** CHAPTER 6: INVERSE KINEMATICS *** ''' def IKinBody(Blist, M, T, thetalist0, eomg, ev): """Computes inverse kinematics in the body frame for an open chain robot - - :param Blist: The joint screw axes in the end-effector frame when the + + :param Blist: The joint screw axes in the end-effector frame when the manipulator is at the home position, in the format of a - matrix with axes as the columns + matrix with axes as the columns :param M: The home configuration of the end-effector :param T: The desired end-effector configuration Tsd - :param thetalist0: An initial guess of joint angles that are close to + :param thetalist0: An initial guess of joint angles that are close to satisfying Tsd - :param eomg: A small positive tolerance on the end-effector orientation - error. The returned joint angles must give an end-effector + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector orientation error less than eomg - :param ev: A small positive tolerance on the end-effector linear position - error. The returned joint angles must give an end-effector + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector position error less than ev :return thetalist: Joint angles that achieve T within the specified tolerances, - :return success: A logical value where TRUE means that the function found - a solution and FALSE means that it ran through the set + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set number of maximum iterations without finding a solution within the tolerances eomg and ev. Uses an iterative Newton-Raphson root-finding method. - The maximum number of iterations before the algorithm is terminated has - been hardcoded in as a variable called maxiterations. It is set to 20 at - the start of the function, but can be changed if needed. - - Example Input: + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. + + Example Input: Blist = np.array([[0, 0, -1, 2, 0, 0], - [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 1, 0], [0, 0, 1, 0, 0, 0.1]]).T - M = np.array([[-1, 0, 0, 0], - [ 0, 1, 0, 6], - [ 0, 0, -1, 2], + M = np.array([[-1, 0, 0, 0], + [ 0, 1, 0, 6], + [ 0, 0, -1, 2], [ 0, 0, 0, 1]]) - T = np.array([[0, 1, 0, -5], - [1, 0, 0, 4], - [0, 0, -1, 1.6858], + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], [0, 0, 0, 1]]) thetalist0 = np.array([1.5, 2.5, 3]) eomg = 0.01 @@ -769,44 +769,44 @@ def IKinBody(Blist, M, T, thetalist0, eomg, ev): def IKinSpace(Slist, M, T, thetalist0, eomg, ev): """Computes inverse kinematics in the space frame for an open chain robot - :param Slist: The joint screw axes in the space frame when the - manipulator is at the home position, in the format of a - matrix with axes as the columns + :param Slist: The joint screw axes in the space frame when the + manipulator is at the home position, in the format of a + matrix with axes as the columns :param M: The home configuration of the end-effector :param T: The desired end-effector configuration Tsd - :param thetalist0: An initial guess of joint angles that are close to + :param thetalist0: An initial guess of joint angles that are close to satisfying Tsd - :param eomg: A small positive tolerance on the end-effector orientation - error. The returned joint angles must give an end-effector + :param eomg: A small positive tolerance on the end-effector orientation + error. The returned joint angles must give an end-effector orientation error less than eomg - :param ev: A small positive tolerance on the end-effector linear position - error. The returned joint angles must give an end-effector + :param ev: A small positive tolerance on the end-effector linear position + error. The returned joint angles must give an end-effector position error less than ev - :return thetalist: Joint angles that achieve T within the specified + :return thetalist: Joint angles that achieve T within the specified tolerances, - :return success: A logical value where TRUE means that the function found - a solution and FALSE means that it ran through the set + :return success: A logical value where TRUE means that the function found + a solution and FALSE means that it ran through the set number of maximum iterations without finding a solution within the tolerances eomg and ev. Uses an iterative Newton-Raphson root-finding method. - The maximum number of iterations before the algorithm is terminated has - been hardcoded in as a variable called maxiterations. It is set to 20 at - the start of the function, but can be changed if needed. + The maximum number of iterations before the algorithm is terminated has + been hardcoded in as a variable called maxiterations. It is set to 20 at + the start of the function, but can be changed if needed. - Example Input: + Example Input: Slist = np.array([[0, 0, 1, 4, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, -1, -6, 0, -0.1]]).T M = np.array([[-1, 0, 0, 0], [0, 1, 0, 6], [0, 0, -1, 2], [0, 0, 0, 1]]) - T = np.array([[0, 1, 0, -5], - [1, 0, 0, 4], - [0, 0, -1, 1.6858], - [0, 0, 0, 1]]) - thetalist0 = np.array([1.5, 2.5, 3]) - eomg = 0.01 - ev = 0.001 + T = np.array([[0, 1, 0, -5], + [1, 0, 0, 4], + [0, 0, -1, 1.6858], + [0, 0, 0, 1]]) + thetalist0 = np.array([1.5, 2.5, 3]) + eomg = 0.01 + ev = 0.001 Output: - (np.array([ 1.57073783, 2.99966384, 3.1415342 ]), True) + (np.array([ 1.57073783, 2.99966384, 3.1415342 ]), True) """ thetalist = np.array(thetalist0).copy() i = 0 @@ -840,20 +840,20 @@ def ad(V): Used to calculate the Lie bracket [V1, V2] = [adV1]V2 - Example Input: - V = np.array([1, 2, 3, 4, 5, 6]) + Example Input: + V = np.array([1, 2, 3, 4, 5, 6]) Output: - np.array([[ 0, -3, 2, 0, 0, 0], - [ 3, 0, -1, 0, 0, 0], - [-2, 1, 0, 0, 0, 0], - [ 0, -6, 5, 0, -3, 2], - [ 6, 0, -4, 3, 0, -1], - [-5, 4, 0, -2, 1, 0]]) + np.array([[ 0, -3, 2, 0, 0, 0], + [ 3, 0, -1, 0, 0, 0], + [-2, 1, 0, 0, 0, 0], + [ 0, -6, 5, 0, -3, 2], + [ 6, 0, -4, 3, 0, -1], + [-5, 4, 0, -2, 1, 0]]) """ omgmat = VecToso3([V[0], V[1], V[2]]) - return np.r_[np.c_[omgmat, np.zeros((3, 3))], + return np.r_[np.c_[omgmat, np.zeros((3, 3))], np.c_[VecToso3([V[3], V[4], V[5]]), omgmat]] - + def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ Glist, Slist): """Computes inverse dynamics in the space frame for an open chain robot @@ -862,49 +862,49 @@ def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ :param dthetalist: n-vector of joint rates :param ddthetalist: n-vector of joint accelerations :param g: Gravity vector g - :param Ftip: Spatial force applied by the end-effector expressed in frame + :param Ftip: Spatial force applied by the end-effector expressed in frame {n+1} - :param Mlist: List of link frames {i} relative to {i-1} at the home + :param Mlist: List of link frames {i} relative to {i-1} at the home position :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format - of a matrix with axes as the columns + of a matrix with axes as the columns :return: The n-vector of required joint forces/torques - This function uses forward-backward Newton-Euler iterations to solve the + This function uses forward-backward Newton-Euler iterations to solve the equation: taulist = Mlist(thetalist)ddthetalist + c(thetalist,dthetalist) \ + g(thetalist) + Jtr(thetalist)Ftip - + Example Input (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - ddthetalist = np.array([2, 1.5, 1]) - g = np.array([0, 0, -9.8]) - Ftip = np.array([1, 1, 1, 1, 1, 1]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: np.array([74.69616155, -33.06766016, -3.23057314]) """ @@ -917,7 +917,7 @@ def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ Vdi[:, 0] = np.r_[[0, 0, 0], -np.array(g)] AdTi[n] = Adjoint(TransInv(Mlist[n])) Fi = np.array(Ftip).copy() - taulist = np.zeros(n) + taulist = np.zeros(n) for i in range(n): Mi = np.dot(Mi,Mlist[i]) Ai[:, i] = np.dot(Adjoint(TransInv(Mi)), np.array(Slist)[:, i]) @@ -937,7 +937,7 @@ def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \ return taulist def MassMatrix(thetalist, Mlist, Glist, Slist): - """Computes the mass matrix of an open chain robot based on the given + """Computes the mass matrix of an open chain robot based on the given configuration :param thetalist: A list of joint variables @@ -945,44 +945,44 @@ def MassMatrix(thetalist, Mlist, Glist, Slist): :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns - :return: The numerical inertia matrix M(thetalist) of an n-joint serial + :return: The numerical inertia matrix M(thetalist) of an n-joint serial chain at the given configuration thetalist - This function calls InverseDynamics n times, each time passing a - ddthetalist vector with a single element equal to one and all other - inputs set to zero. - Each call of InverseDynamics generates a single column, and these columns + This function calls InverseDynamics n times, each time passing a + ddthetalist vector with a single element equal to one and all other + inputs set to zero. + Each call of InverseDynamics generates a single column, and these columns are assembled to create the inertia matrix. - + Example Input (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: - np.array([[ 2.25433380e+01, -3.07146754e-01, -7.18426391e-03] - [-3.07146754e-01, 1.96850717e+00, 4.32157368e-01] - [-7.18426391e-03, 4.32157368e-01, 1.91630858e-01]]) + np.array([[ 2.25433380e+01, -3.07146754e-01, -7.18426391e-03] + [-3.07146754e-01, 1.96850717e+00, 4.32157368e-01] + [-7.18426391e-03, 4.32157368e-01, 1.91630858e-01]]) """ n = len(thetalist) M = np.zeros((n, n)) @@ -1006,38 +1006,38 @@ def VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist): of a matrix with axes as the columns. :return: The vector c(thetalist,dthetalist) of Coriolis and centripetal terms for a given thetalist and dthetalist. - This function calls InverseDynamics with g = 0, Ftip = 0, and + This function calls InverseDynamics with g = 0, Ftip = 0, and ddthetalist = 0. - + Example Input (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: - np.array([0.26453118, -0.05505157, -0.00689132]) + np.array([0.26453118, -0.05505157, -0.00689132]) """ return InverseDynamics(thetalist, dthetalist, [0] * len(thetalist), \ [0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, Glist, \ @@ -1053,38 +1053,38 @@ def GravityForces(thetalist, g, Mlist, Glist, Slist): :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns - :return grav: The joint forces/torques required to overcome gravity at + :return grav: The joint forces/torques required to overcome gravity at thetalist - This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and + This function calls InverseDynamics with Ftip = 0, dthetalist = 0, and ddthetalist = 0. - + Example Inputs (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - g = np.array([0, 0, -9.8]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: np.array([28.40331262, -37.64094817, -5.4415892]) """ @@ -1097,46 +1097,46 @@ def EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist): create the end-effector force Ftip :param thetalist: A list of joint variables - :param Ftip: Spatial force applied by the end-effector expressed in frame + :param Ftip: Spatial force applied by the end-effector expressed in frame {n+1} :param Mlist: List of link frames i relative to i-1 at the home position :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns - :return: The joint forces and torques required only to create the + :return: The joint forces and torques required only to create the end-effector force Ftip - This function calls InverseDynamics with g = 0, dthetalist = 0, and + This function calls InverseDynamics with g = 0, dthetalist = 0, and ddthetalist = 0. - + Example Input (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - Ftip = np.array([1, 1, 1, 1, 1, 1]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: - np.array([1.40954608, 1.85771497, 1.392409]) + np.array([1.40954608, 1.85771497, 1.392409]) """ n = len(thetalist) return InverseDynamics(thetalist, [0] * n, [0] * n, [0, 0, 0], Ftip, \ @@ -1145,7 +1145,7 @@ def EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist): def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \ Glist, Slist): """Computes forward dynamics in the space frame for an open chain robot - + :param thetalist: A list of joint variables :param dthetalist: A list of joint rates :param taulist: An n-vector of joint forces/torques @@ -1160,39 +1160,39 @@ def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \ This function computes ddthetalist by solving: Mlist(thetalist) * ddthetalist = taulist - c(thetalist,dthetalist) \ - g(thetalist) - Jtr(thetalist) * Ftip - + Example Input (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - taulist = np.array([0.5, 0.6, 0.7]) - g = np.array([0, 0, -9.8]) - Ftip = np.array([1, 1, 1, 1, 1, 1]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taulist = np.array([0.5, 0.6, 0.7]) + g = np.array([0, 0, -9.8]) + Ftip = np.array([1, 1, 1, 1, 1, 1]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T Output: - np.array([-0.97392907, 25.58466784, -32.91499212]) + np.array([-0.97392907, 25.58466784, -32.91499212]) """ return np.dot(np.linalg.inv(MassMatrix(thetalist, Mlist, Glist, \ Slist)), \ @@ -1202,8 +1202,8 @@ def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \ - GravityForces(thetalist, g, Mlist, Glist, Slist) \ - EndEffectorForces(thetalist, Ftip, Mlist, Glist, \ Slist)) - -def EulerStep(thetalist, dthetalist, ddthetalist, dt): + +def EulerStep(thetalist, dthetalist, ddthetalist, dt): """Compute the joint angles and velocities at the next timestep using from here first order Euler integration @@ -1211,28 +1211,28 @@ def EulerStep(thetalist, dthetalist, ddthetalist, dt): :param dthetalist: n-vector of joint rates :param ddthetalist: n-vector of joint accelerations :param dt: The timestep delta t - :return thetalistNext: Vector of joint variables after dt from first - order Euler integration + :return thetalistNext: Vector of joint variables after dt from first + order Euler integration :return dthetalistNext: Vector of joint rates after dt from first order Euler integration - + Example Inputs (3 Link Robot): - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - ddthetalist = np.array([2, 1.5, 1]) - dt = 0.1 + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + ddthetalist = np.array([2, 1.5, 1]) + dt = 0.1 Output: -thetalistNext: -array([ 0.11, 0.12, 0.13]) -dthetalistNext: -array([ 0.3 , 0.35, 0.4 ]) + thetalistNext: + array([ 0.11, 0.12, 0.13]) + dthetalistNext: + array([ 0.3 , 0.35, 0.4 ]) """ return thetalist + dt * np.array(dthetalist), \ dthetalist + dt * np.array(ddthetalist) def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ Ftipmat, Mlist, Glist, Slist): - """Calculates the joint forces/torques required to move the serial chain + """Calculates the joint forces/torques required to move the serial chain along the given trajectory using inverse dynamics :param thetamat: An N x n matrix of robot joint variables @@ -1240,87 +1240,89 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ :param ddthetamat: An N x n matrix of robot joint accelerations :param g: Gravity vector g :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- - effector (If there are no tip forces the user should + effector (If there are no tip forces the user should input a zero and a zero matrix will be used) :param Mlist: List of link frames i relative to i-1 at the home position :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns :return: The N x n matrix of joint forces/torques for the specified - trajectory, where each of the N rows is the vector of joint - forces/torques at each time step - + trajectory, where each of the N rows is the vector of joint + forces/torques at each time step + Example Inputs (3 Link Robot): - import modern_robotics as mr - #Create a trajectory to follow using functions from Chapter 9 - thetastart = np.array([0, 0, 0]) - thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) - Tf = 3 - N= 1000 - method = 5 - traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) - thetamat = np.array(traj).copy() - dthetamat = np.zeros((1000,3 )) - ddthetamat = np.zeros((1000, 3)) - dt = Tf / (N - 1.0) - for i in range(np.array(traj).shape[0] - 1): - dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt - ddthetamat[i + 1, :] \ - = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt - #Initialise robot descripstion (Example with 3 links) - g = np.array([0, 0, -9.8]) - Ftipmat = np.ones((N, 6)) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T - taumat \ - = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ + from __future__ import print_function + import numpy as np + import modern_robotics as mr + # Create a trajectory to follow using functions from Chapter 9 + thetastart = np.array([0, 0, 0]) + thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) + Tf = 3 + N= 1000 + method = 5 + traj = mr.JointTrajectory(thetastart, thetaend, Tf, N, method) + thetamat = np.array(traj).copy() + dthetamat = np.zeros((1000,3 )) + ddthetamat = np.zeros((1000, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt + ddthetamat[i + 1, :] \ + = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((N, 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + taumat \ + = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ Ftipmat, Mlist, Glist, Slist) - #Output using matplotlib to plot the joint forces/torques - Tau1 = taumat[:, 0] - Tau2 = taumat[:, 1] - Tau3 = taumat[:, 2] - timestamp = np.linspace(0, Tf, N) - try: - import matplotlib.pyplot as plt - except: - print('The result will not be plotted due to a lack of package matplotlib') - else: - plt.plot(timestamp, Tau1, label = "Tau1") - plt.plot(timestamp, Tau2, label = "Tau2") - plt.plot(timestamp, Tau3, label = "Tau3") - plt.ylim (-40, 120) - plt.legend(loc = 'lower right') - plt.xlabel("Time") - plt.ylabel("Torque") - plt.title("Plot of Torque Trajectories") - plt.show() + # Output using matplotlib to plot the joint forces/torques + Tau1 = taumat[:, 0] + Tau2 = taumat[:, 1] + Tau3 = taumat[:, 2] + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, Tau1, label = "Tau1") + plt.plot(timestamp, Tau2, label = "Tau2") + plt.plot(timestamp, Tau3, label = "Tau3") + plt.ylim (-40, 120) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Torque") + plt.title("Plot of Torque Trajectories") + plt.show() """ thetamat = np.array(thetamat).T dthetamat = np.array(dthetamat).T ddthetamat = np.array(ddthetamat).T - Ftipmat = np.array(Ftipmat).T + Ftipmat = np.array(Ftipmat).T taumat = np.array(thetamat).copy() for i in range(np.array(thetamat).shape[1]): taumat[:, i] \ @@ -1337,96 +1339,98 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \ :param thetalist: n-vector of initial joint variables :param dthetalist: n-vector of initial joint rates - :param taumat: An N x n matrix of joint forces/torques, where each row is + :param taumat: An N x n matrix of joint forces/torques, where each row is the joint effort at any time step :param g: Gravity vector g :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- - effector (If there are no tip forces the user should + effector (If there are no tip forces the user should input a zero and a zero matrix will be used) - :param Mlist: List of link frames {i} relative to {i-1} at the home - position + :param Mlist: List of link frames {i} relative to {i-1} at the home + position :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns :param dt: The timestep between consecutive joint forces/torques - :param intRes: Integration resolution is the number of times integration - (Euler) takes places between each time step. Must be an + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an integer value greater than or equal to 1 - :return thetamat: The N x n matrix of robot joint angles resulting from + :return thetamat: The N x n matrix of robot joint angles resulting from the specified joint forces/torques :return dthetamat: The N x n matrix of robot joint velocities - This function calls a numerical integration procedure that uses + This function calls a numerical integration procedure that uses ForwardDynamics. - + Example Inputs (3 Link Robot): - import modern_robotics as mr - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5], - [4.31, -0.68, -5.19], [5.18, 5.63, -4.31], - [5.85, 8.17, -2.59], [5.78, 2.79, -1.7], - [4.99, -5.3, -1.19], [4.08, -9.41, 0.07], - [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]) - #Initialise robot description (Example with 3 links) - g = np.array([0, 0, -9.8]) - Ftipmat = np.ones((np.array(taumat).shape[0], 6)) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T - dt = 0.1 - intRes = 8 - thetamat,dthetamat \ - = mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \ - Ftipmat, Mlist, Glist, Slist, dt, \ - intRes) - #Output using matplotlib to plot the joint angle/velocities - theta1 = thetamat[:, 0] - theta2 = thetamat[:, 1] - theta3 = thetamat[:, 2] - dtheta1 = dthetamat[:, 0] - dtheta2 = dthetamat[:, 1] - dtheta3 = dthetamat[:, 2] - N = np.array(taumat).shape[0] - Tf = np.array(taumat).shape[0] * dt - timestamp = np.linspace(0, Tf, N) - try: - import matplotlib.pyplot as plt - except: - print(The result will not be plotted due to a lack of package matplotlib) - else: - plt.plot(timestamp, theta1, label = "Theta1") - plt.plot(timestamp, theta2, label = "Theta2") - plt.plot(timestamp, theta3, label = "Theta3") - plt.plot(timestamp, dtheta1, label = "DTheta1") - plt.plot(timestamp, dtheta2, label = "DTheta2") - plt.plot(timestamp, dtheta3, label = "DTheta3") - plt.ylim (-12, 10) - plt.legend(loc = 'lower right') - plt.xlabel("Time") - plt.ylabel("Joint Angles/Velocities") - plt.title("Plot of Joint Angles and Joint Velocities") - plt.show() + from __future__ import print_function + import numpy as np + import modern_robotics as mr + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5], + [4.31, -0.68, -5.19], [5.18, 5.63, -4.31], + [5.85, 8.17, -2.59], [5.78, 2.79, -1.7], + [4.99, -5.3, -1.19], [4.08, -9.41, 0.07], + [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + Ftipmat = np.ones((np.array(taumat).shape[0], 6)) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.1 + intRes = 8 + thetamat,dthetamat \ + = mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \ + Ftipmat, Mlist, Glist, Slist, dt, \ + intRes) + # Output using matplotlib to plot the joint angle/velocities + theta1 = thetamat[:, 0] + theta2 = thetamat[:, 1] + theta3 = thetamat[:, 2] + dtheta1 = dthetamat[:, 0] + dtheta2 = dthetamat[:, 1] + dtheta3 = dthetamat[:, 2] + N = np.array(taumat).shape[0] + Tf = np.array(taumat).shape[0] * dt + timestamp = np.linspace(0, Tf, N) + try: + import matplotlib.pyplot as plt + except: + print('The result will not be plotted due to a lack of package matplotlib') + else: + plt.plot(timestamp, theta1, label = "Theta1") + plt.plot(timestamp, theta2, label = "Theta2") + plt.plot(timestamp, theta3, label = "Theta3") + plt.plot(timestamp, dtheta1, label = "DTheta1") + plt.plot(timestamp, dtheta2, label = "DTheta2") + plt.plot(timestamp, dtheta3, label = "DTheta3") + plt.ylim (-12, 10) + plt.legend(loc = 'lower right') + plt.xlabel("Time") + plt.ylabel("Joint Angles/Velocities") + plt.title("Plot of Joint Angles and Joint Velocities") + plt.show() """ taumat = np.array(taumat).T Ftipmat = np.array(Ftipmat).T @@ -1434,7 +1438,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \ thetamat[:, 0] = thetalist dthetamat = taumat.copy().astype(np.float) dthetamat[:, 0] = dthetalist - for i in range(np.array(taumat).shape[1] - 1): + for i in range(np.array(taumat).shape[1] - 1): for j in range(intRes): ddthetalist \ = ForwardDynamics(thetalist, dthetalist, taumat[:, i], g, \ @@ -1456,14 +1460,14 @@ def CubicTimeScaling(Tf, t): :param Tf: Total time of the motion in seconds from rest to rest :param t: The current time t satisfying 0 < t < Tf - :return: The path parameter s(t) corresponding to a third-order - polynomial motion that begins and ends at zero velocity - - Example Input: - Tf = 2 - t = 0.6 + :return: The path parameter s(t) corresponding to a third-order + polynomial motion that begins and ends at zero velocity + + Example Input: + Tf = 2 + t = 0.6 Output: - 0.216 + 0.216 """ return 3 * (1.0 * t / Tf) ** 2 - 2 * (1.0 * t / Tf) ** 3 @@ -1472,15 +1476,15 @@ def QuinticTimeScaling(Tf, t): :param Tf: Total time of the motion in seconds from rest to rest :param t: The current time t satisfying 0 < t < Tf - :return: The path parameter s(t) corresponding to a fifth-order - polynomial motion that begins and ends at zero velocity and zero + :return: The path parameter s(t) corresponding to a fifth-order + polynomial motion that begins and ends at zero velocity and zero acceleration - - Example Input: - Tf = 2 - t = 0.6 + + Example Input: + Tf = 2 + t = 0.6 Output: - 0.16308 + 0.16308 """ return 10 * (1.0 * t / Tf) ** 3 - 15 * (1.0 * t / Tf) ** 4 \ + 6 * (1.0 * t / Tf) ** 5 @@ -1494,26 +1498,26 @@ def JointTrajectory(thetastart, thetaend, Tf, N, method): :param N: The number of points N > 1 (Start and stop) in the discrete representation of the trajectory :param method: The time-scaling method, where 3 indicates cubic (third- - order polynomial) time scaling and 5 indicates quintic - (fifth-order polynomial) time scaling + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling :return: A trajectory as an N x n matrix, where each row is an n-vector - of joint variables at an instant in time. The first row is - thetastart and the Nth row is thetaend . The elapsed time - between each row is Tf / (N - 1) - - Example Input: - thetastart = np.array([1, 0, 0, 1, 1, 0.2, 0,1]) - thetaend = np.array([1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]) - Tf = 4 - N = 6 - method = 3 + of joint variables at an instant in time. The first row is + thetastart and the Nth row is thetaend . The elapsed time + between each row is Tf / (N - 1) + + Example Input: + thetastart = np.array([1, 0, 0, 1, 1, 0.2, 0,1]) + thetaend = np.array([1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]) + Tf = 4 + N = 6 + method = 3 Output: - np.array([[ 1, 0, 0, 1, 1, 0.2, 0, 1] - [1.0208, 0.052, 0.0624, 1.0104, 1.104, 0.3872, 0.0936, 1] - [1.0704, 0.176, 0.2112, 1.0352, 1.352, 0.8336, 0.3168, 1] - [1.1296, 0.324, 0.3888, 1.0648, 1.648, 1.3664, 0.5832, 1] - [1.1792, 0.448, 0.5376, 1.0896, 1.896, 1.8128, 0.8064, 1] - [ 1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]]) + np.array([[ 1, 0, 0, 1, 1, 0.2, 0, 1] + [1.0208, 0.052, 0.0624, 1.0104, 1.104, 0.3872, 0.0936, 1] + [1.0704, 0.176, 0.2112, 1.0352, 1.352, 0.8336, 0.3168, 1] + [1.1296, 0.324, 0.3888, 1.0648, 1.648, 1.3664, 0.5832, 1] + [1.1792, 0.448, 0.5376, 1.0896, 1.896, 1.8128, 0.8064, 1] + [ 1.2, 0.5, 0.6, 1.1, 2, 2, 0.9, 1]]) """ N = int(N) timegap = Tf / (N - 1.0) @@ -1522,13 +1526,13 @@ def JointTrajectory(thetastart, thetaend, Tf, N, method): if method == 3: s = CubicTimeScaling(Tf, timegap * i) else: - s = QuinticTimeScaling(Tf, timegap * i) + s = QuinticTimeScaling(Tf, timegap * i) traj[:, i] = s * np.array(thetaend) + (1 - s) * np.array(thetastart) traj = np.array(traj).T return traj def ScrewTrajectory(Xstart, Xend, Tf, N, method): - """Computes a trajectory as a list of N SE(3) matrices corresponding to + """Computes a trajectory as a list of N SE(3) matrices corresponding to the screw motion about a space screw axis :param Xstart: The initial end-effector configuration @@ -1537,41 +1541,41 @@ def ScrewTrajectory(Xstart, Xend, Tf, N, method): :param N: The number of points N > 1 (Start and stop) in the discrete representation of the trajectory :param method: The time-scaling method, where 3 indicates cubic (third- - order polynomial) time scaling and 5 indicates quintic - (fifth-order polynomial) time scaling - :return: The discretized trajectory as a list of N matrices in SE(3) + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling + :return: The discretized trajectory as a list of N matrices in SE(3) separated in time by Tf/(N-1). The first in the list is Xstart - and the Nth is Xend + and the Nth is Xend - Example Input: - Xstart = np.array([[1, 0, 0, 1], - [0, 1, 0, 0], - [0, 0, 1, 1], - [0, 0, 0, 1]]) - Xend = np.array([[0, 0, 1, 0.1], - [1, 0, 0, 0], - [0, 1, 0, 4.1], - [0, 0, 0, 1]]) - Tf = 5 - N = 4 - method = 3 + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 3 Output: - [np.array([[1, 0, 0, 1] - [0, 1, 0, 0] - [0, 0, 1, 1] - [0, 0, 0, 1]]), - np.array([[0.904, -0.25, 0.346, 0.441] - [0.346, 0.904, -0.25, 0.529] - [-0.25, 0.346, 0.904, 1.601] - [ 0, 0, 0, 1]]), - np.array([[0.346, -0.25, 0.904, -0.117] - [0.904, 0.346, -0.25, 0.473] - [-0.25, 0.904, 0.346, 3.274] - [ 0, 0, 0, 1]]), - np.array([[0, 0, 1, 0.1] - [1, 0, 0, 0] - [0, 1, 0, 4.1] - [0, 0, 0, 1]])] + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[0.904, -0.25, 0.346, 0.441] + [0.346, 0.904, -0.25, 0.529] + [-0.25, 0.346, 0.904, 1.601] + [ 0, 0, 0, 1]]), + np.array([[0.346, -0.25, 0.904, -0.117] + [0.904, 0.346, -0.25, 0.473] + [-0.25, 0.904, 0.346, 3.274] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] """ N = int(N) timegap = Tf / (N - 1.0) @@ -1580,60 +1584,60 @@ def ScrewTrajectory(Xstart, Xend, Tf, N, method): if method == 3: s = CubicTimeScaling(Tf, timegap * i) else: - s = QuinticTimeScaling(Tf, timegap * i) + s = QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.dot(Xstart, MatrixExp6(MatrixLog6(np.dot(TransInv(Xstart), \ Xend)) * s)) return traj def CartesianTrajectory(Xstart, Xend, Tf, N, method): - """Computes a trajectory as a list of N SE(3) matrices corresponding to + """Computes a trajectory as a list of N SE(3) matrices corresponding to the origin of the end-effector frame following a straight line :param Xstart: The initial end-effector configuration :param Xend: The final end-effector configuration :param Tf: Total time of the motion in seconds from rest to rest - :param N: The number of points N > 1 (Start and stop) in the discrete + :param N: The number of points N > 1 (Start and stop) in the discrete representation of the trajectory :param method: The time-scaling method, where 3 indicates cubic (third- - order polynomial) time scaling and 5 indicates quintic - (fifth-order polynomial) time scaling + order polynomial) time scaling and 5 indicates quintic + (fifth-order polynomial) time scaling :return: The discretized trajectory as a list of N matrices in SE(3) separated in time by Tf/(N-1). The first in the list is Xstart - and the Nth is Xend - This function is similar to ScrewTrajectory, except the origin of the - end-effector frame follows a straight line, decoupled from the rotational + and the Nth is Xend + This function is similar to ScrewTrajectory, except the origin of the + end-effector frame follows a straight line, decoupled from the rotational motion. - - Example Input: - Xstart = np.array([[1, 0, 0, 1], - [0, 1, 0, 0], - [0, 0, 1, 1], - [0, 0, 0, 1]]) - Xend = np.array([[0, 0, 1, 0.1], - [1, 0, 0, 0], - [0, 1, 0, 4.1], - [0, 0, 0, 1]]) - Tf = 5 - N = 4 - method = 5 + + Example Input: + Xstart = np.array([[1, 0, 0, 1], + [0, 1, 0, 0], + [0, 0, 1, 1], + [0, 0, 0, 1]]) + Xend = np.array([[0, 0, 1, 0.1], + [1, 0, 0, 0], + [0, 1, 0, 4.1], + [0, 0, 0, 1]]) + Tf = 5 + N = 4 + method = 5 Output: - [np.array([[1, 0, 0, 1] - [0, 1, 0, 0] - [0, 0, 1, 1] - [0, 0, 0, 1]]), - np.array([[ 0.937, -0.214, 0.277, 0.811] - [ 0.277, 0.937, -0.214, 0] - [-0.214, 0.277, 0.937, 1.651] - [ 0, 0, 0, 1]]), - np.array([[ 0.277, -0.214, 0.937, 0.289] - [ 0.937, 0.277, -0.214, 0] - [-0.214, 0.937, 0.277, 3.449] - [ 0, 0, 0, 1]]), - np.array([[0, 0, 1, 0.1] - [1, 0, 0, 0] - [0, 1, 0, 4.1] - [0, 0, 0, 1]])] + [np.array([[1, 0, 0, 1] + [0, 1, 0, 0] + [0, 0, 1, 1] + [0, 0, 0, 1]]), + np.array([[ 0.937, -0.214, 0.277, 0.811] + [ 0.277, 0.937, -0.214, 0] + [-0.214, 0.277, 0.937, 1.651] + [ 0, 0, 0, 1]]), + np.array([[ 0.277, -0.214, 0.937, 0.289] + [ 0.937, 0.277, -0.214, 0] + [-0.214, 0.937, 0.277, 3.449] + [ 0, 0, 0, 1]]), + np.array([[0, 0, 1, 0.1] + [1, 0, 0, 0] + [0, 1, 0, 4.1] + [0, 0, 0, 1]])] """ N = int(N) timegap = Tf / (N - 1.0) @@ -1644,7 +1648,7 @@ def CartesianTrajectory(Xstart, Xend, Tf, N, method): if method == 3: s = CubicTimeScaling(Tf, timegap * i) else: - s = QuinticTimeScaling(Tf, timegap * i) + s = QuinticTimeScaling(Tf, timegap * i) traj[i] \ = np.r_[np.c_[np.dot(Rstart, \ MatrixExp3(MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \ @@ -1664,57 +1668,57 @@ def ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, \ :param dthetalist: n-vector of joint rates :param eint: n-vector of the time-integral of joint errors :param g: Gravity vector g - :param Mlist: List of link frames {i} relative to {i-1} at the home - position + :param Mlist: List of link frames {i} relative to {i-1} at the home + position :param Glist: Spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format - of a matrix with axes as the columns + of a matrix with axes as the columns :param thetalistd: n-vector of reference joint variables :param dthetalistd: n-vector of reference joint velocities :param ddthetalistd: n-vector of reference joint accelerations :param Kp: The feedback proportional gain (identical for each joint) :param Ki: The feedback integral gain (identical for each joint) :param Kd: The feedback derivative gain (identical for each joint) - :return: The vector of joint forces/torques computed by the feedback - linearizing controller at the current instant - - Example Input: - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - eint = np.array([0.2, 0.2, 0.2]) - g = np.array([0, 0, -9.8]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T - thetalistd = np.array([1.0, 1.0, 1.0]) - dthetalistd = np.array([2, 1.2, 2]) - ddthetalistd = np.array([0.1, 0.1, 0.1]) - Kp = 1.3 - Ki = 1.2 - Kd = 1.1 + :return: The vector of joint forces/torques computed by the feedback + linearizing controller at the current instant + + Example Input: + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + eint = np.array([0.2, 0.2, 0.2]) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + thetalistd = np.array([1.0, 1.0, 1.0]) + dthetalistd = np.array([2, 1.2, 2]) + ddthetalistd = np.array([0.1, 0.1, 0.1]) + Kp = 1.3 + Ki = 1.2 + Kd = 1.1 Output: - np.array([133.00525246, -29.94223324, -3.03276856]) + np.array([133.00525246, -29.94223324, -3.03276856]) """ e = np.subtract(thetalistd, thetalist) return np.dot(MassMatrix(thetalist, Mlist, Glist, Slist), \ @@ -1726,123 +1730,125 @@ def ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, \ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \ Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, \ Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes): - """Simulates the computed torque controller over a given desired + """Simulates the computed torque controller over a given desired trajectory :param thetalist: n-vector of initial joint variables :param dthetalist: n-vector of initial joint velocities :param g: Actual gravity vector g :param Ftipmat: An N x 6 matrix of spatial forces applied by the end- - effector (If there are no tip forces the user should - input a zero and a zero matrix will be used) - :param Mlist: Actual list of link frames i relative to i-1 at the home + effector (If there are no tip forces the user should + input a zero and a zero matrix will be used) + :param Mlist: Actual list of link frames i relative to i-1 at the home position :param Glist: Actual spatial inertia matrices Gi of the links :param Slist: Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns - :param thetamatd: An Nxn matrix of desired joint variables from the + :param thetamatd: An Nxn matrix of desired joint variables from the reference trajectory :param dthetamatd: An Nxn matrix of desired joint velocities :param ddthetamatd: An Nxn matrix of desired joint accelerations - :param gtilde: The gravity vector based on the model of the actual robot + :param gtilde: The gravity vector based on the model of the actual robot (actual values given above) - :param Mtildelist: The link frame locations based on the model of the + :param Mtildelist: The link frame locations based on the model of the actual robot (actual values given above) - :param Gtildelist: The link spatial inertias based on the model of the + :param Gtildelist: The link spatial inertias based on the model of the actual robot (actual values given above) :param Kp: The feedback proportional gain (identical for each joint) :param Ki: The feedback integral gain (identical for each joint) :param Kd: The feedback derivative gain (identical for each joint) :param dt: The timestep between points on the reference trajectory - :param intRes: Integration resolution is the number of times integration - (Euler) takes places between each time step. Must be an + :param intRes: Integration resolution is the number of times integration + (Euler) takes places between each time step. Must be an integer value greater than or equal to 1 :return taumat: An Nxn matrix of the controllers commanded joint forces/ - torques, where each row of n forces/torques corresponds - to a single time instant + torques, where each row of n forces/torques corresponds + to a single time instant :return thetamat: An Nxn matrix of actual joint angles - The end of this function plots all the actual and desired joint angles + The end of this function plots all the actual and desired joint angles using matplotlib and random libraries. - - Example Input: - from modern_robotics import JointTrajectory - thetalist = np.array([0.1, 0.1, 0.1]) - dthetalist = np.array([0.1, 0.2, 0.3]) - #Initialise robot description (Example with 3 links) - g = np.array([0, 0, -9.8]) - M01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.089159], - [0, 0, 0, 1]]) - M12 = np.array([[ 0, 0, 1, 0.28], - [ 0, 1, 0, 0.13585], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - M23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.1197], - [0, 0, 1, 0.395], - [0, 0, 0, 1]]) - M34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.14225], - [0, 0, 0, 1]]) - G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) - G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) - G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) - Glist = np.array([G1, G2, G3]) - Mlist = np.array([M01, M12, M23, M34]) - Slist = np.array([[1, 0, 1, 0, 1, 0], - [0, 1, 0, -0.089, 0, 0], - [0, 1, 0, -0.089, 0, 0.425]]).T - dt = 0.01 - #Create a trajectory to follow - thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi]) - Tf = 1 - N = int(1.0 * Tf / dt) - method = 5 - traj = mr.JointTrajectory(thetalist, thetaend, Tf, N, method) - thetamatd = np.array(traj).copy() - dthetamatd = np.zeros((N, 3)) - ddthetamatd = np.zeros((N, 3)) - dt = Tf / (N - 1.0) - for i in range(np.array(traj).shape[0] - 1): - dthetamatd[i + 1, :] \ + + Example Input: + from __future__ import print_function + import numpy as np + from modern_robotics import JointTrajectory + thetalist = np.array([0.1, 0.1, 0.1]) + dthetalist = np.array([0.1, 0.2, 0.3]) + # Initialize robot description (Example with 3 links) + g = np.array([0, 0, -9.8]) + M01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.089159], + [0, 0, 0, 1]]) + M12 = np.array([[ 0, 0, 1, 0.28], + [ 0, 1, 0, 0.13585], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + M23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.1197], + [0, 0, 1, 0.395], + [0, 0, 0, 1]]) + M34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.14225], + [0, 0, 0, 1]]) + G1 = np.diag([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7]) + G2 = np.diag([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393]) + G3 = np.diag([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275]) + Glist = np.array([G1, G2, G3]) + Mlist = np.array([M01, M12, M23, M34]) + Slist = np.array([[1, 0, 1, 0, 1, 0], + [0, 1, 0, -0.089, 0, 0], + [0, 1, 0, -0.089, 0, 0.425]]).T + dt = 0.01 + # Create a trajectory to follow + thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi]) + Tf = 1 + N = int(1.0 * Tf / dt) + method = 5 + traj = mr.JointTrajectory(thetalist, thetaend, Tf, N, method) + thetamatd = np.array(traj).copy() + dthetamatd = np.zeros((N, 3)) + ddthetamatd = np.zeros((N, 3)) + dt = Tf / (N - 1.0) + for i in range(np.array(traj).shape[0] - 1): + dthetamatd[i + 1, :] \ = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt - ddthetamatd[i + 1, :] \ - = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt - #Possibly wrong robot description (Example with 3 links) - gtilde = np.array([0.8, 0.2, -8.8]) - Mhat01 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.1], - [0, 0, 0, 1]]) - Mhat12 = np.array([[ 0, 0, 1, 0.3], - [ 0, 1, 0, 0.2], - [-1, 0, 0, 0], - [ 0, 0, 0, 1]]) - Mhat23 = np.array([[1, 0, 0, 0], - [0, 1, 0, -0.2], - [0, 0, 1, 0.4], - [0, 0, 0, 1]]) - Mhat34 = np.array([[1, 0, 0, 0], - [0, 1, 0, 0], - [0, 0, 1, 0.2], - [0, 0, 0, 1]]) - Ghat1 = np.diag([0.1, 0.1, 0.1, 4, 4, 4]) - Ghat2 = np.diag([0.3, 0.3, 0.1, 9, 9, 9]) - Ghat3 = np.diag([0.1, 0.1, 0.1, 3, 3, 3]) - Gtildelist = np.array([Ghat1, Ghat2, Ghat3]) - Mtildelist = np.array([Mhat01, Mhat12, Mhat23, Mhat34]) - Ftipmat = np.ones((np.array(traj).shape[0], 6)) - Kp = 20 - Ki = 10 - Kd = 18 - intRes = 8 - taumat,thetamat \ - = mr.SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, \ - Glist, Slist, thetamatd, dthetamatd, \ - ddthetamatd, gtilde, Mtildelist, Gtildelist, \ - Kp, Ki, Kd, dt, intRes) + ddthetamatd[i + 1, :] \ + = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt + # Possibly wrong robot description (Example with 3 links) + gtilde = np.array([0.8, 0.2, -8.8]) + Mhat01 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.1], + [0, 0, 0, 1]]) + Mhat12 = np.array([[ 0, 0, 1, 0.3], + [ 0, 1, 0, 0.2], + [-1, 0, 0, 0], + [ 0, 0, 0, 1]]) + Mhat23 = np.array([[1, 0, 0, 0], + [0, 1, 0, -0.2], + [0, 0, 1, 0.4], + [0, 0, 0, 1]]) + Mhat34 = np.array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0.2], + [0, 0, 0, 1]]) + Ghat1 = np.diag([0.1, 0.1, 0.1, 4, 4, 4]) + Ghat2 = np.diag([0.3, 0.3, 0.1, 9, 9, 9]) + Ghat3 = np.diag([0.1, 0.1, 0.1, 3, 3, 3]) + Gtildelist = np.array([Ghat1, Ghat2, Ghat3]) + Mtildelist = np.array([Mhat01, Mhat12, Mhat23, Mhat34]) + Ftipmat = np.ones((np.array(traj).shape[0], 6)) + Kp = 20 + Ki = 10 + Kd = 18 + intRes = 8 + taumat,thetamat \ + = mr.SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, \ + Glist, Slist, thetamatd, dthetamatd, \ + ddthetamatd, gtilde, Mtildelist, Gtildelist, \ + Kp, Ki, Kd, dt, intRes) """ Ftipmat = np.array(Ftipmat).T thetamatd = np.array(thetamatd).T @@ -1868,8 +1874,8 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \ 1.0 * dt / intRes) taumat[:, i] = taulist thetamat[:, i] = thetacurrent - eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent)) - #Output using matplotlib to plot + eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent)) + # Output using matplotlib to plot try: import matplotlib.pyplot as plt except: @@ -1892,5 +1898,5 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \ plt.title("Plot of Actual and Desired Joint Angles") plt.show() taumat = np.array(taumat).T - thetamat = np.array(thetamat).T + thetamat = np.array(thetamat).T return (taumat, thetamat)