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

Major formatting update to Python library 

+ Fixed some spelling mistakes
 + Cleaned up tab/space indentation. Previously some of the samples would not
   work if copied/pasted into a Python interpreter because of mixed tabs/spaces
 + Cleared all trailing whitespace
 + Missing quotation marks in some sample code caused mistakes with printing
   messages
 + Added all import statements to some of the examples that already had some
   import statements
This commit is contained in:
Jarvis Schultz 2018-11-13 17:09:35 -06:00
parent 77dcf6617c
commit 36c388aa20
1 changed files with 832 additions and 826 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
packages/Python/modern_robotics/core.py
View File

@ -182,7 +182,7 @@ def MatrixLog3(R):
def RpToTrans(R, p): def RpToTrans(R, p):
"""Converts a rotation matrix and a position vector into homogeneous """Converts a rotation matrix and a position vector into homogeneous
tranformation matrix transformation matrix
:param R: A 3x3 rotation matrix :param R: A 3x3 rotation matrix
:param p: A 3-vector :param p: A 3-vector
@ -305,7 +305,7 @@ def Adjoint(T):
np.c_[np.dot(VecToso3(p), R), R]] np.c_[np.dot(VecToso3(p), R), R]]
def ScrewToAxis(q, s, h): 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 normalized screw axis
:param q: A point lying on the screw axis :param q: A point lying on the screw axis
@ -323,10 +323,10 @@ def ScrewToAxis(q, s, h):
return np.r_[s, np.cross(q, s) + np.dot(h, s)] return np.r_[s, np.cross(q, s) + np.dot(h, s)]
def AxisAng6(expc6): 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 form
:param expc6: A 6-vector of exponential corrdinates for rigid-body motion :param expc6: A 6-vector of exponential coordinates for rigid-body motion
S*theta S*theta
:return S: The corresponding normalized screw axis :return S: The corresponding normalized screw axis
:return theta: The distance traveled along/about S :return theta: The distance traveled along/about S
@ -1222,10 +1222,10 @@ def EulerStep(thetalist, dthetalist, ddthetalist, dt):
ddthetalist = np.array([2, 1.5, 1]) ddthetalist = np.array([2, 1.5, 1])
dt = 0.1 dt = 0.1
Output: Output:
thetalistNext: thetalistNext:
array([ 0.11, 0.12, 0.13]) array([ 0.11, 0.12, 0.13])
dthetalistNext: dthetalistNext:
array([ 0.3 , 0.35, 0.4 ]) array([ 0.3 , 0.35, 0.4 ])
""" """
return thetalist + dt * np.array(dthetalist), \ return thetalist + dt * np.array(dthetalist), \
dthetalist + dt * np.array(ddthetalist) dthetalist + dt * np.array(ddthetalist)
@ -1251,8 +1251,10 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
forces/torques at each time step forces/torques at each time step
Example Inputs (3 Link Robot): Example Inputs (3 Link Robot):
from __future__ import print_function
import numpy as np
import modern_robotics as mr import modern_robotics as mr
#Create a trajectory to follow using functions from Chapter 9 # Create a trajectory to follow using functions from Chapter 9
thetastart = np.array([0, 0, 0]) thetastart = np.array([0, 0, 0])
thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2]) thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2])
Tf = 3 Tf = 3
@ -1267,7 +1269,7 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt dthetamat[i + 1, :] = (thetamat[i + 1, :] - thetamat[i, :]) / dt
ddthetamat[i + 1, :] \ ddthetamat[i + 1, :] \
= (dthetamat[i + 1, :] - dthetamat[i, :]) / dt = (dthetamat[i + 1, :] - dthetamat[i, :]) / dt
#Initialise robot descripstion (Example with 3 links) # Initialize robot description (Example with 3 links)
g = np.array([0, 0, -9.8]) g = np.array([0, 0, -9.8])
Ftipmat = np.ones((N, 6)) Ftipmat = np.ones((N, 6))
M01 = np.array([[1, 0, 0, 0], M01 = np.array([[1, 0, 0, 0],
@ -1297,7 +1299,7 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
taumat \ taumat \
= mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \ = mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
Ftipmat, Mlist, Glist, Slist) Ftipmat, Mlist, Glist, Slist)
#Output using matplotlib to plot the joint forces/torques # Output using matplotlib to plot the joint forces/torques
Tau1 = taumat[:, 0] Tau1 = taumat[:, 0]
Tau2 = taumat[:, 1] Tau2 = taumat[:, 1]
Tau3 = taumat[:, 2] Tau3 = taumat[:, 2]
@ -1359,6 +1361,8 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
ForwardDynamics. ForwardDynamics.
Example Inputs (3 Link Robot): Example Inputs (3 Link Robot):
from __future__ import print_function
import numpy as np
import modern_robotics as mr import modern_robotics as mr
thetalist = np.array([0.1, 0.1, 0.1]) thetalist = np.array([0.1, 0.1, 0.1])
dthetalist = np.array([0.1, 0.2, 0.3]) dthetalist = np.array([0.1, 0.2, 0.3])
@ -1367,7 +1371,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
[5.85, 8.17, -2.59], [5.78, 2.79, -1.7], [5.85, 8.17, -2.59], [5.78, 2.79, -1.7],
[4.99, -5.3, -1.19], [4.08, -9.41, 0.07], [4.99, -5.3, -1.19], [4.08, -9.41, 0.07],
[3.56, -10.1, 0.97], [3.49, -9.41, 1.23]]) [3.56, -10.1, 0.97], [3.49, -9.41, 1.23]])
#Initialise robot description (Example with 3 links) # Initialize robot description (Example with 3 links)
g = np.array([0, 0, -9.8]) g = np.array([0, 0, -9.8])
Ftipmat = np.ones((np.array(taumat).shape[0], 6)) Ftipmat = np.ones((np.array(taumat).shape[0], 6))
M01 = np.array([[1, 0, 0, 0], M01 = np.array([[1, 0, 0, 0],
@ -1400,7 +1404,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
= mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \ = mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \
Ftipmat, Mlist, Glist, Slist, dt, \ Ftipmat, Mlist, Glist, Slist, dt, \
intRes) intRes)
#Output using matplotlib to plot the joint angle/velocities # Output using matplotlib to plot the joint angle/velocities
theta1 = thetamat[:, 0] theta1 = thetamat[:, 0]
theta2 = thetamat[:, 1] theta2 = thetamat[:, 1]
theta3 = thetamat[:, 2] theta3 = thetamat[:, 2]
@ -1413,7 +1417,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
try: try:
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
except: except:
print(The result will not be plotted due to a lack of package matplotlib) print('The result will not be plotted due to a lack of package matplotlib')
else: else:
plt.plot(timestamp, theta1, label = "Theta1") plt.plot(timestamp, theta1, label = "Theta1")
plt.plot(timestamp, theta2, label = "Theta2") plt.plot(timestamp, theta2, label = "Theta2")
@ -1765,10 +1769,12 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
using matplotlib and random libraries. using matplotlib and random libraries.
Example Input: Example Input:
from __future__ import print_function
import numpy as np
from modern_robotics import JointTrajectory from modern_robotics import JointTrajectory
thetalist = np.array([0.1, 0.1, 0.1]) thetalist = np.array([0.1, 0.1, 0.1])
dthetalist = np.array([0.1, 0.2, 0.3]) dthetalist = np.array([0.1, 0.2, 0.3])
#Initialise robot description (Example with 3 links) # Initialize robot description (Example with 3 links)
g = np.array([0, 0, -9.8]) g = np.array([0, 0, -9.8])
M01 = np.array([[1, 0, 0, 0], M01 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0], [0, 1, 0, 0],
@ -1795,7 +1801,7 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
[0, 1, 0, -0.089, 0, 0], [0, 1, 0, -0.089, 0, 0],
[0, 1, 0, -0.089, 0, 0.425]]).T [0, 1, 0, -0.089, 0, 0.425]]).T
dt = 0.01 dt = 0.01
#Create a trajectory to follow # Create a trajectory to follow
thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi]) thetaend = np.array([np.pi / 2, np.pi, 1.5 * np.pi])
Tf = 1 Tf = 1
N = int(1.0 * Tf / dt) N = int(1.0 * Tf / dt)
@ -1810,7 +1816,7 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
= (thetamatd[i + 1, :] - thetamatd[i, :]) / dt = (thetamatd[i + 1, :] - thetamatd[i, :]) / dt
ddthetamatd[i + 1, :] \ ddthetamatd[i + 1, :] \
= (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt = (dthetamatd[i + 1, :] - dthetamatd[i, :]) / dt
#Possibly wrong robot description (Example with 3 links) # Possibly wrong robot description (Example with 3 links)
gtilde = np.array([0.8, 0.2, -8.8]) gtilde = np.array([0.8, 0.2, -8.8])
Mhat01 = np.array([[1, 0, 0, 0], Mhat01 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0], [0, 1, 0, 0],
@ -1869,7 +1875,7 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
taumat[:, i] = taulist taumat[:, i] = taulist
thetamat[:, i] = thetacurrent thetamat[:, i] = thetacurrent
eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent)) eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent))
#Output using matplotlib to plot # Output using matplotlib to plot
try: try:
import matplotlib.pyplot as plt import matplotlib.pyplot as plt
except: except: