Robotics
/
Modern_Robotics
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Continued reformatting python source doc comments

This commit is contained in:
Bill Hunt 2018-07-09 08:57:31 -05:00
parent e1be446675
commit 5779d2d14f
1 changed files with 145 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

125
code/Python/modern_robotics.py
View File

@ -382,10 +382,12 @@ def AxisAng6(expc6):
return (np.array(expc6 / theta), theta) return (np.array(expc6 / theta), theta)
def MatrixExp6(se3mat): def MatrixExp6(se3mat):
#Takes a se(3) representation of exponential coordinates. """Computes the matrix exponential of an se3 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. 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: Example Input:
se3mat = np.array([[0, 0, 0, 0], se3mat = np.array([[0, 0, 0, 0],
[0, 0, -1.57079632, 2.35619449], [0, 0, -1.57079632, 2.35619449],
@ -396,7 +398,11 @@ Output:
[0.0, 0.0, -1.0, 0.0], [0.0, 0.0, -1.0, 0.0],
[0.0, 1.0, 0.0, 3.0], [0.0, 1.0, 0.0, 3.0],
[ 0, 0, 0, 1]] [ 0, 0, 0, 1]]
'''
:param se3mat: A matrix in se3
:returns: The matrix exponential of se3mat
"""
omgtheta = so3ToVec(np.array(se3mat)[0: 3: 1, 0: 3: 1]) omgtheta = so3ToVec(np.array(se3mat)[0: 3: 1, 0: 3: 1])
if NearZero(np.linalg.norm(omgtheta)): if NearZero(np.linalg.norm(omgtheta)):
return np.r_[np.c_[np.eye(3), return np.r_[np.c_[np.eye(3),
@ -416,9 +422,11 @@ Output:
[[0, 0, 0, 1]]] [[0, 0, 0, 1]]]
def MatrixLog6(T): def MatrixLog6(T):
#Takes a transformation matrix T in SE(3). """Computes the matrix logarithm of a homogeneous transformation matrix
#Returns the corresponding se(3) representation of exponential coordinates.
''' Takes a transformation matrix T in SE(3).
Returns the corresponding se(3) representation of exponential coordinates.
Example Input: Example Input:
T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]]) T = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]])
Output: Output:
@ -426,7 +434,10 @@ Output:
[ 0. 0. -1.57079633 2.35619449] [ 0. 0. -1.57079633 2.35619449]
[ 0. 1.57079633 0. 2.35619449] [ 0. 1.57079633 0. 2.35619449]
[ 0. 0. 0. 0. ]] [ 0. 0. 0. 0. ]]
'''
:param R: A matrix in SE3
:returns: The matrix logarithm of R
"""
R, p = TransToRp(T) R, p = TransToRp(T)
if NearZero(np.linalg.norm(R - np.eye(3))): if NearZero(np.linalg.norm(R - np.eye(3))):
return np.r_[np.c_[np.zeros((3, 3)), return np.r_[np.c_[np.zeros((3, 3)),
@ -607,15 +618,14 @@ def TestIfSE3 (T):
''' '''
def FKinBody(M, Blist, thetalist): def FKinBody(M, Blist, thetalist):
#Takes M: The home configuration (position and orientation) of the """Computes forward kinematics in the body frame for an open chain robot
# end-effector,
# Blist: The joint screw axes in the end-effector frame when the Takes the home configuration (position and orientation) of the end-effector,
# manipulator is at the home position, in the format of a the joint screw axes in the end-effector frame when the manipulator is at the home position,
# matrix with axes as the columns, and a list of joint values.
# thetalist: A list of joint coordinates. Returns T in SE(3) representing the end-effector frame when the joints are
#Returns T IN SE(3) representing the end-effector frame when the joints are at the specified coordinates (i.t.o Body Frame).
#at the specified coordinates (i.t.o Body Frame).
'''
Example Input: Example Input:
M = np.array([[-1, 0, 0, 0], [0, 1, 0, 6], [0, 0, -1, 2], [0, 0, 0, 1]]) 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], Blist = np.array([[0, 0, -1, 2, 0, 0],
@ -627,7 +637,15 @@ Output:
[ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00], [ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00],
[ 0. 0. -1. 1.68584073], [ 0. 0. -1. 1.68584073],
[ 0. 0. 0. 1.]] [ 0. 0. 0. 1.]]
'''
: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 matrix with axes as the columns
:param thetalist: A list of joint coordinates
"returns: A homogeneous transformation matrix representing the end-effector frame
when the joints are at the specified coordinates (i.t.o Body Frame)
"""
T = np.array(M) T = np.array(M)
for i in range(len(thetalist)): for i in range(len(thetalist)):
T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i] \ T = np.dot(T,MatrixExp6(VecTose3(np.array(Blist)[:, i] \
@ -635,15 +653,14 @@ Output:
return T return T
def FKinSpace(M, Slist, thetalist): def FKinSpace(M, Slist, thetalist):
#Takes M: the home configuration (position and orientation) of the """Computes forward kinematics in the space frame for an open chain robot
# end-effector,
# Slist: The joint screw axes in the space frame when the manipulator Takes the home configuration (position and orientation) of the end-effector,
# is at the home position, in the format of a matrix with axes the joint screw axes in the space frame when the manipulator is at the home position,
# as the columns, and a list of joint values.
# thetalist: A list of joint coordinates. Returns T in SE(3) representing the end-effector frame when the joints are
#Returns T in SE(3) representing the end-effector frame when the joints are at the specified coordinates (i.t.o Space Frame).
#at the specified coordinates (i.t.o Space Frame).
'''
Example Input: Example Input:
M = np.array([[-1, 0, 0, 0], [0, 1, 0, 6], [0, 0, -1, 2], [0, 0, 0, 1]]) 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], Slist = np.array([[0, 0, 1, 4, 0, 0],
@ -655,7 +672,15 @@ Output:
[ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00], [ 1.00000000e+00 1.14423775e-17 0.00000000e+00 4.00000000e+00],
[ 0. 0. -1. 1.68584073], [ 0. 0. -1. 1.68584073],
[ 0. 0. 0. 1.]] [ 0. 0. 0. 1.]]
'''
: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 matrix with axes as the columns
:param thetalist: A list of joint coordinates
:returns: A homogeneous transformation matrix representing the end-effector frame
when the joints are at the specified coordinates (i.t.o Space Frame).
"""
T = np.array(M) T = np.array(M)
for i in range(len(thetalist)-1, -1, -1): for i in range(len(thetalist)-1, -1, -1):
T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \ T = np.dot(MatrixExp6(VecTose3(np.array(Slist)[:, i] \
@ -667,12 +692,12 @@ Output:
''' '''
def JacobianBody(Blist, thetalist): def JacobianBody(Blist, thetalist):
#Takes Blist: The joint screw axes in the end-effector frame when the """Computes the body Jacobian for an open chain robot
# manipulator is at the home position, in the format of a
# matrix with axes as the columns, Takes the joint screw axes in the end-effector frame when the
# thetalist: A list of joint coordinates. manipulator is at the home position, and a list of joint value.
#Returns the corresponding body Jacobian (6xn real numbers). Returns the corresponding body Jacobian (6xn real numbers).
'''
Example Input: Example Input:
Blist = np.array([[0, 0, 1, 0, 0.2, 0.2], Blist = np.array([[0, 0, 1, 0, 0.2, 0.2],
[1, 0, 0, 2, 0, 3], [1, 0, 0, 2, 0, 3],
@ -686,7 +711,13 @@ Output:
[ 2.32586047 1.66809 0.56410831 0.2] [ 2.32586047 1.66809 0.56410831 0.2]
[-1.44321167 2.94561275 1.43306521 0.3] [-1.44321167 2.94561275 1.43306521 0.3]
[-2.06639565 1.82881722 -1.58868628 0.4]] [-2.06639565 1.82881722 -1.58868628 0.4]]
'''
: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
:returns: The body Jacobian corresponding to the inputs (6xn real numbers)
"""
Jb = np.array(Blist).copy() Jb = np.array(Blist).copy()
T = np.eye(4) T = np.eye(4)
for i in range(len(thetalist) - 2, -1, -1): for i in range(len(thetalist) - 2, -1, -1):
@ -696,18 +727,18 @@ Output:
return Jb return Jb
def JacobianSpace(Slist, thetalist): def JacobianSpace(Slist, thetalist):
#Takes Slist: The joint screw axes in the space frame when the manipulator """Computes the space Jacobian for an open chain robot
# is at the home position, in the format of a matrix with axes
# as the columns, Takes the joint screw axes in the space frame when the manipulator
# thetalist: A list of joint coordinates. is at the home position, and a list of joint values.
#Returns the corresponding space Jacobian (6xn real numbers). Returns the corresponding space Jacobian (6xn real numbers).
'''
Example Input: Example Input:
Slist = np.array([[0, 0, 1, 0, 0.2, 0.2], Slist = np.array([[0, 0, 1, 0, 0.2, 0.2],
[1, 0, 0, 2, 0, 3], [1, 0, 0, 2, 0, 3],
[0, 1, 0, 0, 2, 1], [0, 1, 0, 0, 2, 1],
[1, 0, 0, 0.2, 0.3, 0.4]]).T [1, 0, 0, 0.2, 0.3, 0.4]]).T
thetalist = np.array([0.2, 1.1, 0.1, 1.2])
Output: Output:
[[ 0. 0.98006658 -0.09011564 0.95749426] [[ 0. 0.98006658 -0.09011564 0.95749426]
[ 0. 0.19866933 0.4445544 0.28487557] [ 0. 0.19866933 0.4445544 0.28487557]
@ -715,7 +746,13 @@ Output:
[ 0. 1.95218638 -2.21635216 -0.51161537] [ 0. 1.95218638 -2.21635216 -0.51161537]
[ 0.2 0.43654132 -2.43712573 2.77535713] [ 0.2 0.43654132 -2.43712573 2.77535713]
[ 0.2 2.96026613 3.23573065 2.22512443]] [ 0.2 2.96026613 3.23573065 2.22512443]]
'''
: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
:returns: The space Jacobian corresponding to the inputs (6xn real numbers)
"""
Js = np.array(Slist).copy() Js = np.array(Slist).copy()
T = np.eye(4) T = np.eye(4)
for i in range(1, len(thetalist)): for i in range(1, len(thetalist)):