Update: Make matplot as an option
This commit is contained in:
parent
593fb0478e
commit
30aade3ea7
|
|
@ -1,4 +1,4 @@
|
||||||
import numpy as np
|
from __version__ import __version__
|
||||||
|
|
||||||
from core import *
|
from core import *
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -18,6 +18,7 @@ Optional library: matplotlib
|
||||||
*** IMPORTS ***
|
*** IMPORTS ***
|
||||||
'''
|
'''
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
import matplotlib.pyplot as plt
|
import matplotlib.pyplot as plt
|
||||||
|
|
||||||
'''
|
'''
|
||||||
|
|
@ -1301,16 +1302,20 @@ def InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, \
|
||||||
Tau2 = taumat[:, 1]
|
Tau2 = taumat[:, 1]
|
||||||
Tau3 = taumat[:, 2]
|
Tau3 = taumat[:, 2]
|
||||||
timestamp = np.linspace(0, Tf, N)
|
timestamp = np.linspace(0, Tf, N)
|
||||||
import matplotlib.pyplot as plt
|
try:
|
||||||
plt.plot(timestamp, Tau1, label = "Tau1")
|
import matplotlib.pyplot as plt
|
||||||
plt.plot(timestamp, Tau2, label = "Tau2")
|
except:
|
||||||
plt.plot(timestamp, Tau3, label = "Tau3")
|
print 'The result will not be plotted due to a lack of package matplotlib'
|
||||||
plt.ylim (-40, 120)
|
else:
|
||||||
plt.legend(loc = 'lower right')
|
plt.plot(timestamp, Tau1, label = "Tau1")
|
||||||
plt.xlabel("Time")
|
plt.plot(timestamp, Tau2, label = "Tau2")
|
||||||
plt.ylabel("Torque")
|
plt.plot(timestamp, Tau3, label = "Tau3")
|
||||||
plt.title("Plot of Torque Trajectories")
|
plt.ylim (-40, 120)
|
||||||
plt.show()
|
plt.legend(loc = 'lower right')
|
||||||
|
plt.xlabel("Time")
|
||||||
|
plt.ylabel("Torque")
|
||||||
|
plt.title("Plot of Torque Trajectories")
|
||||||
|
plt.show()
|
||||||
"""
|
"""
|
||||||
thetamat = np.array(thetamat).T
|
thetamat = np.array(thetamat).T
|
||||||
dthetamat = np.array(dthetamat).T
|
dthetamat = np.array(dthetamat).T
|
||||||
|
|
@ -1355,7 +1360,6 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
||||||
|
|
||||||
Example Inputs (3 Link Robot):
|
Example Inputs (3 Link Robot):
|
||||||
import modern_robotics as mr
|
import modern_robotics as mr
|
||||||
import matplotlib.pyplot as plt
|
|
||||||
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])
|
||||||
taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5],
|
taumat = np.array([[3.63, -6.58, -5.57], [3.74, -5.55, -5.5],
|
||||||
|
|
@ -1406,18 +1410,23 @@ def ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, \
|
||||||
N = np.array(taumat).shape[0]
|
N = np.array(taumat).shape[0]
|
||||||
Tf = np.array(taumat).shape[0] * dt
|
Tf = np.array(taumat).shape[0] * dt
|
||||||
timestamp = np.linspace(0, Tf, N)
|
timestamp = np.linspace(0, Tf, N)
|
||||||
plt.plot(timestamp, theta1, label = "Theta1")
|
try:
|
||||||
plt.plot(timestamp, theta2, label = "Theta2")
|
import matplotlib.pyplot as plt
|
||||||
plt.plot(timestamp, theta3, label = "Theta3")
|
except:
|
||||||
plt.plot(timestamp, dtheta1, label = "DTheta1")
|
print 'The result will not be plotted due to a lack of package matplotlib'
|
||||||
plt.plot(timestamp, dtheta2, label = "DTheta2")
|
else:
|
||||||
plt.plot(timestamp, dtheta3, label = "DTheta3")
|
plt.plot(timestamp, theta1, label = "Theta1")
|
||||||
plt.ylim (-12, 10)
|
plt.plot(timestamp, theta2, label = "Theta2")
|
||||||
plt.legend(loc = 'lower right')
|
plt.plot(timestamp, theta3, label = "Theta3")
|
||||||
plt.xlabel("Time")
|
plt.plot(timestamp, dtheta1, label = "DTheta1")
|
||||||
plt.ylabel("Joint Angles/Velocities")
|
plt.plot(timestamp, dtheta2, label = "DTheta2")
|
||||||
plt.title("Plot of Joint Angles and Joint Velocities")
|
plt.plot(timestamp, dtheta3, label = "DTheta3")
|
||||||
plt.show()
|
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
|
taumat = np.array(taumat).T
|
||||||
Ftipmat = np.array(Ftipmat).T
|
Ftipmat = np.array(Ftipmat).T
|
||||||
|
|
@ -1861,23 +1870,27 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
|
||||||
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
|
||||||
links = np.array(thetamat).shape[0]
|
try:
|
||||||
N = np.array(thetamat).shape[1]
|
import matplotlib.pyplot as plt
|
||||||
Tf = N * dt
|
except:
|
||||||
timestamp = np.linspace(0, Tf, N)
|
print 'The result will not be plotted due to a lack of package matplotlib'
|
||||||
#timestampd = np.linspace(0,Tf,(N/intRes))
|
else:
|
||||||
for i in range(links):
|
links = np.array(thetamat).shape[0]
|
||||||
col = [np.random.uniform(0, 1), np.random.uniform(0, 1),
|
N = np.array(thetamat).shape[1]
|
||||||
np.random.uniform(0, 1)]
|
Tf = N * dt
|
||||||
plt.plot(timestamp, thetamat[i, :], "-", color=col, \
|
timestamp = np.linspace(0, Tf, N)
|
||||||
label = ("ActualTheta" + str(i + 1)))
|
for i in range(links):
|
||||||
plt.plot(timestamp, thetamatd[i, :], ".", color=col, \
|
col = [np.random.uniform(0, 1), np.random.uniform(0, 1),
|
||||||
label = ("DesiredTheta" + str(i + 1)))
|
np.random.uniform(0, 1)]
|
||||||
plt.legend(loc = 'upper left')
|
plt.plot(timestamp, thetamat[i, :], "-", color=col, \
|
||||||
plt.xlabel("Time")
|
label = ("ActualTheta" + str(i + 1)))
|
||||||
plt.ylabel("Joint Angles")
|
plt.plot(timestamp, thetamatd[i, :], ".", color=col, \
|
||||||
plt.title("Plot of Actual and Desired Joint Angles")
|
label = ("DesiredTheta" + str(i + 1)))
|
||||||
plt.show()
|
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
|
taumat = np.array(taumat).T
|
||||||
thetamat = np.array(thetamat).T
|
thetamat = np.array(thetamat).T
|
||||||
return (taumat, thetamat)
|
return (taumat, thetamat)
|
||||||
|
|
|
||||||
|
|
@ -2,7 +2,7 @@ from setuptools import setup
|
||||||
|
|
||||||
setup(
|
setup(
|
||||||
name = "modern_robotics",
|
name = "modern_robotics",
|
||||||
version = "1.0.0",
|
version = "__version__",
|
||||||
author = "Huan Weng, Mikhail Todes, Jarvis Schultz, Bill Hunt",
|
author = "Huan Weng, Mikhail Todes, Jarvis Schultz, Bill Hunt",
|
||||||
author_email = "huanweng@u.northwestern.edu",
|
author_email = "huanweng@u.northwestern.edu",
|
||||||
description = ("Modern Robotics: Mechanics, Planning, and Control: Code Library"),
|
description = ("Modern Robotics: Mechanics, Planning, and Control: Code Library"),
|
||||||
|
|
@ -16,8 +16,7 @@ setup(
|
||||||
"License :: OSI Approved :: BSD License",
|
"License :: OSI Approved :: BSD License",
|
||||||
],
|
],
|
||||||
install_requires=[
|
install_requires=[
|
||||||
'numpy',
|
'numpy'
|
||||||
'scipy',
|
|
||||||
],
|
],
|
||||||
platforms='Linux, Mac, Windows',
|
platforms='Linux, Mac, Windows',
|
||||||
)
|
)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue