Update: Make matplot as an option

This commit is contained in:
HuanWeng 2018-07-24 14:41:57 -05:00
parent 593fb0478e
commit 30aade3ea7
3 changed files with 58 additions and 46 deletions

View File

@ -1,4 +1,4 @@
import numpy as np from __version__ import __version__
from core import * from core import *

View File

@ -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
@ -1859,25 +1868,29 @@ def SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist, \
1.0 * dt / intRes) 1.0 * dt / intRes)
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
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)

View File

@ -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',
) )