Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Modern_Robotics/code/Python/modern_robotics.py

1827 lines
70 KiB
Python
Raw Blame History

'''
***************************************************************************
Library of functions written to accompany the algorithms described in
Modern Robotics: Mechanics, Planning, and Control.
***************************************************************************
Author: Mikhail Todes, Huan Weng, Bill Hunt
Date: June 2018
***************************************************************************
Language: Python
Also available in: MATLAB, Mathematica
Included libraries: numpy, matplotlib
***************************************************************************
'''
'''
*** IMPORTS ***
'''
import numpy as np
import matplotlib.pyplot as plt
'''
*** BASIC HELPER FUNCTIONS ***
'''
def NearZero(z):
"""Determines whether a scalar is small enough to be treated as zero
Takes a scalar; checks if the scalar is small enough to be neglected.
Example Input:
z = -1e-7
Output:
True
:param z: A scalar input to check
:returns: True if z is close to zero, false otherwise
"""
return abs(z) < 1e-6
def Normalize(V):
"""Normalizes a vector
Accepts a vector; returns the unit vector pointing in the same direction as the input.
Example Input:
V = np.array([1, 2, 3])
Output:
np.array([0.2672612419124244, 0.5345224838248488, 0.8017837257372732])
:param z: A vector
:returns: A unit vector pointing in the same direction as z
"""
return V / np.linalg.norm(V)
'''
*** CHAPTER 3: RIGID-BODY MOTIONS ***
'''
def RotInv(R):
"""Inverts a rotation matrix
Accepts a rotation matrix; returns the inverse of that matrix.
Example Input:
R = np.array([[0, 0, 1],
[1, 0, 0],
[0, 1, 0]])
Output:
[[0, 1, 0],
[0, 0, 1],
[1, 0, 0]]
:param R: A rotation matrix
:returns: The inverse of R
"""
return np.array(R).T
def VecToso3(omg):
"""Converts a 3-vector to an so3 representation
Takes a 3-vector (angular velocity).
Returns the skew symmetric matrix in so3.
Example Input:
omg = np.array([1, 2, 3])
Output:
[[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]]
:param omg: A 3-vector
:returns: The skew symmetric representation of omg
"""
return np.array([[0, -omg[2], omg[1]],
[omg[2], 0, -omg[0]],
[-omg[1], omg[0], 0]])
def so3ToVec(so3mat):
"""Converts an so3 representation to a 3-vector
Takes a 3x3 skew-symmetric matrix (an element of so(3)).
Returns the corresponding vector (angular velocity).
Example Input:
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
Output:
[1, 2, 3]
:param so3mat: A 3x3 skew-symmetric matrix
:returns: The 3-vector corresponding to 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 axis-angle form
Takes A 3-vector of exponential coordinates for rotation.
Returns unit rotation axis omghat and the corresponding rotation angle theta.
Example Input:
expc3 = np.array([1, 2, 3])
Output:
(array([0.2672612419124244, 0.5345224838248488, 0.8017837257372732]), 3.7416573867739413)
:param expc3: A 3-vector of exponential coordinates for rotation
:returns: A unit rotation axis and a rotation angle
"""
return (Normalize(expc3), np.linalg.norm(expc3))
def MatrixExp3(so3mat):
"""Computes the matrix exponential of a matrix in so3
Takes a so(3) representation of exponential coordinates.
Returns R in SO(3) that is achieved by rotating about omghat by theta from
an initial orientation R = I.
Example Input:
so3mat = np.array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
Output:
[[-0.69492056, 0.71352099, 0.08929286],
[-0.19200697, -0.30378504, 0.93319235],
[ 0.69297817, 0.6313497 , 0.34810748]]
:param so3mat: A 3x3 skew-symmetric matrix
:returns: The matrix exponential of so3mat
"""
omgtheta = so3ToVec(so3mat)
if NearZero(np.linalg.norm(omgtheta)):
return np.eye(3)
else:
theta = AxisAng3(omgtheta)[1]
omgmat = so3mat / theta
return np.eye(3) + np.sin(theta) * omgmat \
+ (1 - np.cos(theta)) * np.dot(omgmat, omgmat)
def MatrixLog3(R):
"""Computes the matrix logarithm of a rotation matrix
Takes R (rotation matrix).
Returns the corresponding so(3) representation of exponential coordinates.
Example Input:
R = np.array([[0, 0, 1],
[1, 0, 0],
[0, 1, 0]])
Output:
[[ 0, -1.20919958, 1.20919958],
[ 1.20919958, 0, -1.20919958],
[-1.20919958, 1.20919958, 0]]
:param R: A 3x3 rotation matrix
:returns: The matrix logarithm of R
"""
if NearZero(np.linalg.norm(R - np.eye(3))):
return np.zeros((3, 3))
elif NearZero(np.trace(R) + 1):
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]):
omg = (1.0 / np.sqrt(2 * (1 + R[1][1]))) \
* np.array([R[0][1], 1 + R[1][1], R[2][1]])
else:
omg = (1.0 / np.sqrt(2 * (1 + R[0][0]))) \
* np.array([1 + R[0][0], R[1][0], R[2][0]])
return VecToso3(np.pi * omg)
else:
acosinput = (np.trace(R) - 1) / 2.0
if acosinput > 1:
acosinput = 1
elif acosinput < -1:
acosinput = -1
theta = np.arccos(acosinput)
return theta / 2.0 / np.sin(theta) * (R - np.array(R).T)
def RpToTrans(R, p):
"""Converts a rotation matrix and a position vector into homogeneous tranformation matrix
Takes rotation matrix R and position p.
Returns corresponding homogeneous transformation matrix T in SE(3).
Example Input:
R = np.array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]])
p = np.array([1, 2, 5])
Output:
[[1, 0, 0, 1],
[0, 0, -1, 2],
[0, 1, 0, 5],
[0, 0, 0, 1]]
:param R: A 3x3 rotation matrix
:param p: A 3-vector
:returns: A homogeneous transformation matrix corresponding to the inputs
"""
return np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
def TransToRp(T):
"""Converts a homogeneous transformation matrix into a rotation matrix and position vector
Takes transformation matrix T in SE(3).
Returns R: The corresponding rotation matrix,
p: The corresponding position vector.
Example Input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
(array([[1, 0, 0],
[0, 0, -1],
[0, 1, 0]]),
array([0, 0, 3]))
:param T: A homogeneous transformation matrix
:returns: A rotation matrix and a position vector, corresponding to the input
"""
R = np.array([[T[0][0], T[0][1], T[0][2]],
[T[1][0], T[1][1], T[1][2]],
[T[2][0], T[2][1], T[2][2]]])
return R, np.array([T[0][3], T[1][3], T[2][3]])
def TransInv(T):
#Takes a transformation matrix T.
#Returns its inverse.
#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],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
[[1, 0, 0, 0],
[0, 0, 1, -3],
[0, -1, 0, 0],
[0, 0, 0, 1]]
'''
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):
#Takes a 6-vector (representing a spatial velocity).
#Returns the corresponding 4x4 se(3) matrix.
'''
Example Input:
V = np.array([1, 2, 3, 4, 5, 6])
Output:
[[ 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]]],
np.zeros((1, 4))]
def se3ToVec(se3mat):
#Takes se3mat a 4x4 se(3) matrix.
#Returns the corresponding 6-vector (representing spatial velocity).
'''
Example Input:
se3mat = np.array([[ 0, -3, 2, 4],
[ 3, 0, -1, 5],
[-2, 1, 0, 6],
[ 0, 0, 0, 0]])
Output:
[1, 2, 3, 4, 5, 6]
'''
return np.r_[[se3mat[2][1], se3mat[0][2], se3mat[1][0]],
[se3mat[0][3], se3mat[1][3], se3mat[2][3]]]
def Adjoint(T):
#Takes T a transformation matrix SE(3).
#Returns the corresponding 6x6 adjoint representation [AdT].
'''
Example Input:
T = np.array([[1, 0, 0, 0],
[0, 0, -1, 0],
[0, 1, 0, 3],
[0, 0, 0, 1]])
Output:
[[1, 0, 0, 0, 0, 0],
[0, 0, -1, 0, 0, 0],
[0, 1, 0, 0, 0, 0],
[0, 0, 3, 1, 0, 0],
[3, 0, 0, 0, 0, -1],
[0, 0, 0, 0, 1, 0]]
'''
R, p = TransToRp(T)
return np.r_[np.c_[R, np.zeros((3, 3))],
np.c_[np.dot(VecToso3(p), R), R]]
def ScrewToAxis(q, s, h):
#Takes q: A point lying on the screw axis,
# s: A unit vector in the direction of the screw axis,
# h: The pitch of the screw axis.
#Returns the corresponding normalized screw axis.
'''
Example Input:
q = np.array([3, 0, 0])
s = np.array([0, 0, 1])
h = 2
Output:
[0, 0, 1, 0, -3, 2]
'''
return np.r_[s, np.cross(q, s) + np.dot(h, s)]
def AxisAng6(expc6):
#Takes a 6-vector of exponential coordinates for rigid-body motion S*theta.
#Returns S: The corresponding normalized screw axis,
# theta: The distance traveled along/about S.
'''
Example Input:
expc6 = np.array([1, 0, 0, 1, 2, 3])
Output:
([1.0, 0.0, 0.0, 1.0, 2.0, 3.0],
1.0)
'''
theta = np.linalg.norm([expc6[0], expc6[1], expc6[2]])
if NearZero(theta):
theta = np.linalg.norm([expc6[3], expc6[4], expc6[5]])
return (np.array(expc6 / theta), theta)
def MatrixExp6(se3mat):
#Takes a se(3) representation of exponential coordinates.
#Returns a T matrix SE(3) that is achieved by traveling along/about the
#screw axis S for a distance theta from an initial configuration T = I.
'''
Example Input:
se3mat = np.array([[0, 0, 0, 0],
[0, 0, -1.57079632, 2.35619449],
[0, 1.57079632, 0, 2.35619449],
[0, 0, 0, 0]])
Output:
[[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]]
'''
omgtheta = so3ToVec(np.array(se3mat)[0: 3: 1, 0: 3: 1])
if NearZero(np.linalg.norm(omgtheta)):
return np.r_[np.c_[np.eye(3),
[se3mat[0][3], se3mat[1][3], se3mat[2][3]]],
[[0, 0, 0, 1]]]
else:
theta = AxisAng3(omgtheta)[1]
omgmat = np.array(se3mat)[0: 3, 0: 3] / theta
return np.r_[np.c_[MatrixExp3(np.array(se3mat)[0: 3: 1, 0: 3: 1]),
np.dot(np.eye(3) * theta \
+ (1 - np.cos(theta)) * omgmat \
+ (theta - np.sin(theta)) \
* np.dot(omgmat,omgmat),
[se3mat[0][3],
se3mat[1][3],
se3mat[2][3]]) / theta],
[[0, 0, 0, 1]]]
def MatrixLog6(T):
#Takes a transformation matrix T in SE(3).
#Returns the corresponding se(3) representation of exponential coordinates.
'''
Example Input:
T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]])
Output:
[[ 0. 0. 0. 0. ]
[ 0. 0. -1.57079633 2.35619449]
[ 0. 1.57079633 0. 2.35619449]
[ 0. 0. 0. 0. ]]
'''
R, p = TransToRp(T)
if NearZero(np.linalg.norm(R - np.eye(3))):
return np.r_[np.c_[np.zeros((3, 3)),
[T[0][3], T[1][3], T[2][3]]],
[[0, 0, 0, 0]]]
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,
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]])],
[[0, 0, 0, 0]]]
def ProjectToSO3 (R):
"""Returns a projection of R into SO3
Projects a matrix R to the closest matrix in SO3
using singular-value decomposition (see
http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review).
Example Input:
R = np.array([[ 0.675, 0.150, 0.720],
[ 0.370, 0.771, -0.511],
[-0.630, 0.619, 0.472]])
Output:
np.array([[ 0.67901136, 0.14894516, 0.71885945],
[ 0.37320708, 0.77319584, -0.51272279],
[-0.63218672, 0.61642804, 0.46942137]])
:param R: A matrix near SO3 to project to SO3
:returns: The closest matrix to R that is in SO3
"""
U,s,Vh = np.linalg.svd(R)
result = np.dot(U,Vh)
if np.linalg.det(result) < 0:
result[:,np.argmin(s)] = -result[:,np.argmin(s)]
return result
def ProjectToSE3 (T):
"""Returns a projection of T into SE3
Projects a matrix T to the closest matrix
in SE3 using singular-value decomposition (see
http://hades.mech.northwestern.edu/index.php/Modern_Robotics_Linear_Algebra_Review).
Example Input:
T = 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],
[ 0.003, 0.002, 0.010, 0.9]])
Output:
np.array([[ 0.67901136, 0.14894516, 0.71885945, 1.2 ],
[ 0.37320708, 0.77319584, -0.51272279, 5.4 ],
[-0.63218672, 0.61642804, 0.46942137, 3.6 ],
[ 0. , 0. , 0. , 1. ]])
:param T: A 4x4 matrix to project to SE3
:returns: The closest matrix to T that is in SE3
"""
a=np.array(T)
R = ProjectToSO3(a[:3,:3])
return mr.RpToTrans(R,a[:3,3])
def DistanceToSO3 (R):
"""Returns a quantity describing the distance of R from the SO3 manifold
Computes the distance from R to the SO3 manifold using the following method:
If det(R) <= 0, return a large number. If det(R) > 0, return norm(R^T.R - I).
Example Input:
np.array([[ 1.0, 0.0, 0.0 ],
[ 0.0, 0.1, -0.95 ],
[ 0.0, 1.0, 0.1 ]])
Output:
0.08835
:param R: A 3x3 matrix
:returns: A quantity describing the distance of R from the SO3 manifold
"""
if np.linalg.det(R) > 0:
return np.linalg.norm(np.dot(np.transpose(R),np.array(R)) - np.identity(3))
else:
return 1000000000.
def DistanceToSE3 (T):
"""Returns a quantity describing the distance of T from the SE3 manifold
Computes the distance from T to the SO3 manifold using the following method:
Compute the determinant of R, the top 3x3 submatrix of T. If det(R) <= 0, return a large number.
If det(R) > 0, replace the top 3x3 submatrix of T with R^T.R, and set the first three entries of
the fourth column of T to zero. Then return norm(T - I).
Example Input:
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:
0.134931
:param R: A 4x4 matrix
:returns: A quantity describing the distance of R from the SO3 manifold
"""
T = np.array(T)
if np.linalg.det(T[:3,:3]) > 0:
A = np.vstack([np.c_[np.dot(np.transpose(T[:3,:3]),T[:3,:3]),np.zeros((3,1))] , T[3,:]])
return np.linalg.norm(A - np.identity(4))
else:
return 1000000000.
def TestIfSO3 (R):
"""Returns true if R is close to or on the manifold SO3
Computes the distance d from R to the SO3 manifold using the following method:
If det(R) <= 0, d = a large number. If det(R) > 0, d = norm(R^T.R - I).
if d is close to zero, return true. Otherwise, return false
Example Input:
np.array([[ 1.0, 0.0, 0.0 ],
[ 0.0, 0.1, -0.95 ],
[ 0.0, 1.0, 0.1 ]])
Output:
False
:param R: A 3x3 matrix
:returns: True if R is very close to or in SO3, false otherwise
"""
return NearZero(DistanceToSO3(R))
def TestIfSE3 (T):
"""Returns true if T is close to or on the manifold SE3
Computes the distance d from T to the SE3 manifold using the following method:
Compute the determinant of R, the top 3x3 submatrix of T. If det(R) <= 0, d = a large number.
If det(R) > 0, replace the top 3x3 submatrix of T with R^T.R, and set the first three entries of
the fourth column of T to zero. Then d = norm(T - I).
If d is close to zero, return true. Otherwise, return false.
Example Input:
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
:param R: A 3x3 matrix
:returns: True if R is very close to or in SE3, false otherwise
"""
return NearZero(DistanceToSE3(T))
'''
*** CHAPTER 4: FORWARD KINEMATICS ***
'''
def FKinBody(M, Blist, thetalist):
#Takes M: The home configuration (position and orientation) of the
# end-effector,
# 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,
# thetalist: A list of joint coordinates.
#Returns T IN SE(3) 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], [0, 0, 0, 1]])
Blist = np.array([[0, 0, -1, 2, 0, 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:
[[ -1.14423775e-17 1.00000000e+00 0.00000000e+00 -5.00000000e+00],
[ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00],
[ 0. 0. -1. 1.68584073],
[ 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])))
return T
def FKinSpace(M, Slist, thetalist):
#Takes M: the home configuration (position and orientation) of the
# end-effector,
# 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,
# thetalist: A list of joint coordinates.
#Returns T in SE(3) 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], [0, 0, 0, 1]])
Slist = np.array([[0, 0, 1, 4, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, -1, -6, 0, -0.1]]).T
thetalist = np.array([np.pi / 2.0, 3, np.pi])
Output:
[[ -1.14423775e-17 1.00000000e+00 0.00000000e+00 -5.00000000e+00],
[ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00],
[ 0. 0. -1. 1.68584073],
[ 0. 0. 0. 1.]]
'''
T = np.array(M)
for i in range(len(thetalist)-1, -1, -1):
T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \
* thetalist[i])), T)
return T
'''
*** CHAPTER 5: VELOCITY KINEMATICS AND STATICS***
'''
def JacobianBody(Blist, thetalist):
#Takes 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,
# thetalist: A list of joint coordinates.
#Returns the corresponding body Jacobian (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],
[1, 0, 0, 0.2, 0.3, 0.4]]).T
thetalist = np.array([0.2, 1.1, 0.1, 1.2])
Output:
[[-0.04528405 0.99500417 0. 1. ]
[ 0.74359313 0.09304865 0.36235775 0. ]
[-0.66709716 0.03617541 -0.93203909 0. ]
[ 2.32586047 1.66809 0.56410831 0.2]
[-1.44321167 2.94561275 1.43306521 0.3]
[-2.06639565 1.82881722 -1.58868628 0.4]]
'''
Jb = np.array(Blist).copy()
T = np.eye(4)
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])
return Jb
def JacobianSpace(Slist, thetalist):
#Takes 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,
# thetalist: A list of joint coordinates.
#Returns the corresponding space Jacobian (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],
[1, 0, 0, 0.2, 0.3, 0.4]]).T
thetalist = np.array([0.2, 1.1, 0.1, 1.2])
Output:
[[ 0. 0.98006658 -0.09011564 0.95749426]
[ 0. 0.19866933 0.4445544 0.28487557]
[ 1. 0. 0.89120736 -0.04528405]
[ 0. 1.95218638 -2.21635216 -0.51161537]
[ 0.2 0.43654132 -2.43712573 2.77535713]
[ 0.2 2.96026613 3.23573065 2.22512443]]
'''
Js = np.array(Slist).copy()
T = np.eye(4)
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])
return Js
'''
*** CHAPTER 6: INVERSE KINEMATICS ***
'''
def IKinBody(Blist, M, T, thetalist0, eomg, ev):
#Takes 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,
# M: The home configuration of the end-effector,
# T: The desired end-effector configuration Tsd,
# thetalist0: An initial guess of joint angles that are close to
# satisfying Tsd,
# 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,
# 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.
#Returns thetalist: Joint angles that achieve T within the specified
# tolerances,
# 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:
Blist = np.array([[0, 0, -1, 2, 0, 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], [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
Output:
thetalist:
array([1.57073819, 2.999667, 3.14153913])
success:
True
'''
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Vb = se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(JacobianBody(Blist, \
thetalist)), Vb)
i = i + 1
Vb \
= se3ToVec(MatrixLog6(np.dot(TransInv(FKinBody(M, Blist, \
thetalist)), T)))
err = np.linalg.norm([Vb[0], Vb[1], Vb[2]]) > eomg \
or np.linalg.norm([Vb[3], Vb[4], Vb[5]]) > ev
return (thetalist, not err)
def IKinSpace(Slist, M, T, thetalist0, eomg, ev):
#Takes 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,
# M: The home configuration of the end-effector,
# T: The desired end-effector configuration Tsd,
# thetalist0: An initial guess of joint angles that are close to
# satisfying Tsd,
# 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,
# 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.
#Returns thetalist: Joint angles that achieve T within the specified
# tolerances,
# 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:
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
Output:
thetalist:
array([1.57073785, 2.99966405, 3.14154125])
success:
True
'''
thetalist = np.array(thetalist0).copy()
i = 0
maxiterations = 20
Tsb = FKinSpace(M,Slist, thetalist)
Vs = np.dot(Adjoint(Tsb), \
se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T))))
err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \
or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev
while err and i < maxiterations:
thetalist = thetalist \
+ np.dot(np.linalg.pinv(JacobianSpace(Slist, \
thetalist)), Vs)
i = i + 1
Tsb = FKinSpace(M, Slist, thetalist)
Vs = np.dot(Adjoint(Tsb), \
se3ToVec(MatrixLog6(np.dot(TransInv(Tsb), T))))
err = np.linalg.norm([Vs[0], Vs[1], Vs[2]]) > eomg \
or np.linalg.norm([Vs[3], Vs[4], Vs[5]]) > ev
return (thetalist, not err)
'''
*** CHAPTER 8: DYNAMICS OF OPEN CHAINS ***
'''
def ad(V):
#Takes 6-vector spatial velocity.
#Returns the corresponding 6x6 matrix [adV].
#Used to calculate the Lie bracket [V1, V2] = [adV1]V2
'''
Example Input:
V = np.array([1, 2, 3, 4, 5, 6])
Output:
[[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))],
np.c_[VecToso3([V[3], V[4], V[5]]), omgmat]]
def InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, \
Glist, Slist):
#Takes thetalist: n-vector of joint variables,
# dthetalist: n-vector of joint rates,
# ddthetalist: n-vector of joint accelerations,
# g: Gravity vector g,
# Ftip: Spatial force applied by the end-effector expressed in frame
# {n+1},
# Mlist: List of link frames {i} relative to {i-1} at the home
# position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,.
#Returns taulist: The n-vector of required joint forces/torques.
#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
Output:
[ 74.69616155 -33.06766016 -3.23057314]
'''
n = len(thetalist)
Mi = np.eye(4)
Ai = np.zeros((6, n))
AdTi = [[None]] * (n + 1)
Vi = np.zeros((6, n + 1))
Vdi = np.zeros((6, n + 1))
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)
for i in range(n):
Mi = np.dot(Mi,Mlist[i])
Ai[:, i] = np.dot(Adjoint(TransInv(Mi)), np.array(Slist)[:, i])
AdTi[i] = Adjoint(np.dot(MatrixExp6(VecTose3(Ai[:, i] * \
-thetalist[i])), \
TransInv(Mlist[i])))
Vi[:, i + 1] = np.dot(AdTi[i], Vi[:,i]) + Ai[:, i] * dthetalist[i]
Vdi[:, i + 1] = np.dot(AdTi[i], Vdi[:, i]) \
+ Ai[:, i] * ddthetalist[i] \
+ np.dot(ad(Vi[:, i + 1]), Ai[:, i]) * dthetalist[i]
for i in range (n - 1, -1, -1):
Fi = np.dot(np.array(AdTi[i + 1]).T, Fi) \
+ np.dot(np.array(Glist[i]), Vdi[:, i + 1]) \
- np.dot(np.array(ad(Vi[:, i + 1])).T, \
np.dot(np.array(Glist[i]), Vi[:, i + 1]))
taulist[i] = np.dot(np.array(Fi).T, Ai[:, i])
return taulist
def MassMatrix(thetalist, Mlist, Glist, Slist):
#Takes thetalist: A list of joint variables,
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns.
#Returns M: 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
#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
Output:
[[ 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))
for i in range (n):
ddthetalist = [0] * n
ddthetalist[i] = 1
M[:, i] = InverseDynamics(thetalist,[0] * n, ddthetalist, \
[0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, \
Glist, Slist)
return M
def VelQuadraticForces(thetalist, dthetalist, Mlist, Glist, Slist):
#Takes thetalist: A list of joint variables,
# dthetalist: A list of joint rates,
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns.
#Returns c: 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
#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
Output:
[ 0.26453118 -0.05505157 -0.00689132]
'''
return InverseDynamics(thetalist, dthetalist, [0] * len(thetalist), \
[0, 0, 0], [0, 0, 0, 0, 0, 0], Mlist, Glist, \
Slist)
def GravityForces(thetalist, g, Mlist, Glist, Slist):
#Takes thetalist: A list of joint variables,
# g: 3-vector for gravitational acceleration,
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,.
#Returns grav: The joint forces/torques required to overcome gravity at
# thetalist
#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
Output:
[ 28.40331262 -37.64094817 -5.4415892]
'''
n = len(thetalist)
return InverseDynamics(thetalist, [0] * n, [0] * n, g, \
[0, 0, 0, 0, 0, 0], Mlist, Glist, Slist)
def EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist):
#Takes thetalist: A list of joint variables,
# Ftip: Spatial force applied by the end-effector expressed in frame
# {n+1},
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,.
#Returns JTFtip: The joint forces and torques required only to create the
# end-effector force Ftip.
#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
Output:
[ 1.40954608 1.85771497 1.392409]
'''
n = len(thetalist)
return InverseDynamics(thetalist, [0] * n, [0] * n, [0, 0, 0], Ftip, \
Mlist, Glist, Slist)
def ForwardDynamics(thetalist, dthetalist, taulist, g, Ftip, Mlist, \
Glist, Slist):
#Takes thetalist: A list of joint variables,
# dthetalist: A list of joint rates,
# taulist: An n-vector of joint forces/torques,
# g: Gravity vector g,
# Ftip: Spatial force applied by the end-effector expressed in frame
# {n+1},
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,.
#Returns ddthetalist: The resulting joint accelerations.
#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
Output:
[ -0.97392907 25.58466784 -32.91499212]
'''
return np.dot(np.linalg.inv(MassMatrix(thetalist, Mlist, Glist, \
Slist)), \
np.array(taulist) \
- VelQuadraticForces(thetalist, dthetalist, Mlist, \
Glist, Slist) \
- GravityForces(thetalist, g, Mlist, Glist, Slist) \
- EndEffectorForces(thetalist, Ftip, Mlist, Glist, \
Slist))
def EulerStep(thetalist, dthetalist, ddthetalist, dt):
#Takes thetalist: n-vector of joint variables,
# dthetalist: n-vector of joint rates,
# ddthetalist: n-vector of joint accelerations,
# dt: The timestep delta t.
#Returns thetalistNext: Vector of joint variables after dt from first order
# Euler integration,
# 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
Output:
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):
#Takes thetamat: An N x n matrix of robot joint variables,
# dthetamat: An N x n matrix of robot joint velocities,
# ddthetamat: An N x n matrix of robot joint accelerations,
# g: Gravity vector g,
# 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),
# Mlist: List of link frames i relative to i-1 at the home position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,.
#Returns taumat: 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.
#This function uses InverseDynamics to calculate the joint forces/torques
#required to move the serial chain along the given trajectory.
'''
#Example Inputs (3 Link Robot):
from modern_robotics import JointTrajectory
import matplotlib.pyplot as plt
#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 = 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 = 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)
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
taumat = np.array(thetamat).copy()
for i in range(np.array(thetamat).shape[1]):
taumat[:, i] \
= InverseDynamics(thetamat[:, i], dthetamat[:, i], \
ddthetamat[:, i], g, Ftipmat[:, i], Mlist, \
Glist, Slist)
taumat = np.array(taumat).T
return taumat
def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
Mlist, Glist, Slist, dt, intRes):
#Takes thetalist: n-vector of initial joint variables,
# dthetalist: n-vector of initial joint rates,
# taumat: An N x n matrix of joint forces/torques, where each row is
# the joint effort at any time step,
# g: Gravity vector g,
# 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),
# Mlist: List of link frames {i} relative to {i-1} at the home
# position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,
# dt: The timestep between consecutive joint forces/torques,
# 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
#Returns thetamat: The N x n matrix of robot joint angles resulting from
# the specified joint forces/torques,
# dthetamat: The N x n matrix of robot joint velocities.
#This function simulates the motion of a serial chain given an open-loop
#history of joint forces/torques.
#It calls a numerical integration procedure that uses ForwardDynamics.
'''
#Example Inputs (3 Link Robot):
import matplotlib.pyplot as plt
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 \
= 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)
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
thetamat = taumat.copy()
thetamat[:, 0] = thetalist
dthetamat = taumat.copy()
dthetamat[:, 0] = dthetalist
for i in range(np.array(taumat).shape[1] - 1):
for j in range(intRes):
ddthetalist \
= ForwardDynamics(thetalist, dthetalist, taumat[:, i], g, \
Ftipmat[:, i], Mlist, Glist, Slist)
thetalist,dthetalist = EulerStep(thetalist, dthetalist, \
ddthetalist, 1.0 * dt / intRes)
thetamat[:, i + 1] = thetalist
dthetamat[:, i + 1] = dthetalist
thetamat = np.array(thetamat).T
dthetamat = np.array(dthetamat).T
return thetamat, dthetamat
'''
*** CHAPTER 9: TRAJECTORY GENERATION ***
'''
def CubicTimeScaling(Tf, t):
#Takes Tf: Total time of the motion in seconds from rest to rest,
# t: The current time t satisfying 0 < t < Tf.
#Returns s: 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
'''
return 3 * (1.0 * t / Tf) ** 2 - 2 * (1.0 * t / Tf) ** 3
def QuinticTimeScaling(Tf, t):
#Takes Tf: Total time of the motion in seconds from rest to rest,
# t: The current time t satisfying 0 < t < Tf.
#Returns s: 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
Output:
0.16308
'''
return 10 * (1.0 * t / Tf) ** 3 - 15 * (1.0 * t / Tf) ** 4 \
+ 6 * (1.0 * t / Tf) ** 5
def JointTrajectory(thetastart, thetaend, Tf, N, method):
#Takes thetastart: The initial joint variables,
# thetaend: The final joint variables,
# Tf: Total time of the motion in seconds from rest to rest,
# N: The number of points N > 1 (Start and stop) in the discrete
# representation of the trajectory,
# method: The time-scaling method, where 3 indicates cubic
# (third-order polynomial) time scaling and 5 indicates
# quintic (fifth-order polynomial) time scaling.
#Returns traj: 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).
#The returned trajectory is a straight-line motion in joint space.
'''
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:
[[ 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)
traj = np.zeros((len(thetastart), N))
for i in range(N):
if method == 3:
s = CubicTimeScaling(Tf, timegap * i)
else:
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):
#Takes Xstart: The initial end-effector configuration,
# Xend: The final end-effector configuration,
# Tf: Total time of the motion in seconds from rest to rest,
# N: The number of points N > 1 (Start and stop) in the discrete
# representation of the trajectory,
# method: The time-scaling method, where 3 indicates cubic
# (third-order polynomial) time scaling and 5 indicates
# quintic (fifth-order polynomial) time scaling.
#Returns traj: 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 calculates a trajectory corresponding to the screw motion
#about a space screw axis.
'''
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:
[array([[ 1. 0. 0. 1. ]
[ 0. 1. 0. 0. ]
[ 0. 0. 1. 1. ]
[ 0. 0. 0. 1. ]]),
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. ]]),
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. ]]),
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)
traj = [[None]] * N
for i in range(N):
if method == 3:
s = CubicTimeScaling(Tf, timegap * i)
else:
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):
#Takes Xstart: The initial end-effector configuration,
# Xend: The final end-effector configuration,
# Tf: Total time of the motion in seconds from rest to rest,
# N: The number of points N > 1 (Start and stop) in the discrete
# representation of the trajectory,
# method: The time-scaling method, where 3 indicates cubic
# (third-order polynomial) time scaling and 5 indicates
# quintic (fifth-order polynomial) time scaling.
#Returns traj: 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
#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
Output:
[array([[ 1. 0. 0. 1. ]
[ 0. 1. 0. 0. ]
[ 0. 0. 1. 1. ]
[ 0. 0. 0. 1. ]]),
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. ]]),
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. ]]),
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)
traj = [[None]] * N
Rstart, pstart = TransToRp(Xstart)
Rend, pend = TransToRp(Xend)
for i in range(N):
if method == 3:
s = CubicTimeScaling(Tf, timegap * i)
else:
s = QuinticTimeScaling(Tf, timegap * i)
traj[i] \
= np.r_[np.c_[np.dot(Rstart, \
MatrixExp3(MatrixLog3(np.dot(np.array(Rstart).T,Rend)) * s)), \
s * np.array(pend) + (1 - s) * np.array(pstart)], \
[[0, 0, 0, 1]]]
return traj
'''
*** CHAPTER 11: ROBOT CONTROL ***
'''
def ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, \
thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd):
#Takes thetalist: n-vector of joint variables,
# dthetalist: n-vector of joint rates,
# eint: n-vector of the time-integral of joint errors,
# g: Gravity vector g,
# Mlist: List of link frames {i} relative to {i-1} at the home
# position,
# Glist: Spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,
# thetalistd: n-vector of reference joint variables,
# dthetalistd: n-vector of reference joint velocities,
# ddthetalistd: n-vector of reference joint accelerations,
# Kp: The feedback proportional gain (identical for each joint),
# Ki: The feedback integral gain (identical for each joint),
# Kd: The feedback derivative gain (identical for each joint).
#Returns taulist: 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:
[ 133.00525246 -29.94223324 -3.03276856]
'''
e = np.subtract(thetalistd, thetalist)
return np.dot(MassMatrix(thetalist, Mlist, Glist, Slist), \
Kp * e + Ki * (np.array(eint) + e) \
+ Kd * np.subtract(dthetalistd, dthetalist)) \
+ InverseDynamics(thetalist, dthetalist, ddthetalistd, g, \
[0, 0, 0, 0, 0, 0], Mlist, Glist, Slist)
def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
Slist, thetamatd, dthetamatd, ddthetamatd, gtilde, \
Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes):
#Takes thetalist: n-vector of initial joint variables,
# dthetalist: n-vector of initial joint velocities,
# g: Actual gravity vector g,
# 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),
# Mlist: Actual list of link frames i relative to i-1 at the home
# position,
# Glist: Actual spatial inertia matrices Gi of the links,
# Slist: Screw axes Si of the joints in a space frame, in the format
# of a matrix with axes as the columns,
# thetamatd: An Nxn matrix of desired joint variables from the
# reference trajectory,
# dthetamatd: An Nxn matrix of desired joint velocities,
# ddthetamatd: An Nxn matrix of desired joint accelerations,
# gtilde: The gravity vector based on the model of the actual robot
# (actual values given above),
# Mtildelist: The link frame locations based on the model of the
# actual robot (actual values given above),
# Gtildelist: The link spatial inertias based on the model of the
# actual robot (actual values given above),
# Kp: The feedback proportional gain (identical for each joint),
# Ki: The feedback integral gain (identical for each joint),
# Kd: The feedback derivative gain (identical for each joint),
# dt: The timestep between points on the reference trajectory,
# 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.
#Returns taumat: An Nxn matrix of the controllers commanded joint
# forces/torques, where each row of n forces/torques
# corresponds to a single time instant,
# thetamat: An Nxn matrix of actual 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 = 1.0 * Tf / dt
method = 5
traj = 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 \
= 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
dthetamatd = np.array(dthetamatd).T
ddthetamatd = np.array(ddthetamatd).T
m,n = np.array(thetamatd).shape
thetacurrent = np.array(thetalist).copy()
dthetacurrent = np.array(dthetalist).copy()
eint = np.zeros((m,1)).reshape(m,)
taumat = np.zeros(np.array(thetamatd).shape)
thetamat = np.zeros(np.array(thetamatd).shape)
for i in range(n):
taulist \
= ComputedTorque(thetacurrent, dthetacurrent, eint, gtilde, \
Mtildelist, Gtildelist, Slist, thetamatd[:, i], \
dthetamatd[:, i], ddthetamatd[:, i], Kp, Ki, Kd)
for j in range(intRes):
ddthetalist \
= ForwardDynamics(thetacurrent, dthetacurrent, taulist, g, \
Ftipmat[:, i], Mlist, Glist, Slist)
thetacurrent, dthetacurrent \
= EulerStep(thetacurrent, dthetacurrent, ddthetalist, \
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
links = np.array(thetamat).shape[0]
N = np.array(thetamat).shape[1]
Tf = N * dt
timestamp = np.linspace(0, Tf, N)
#timestampd = np.linspace(0,Tf,(N/intRes))
for i in range(links):
col = [np.random.uniform(0, 1), np.random.uniform(0, 1),
np.random.uniform(0, 1)]
plt.plot(timestamp, thetamat[i, :], "-", color=col, \
label = ("ActualTheta" + str(i + 1)))
plt.plot(timestamp, thetamatd[i, :], ".", color=col, \
label = ("DesiredTheta" + str(i + 1)))
plt.legend(loc = 'upper left')
plt.xlabel("Time")
plt.ylabel("Joint Angles")
plt.title("Plot of Actual and Desired Joint Angles")
plt.show()
taumat = np.array(taumat).T
thetamat = np.array(thetamat).T
return (taumat, thetamat)
Reference in New Issue View Git Blame Copy Permalink