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
parent
77dcf6617c
commit
36c388aa20
|
|
@ -182,7 +182,7 @@ 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
|
||||
|
|
@ -305,7 +305,7 @@ 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
|
||||
|
|
@ -323,10 +323,10 @@ 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
|
||||
: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
|
||||
|
|
@ -1222,10 +1222,10 @@ def EulerStep(thetalist, dthetalist, ddthetalist, dt):
|
|||
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)
|
||||
|
|
@ -1251,8 +1251,10 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
|
|||
forces/torques at each time step
|
||||
|
||||
Example Inputs (3 Link Robot):
|
||||
from __future__ import print_function
|
||||
import numpy as np
|
||||
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])
|
||||
thetaend = np.array([np.pi / 2, np.pi / 2, np.pi / 2])
|
||||
Tf = 3
|
||||
|
|
@ -1267,7 +1269,7 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
|
|||
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)
|
||||
# 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],
|
||||
|
|
@ -1297,7 +1299,7 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
|
|||
taumat \
|
||||
= mr.InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
|
||||
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]
|
||||
Tau2 = taumat[:, 1]
|
||||
Tau3 = taumat[:, 2]
|
||||
|
|
@ -1359,6 +1361,8 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
|||
ForwardDynamics.
|
||||
|
||||
Example Inputs (3 Link Robot):
|
||||
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])
|
||||
|
|
@ -1367,7 +1371,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
|||
[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)
|
||||
# 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],
|
||||
|
|
@ -1400,7 +1404,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
|||
= mr.ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, \
|
||||
Ftipmat, Mlist, Glist, Slist, dt, \
|
||||
intRes)
|
||||
#Output using matplotlib to plot the joint angle/velocities
|
||||
# Output using matplotlib to plot the joint angle/velocities
|
||||
theta1 = thetamat[:, 0]
|
||||
theta2 = thetamat[:, 1]
|
||||
theta3 = thetamat[:, 2]
|
||||
|
|
@ -1413,7 +1417,7 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
|||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
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:
|
||||
plt.plot(timestamp, theta1, label = "Theta1")
|
||||
plt.plot(timestamp, theta2, label = "Theta2")
|
||||
|
|
@ -1765,10 +1769,12 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
|
|||
using matplotlib and random libraries.
|
||||
|
||||
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])
|
||||
#Initialise robot description (Example with 3 links)
|
||||
# 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],
|
||||
|
|
@ -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.425]]).T
|
||||
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])
|
||||
Tf = 1
|
||||
N = int(1.0 * Tf / dt)
|
||||
|
|
@ -1810,7 +1816,7 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
|
|||
= (thetamatd[i + 1, :] - thetamatd[i, :]) / dt
|
||||
ddthetamatd[i + 1, :] \
|
||||
= (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])
|
||||
Mhat01 = np.array([[1, 0, 0, 0],
|
||||
[0, 1, 0, 0],
|
||||
|
|
@ -1869,7 +1875,7 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
|
|||
taumat[:, i] = taulist
|
||||
thetamat[:, i] = thetacurrent
|
||||
eint = np.add(eint, dt * np.subtract(thetamatd[:, i], thetacurrent))
|
||||
#Output using matplotlib to plot
|
||||
# Output using matplotlib to plot
|
||||
try:
|
||||
import matplotlib.pyplot as plt
|
||||
except:
|
||||
|
|
|
|||
Loading…
Reference in New Issue