[ 0.7, 0],
[ 0.1, 0.1]])
xx = S[:,0].copy()
yy = S[:,1].copy()
S[:,0] = xx*cos(theta)-yy*sin(theta)+x
S[:,1] = yy*cos(theta)+xx*sin(theta)+y
aircraft = mpl.patches.Polygon(S, closed=True, facecolor='0.75')
image.patches.append(aircraft)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex3_Aircraft.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(4,'theta')], plotinputs=[(0,'F1'),(1,'F2')])
A.set_limits(xlim=(-1,11), ylim=(-1,7))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex3_Aircraft.gif')
rod1 = mpl.lines.Line2D([0,x1],[0,y1],color='k',zorder=0,linewidth=2.0)
rod2 = mpl.lines.Line2D([x1,x2],[y1,y2],color='0.3',zorder=0,linewidth=2.0)
# pendulums
sphere1 = mpl.patches.Circle((x1,y1),0.01,color='k')
sphere2 = mpl.patches.Circle((0,0),0.01,color='k')
image.lines.append(rod1)
image.lines.append(rod2)
image.patches.append(sphere1)
image.patches.append(sphere2)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex5_Acrobot.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'phi1'),(2,'phi2')], plotinputs=[(0,'u')])
A.set_limits(xlim=(-1.1,1.1), ylim=(-1.1,1.1))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex5_Acrobot.gif')
rod = mpl.lines.Line2D([x_car,x_pendulum], [y_car,y_pendulum],
color='black', zorder=1, linewidth=2.0)
# finally we add the patches and line to the image
image.patches.append(pendulum)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod)
# and return the image
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex0_InvertedPendulumSwingUp.pcl')
# now we can create an instance of the `Animation` class
# with our draw function and the simulation results
#
# to plot the curves of some trajectories along with the picture
# we also pass the appropriate lists as arguments (see documentation)
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'x'), (2,'phi')], plotinputs=[(0,'u')])
# as for now we have to explicitly set the limits of the figure
# (may involves some trial and error)
xmin = np.min(S.sim_data[1][:,0]); xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 0.5, xmax + 0.5), ylim=(-0.6,0.6))
fill=True,facecolor='0.75',linewidth=2.0)
# joint
joint = mpl.patches.Circle((x_car,0),0.005,color='k')
image.lines.append(rod)
image.patches.append(sphere)
image.patches.append(cart)
image.patches.append(joint)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex1_InvertedPendulumTranslation.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'x'), (2,'phi')], plotinputs=[(0,'u')])
xmin = np.min(S.sim_data[1][:,0])
xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 0.5,xmax + 0.5), ylim=(-0.3,0.8))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex1_InvertedPendulumTranslation.gif')
# pendulums
sphere1 = mpl.patches.Circle((x1, y1), 0.01, color='k')
sphere2 = mpl.patches.Circle((0, 0), 0.01, color='k')
image.lines.append(rod1)
image.lines.append(rod2)
image.patches.append(sphere1)
image.patches.append(sphere2)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex5_Acrobot.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'phi1'), (2, 'phi2')],
plotinputs=[(0, 'u')])
A.set_limits(xlim=(-1.1, 1.1), ylim=(-1.1, 1.1))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex5_Acrobot.gif')
fill=True, facecolor='grey', linewidth=2.0)
joint = mpl.patches.Circle((x_car,0), 0.005, color='black')
rod = mpl.lines.Line2D([x_car,x_pendulum], [y_car,y_pendulum],
color='black', zorder=1, linewidth=2.0)
image.patches.append(pendulum)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex7_ConstrainedInvertedPendulum.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'x'), (1,'dx')],
plotinputs=[(0,'u1')])
xmin = np.min(S.sim_data[1][:,0])
xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 0.5, xmax + 0.5), ylim=(-0.6,0.6))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex7_ConstrainedInvertedPendulum.gif')
car = mpl.patches.Rectangle((x_car - 0.5 * car_width, y_car - car_heigth),
car_width,
car_heigth,
fill=True,
facecolor='grey',
linewidth=2.0)
image.patches.append(car)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex6_ConstrainedDoubleIntegrator.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'x'), (1, 'dx')],
plotinputs=[(0, 'u')])
xmin = np.min(S.sim_data[1][:, 0])
xmax = np.max(S.sim_data[1][:, 0])
A.set_limits(xlim=(xmin - 0.1, xmax + 0.1), ylim=(-0.1, 0.1))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
# pendulums
sphere1 = mpl.patches.Circle((x1, y1), 0.01, color='k')
sphere2 = mpl.patches.Circle((0, 0), 0.01, color='k')
image.lines.append(rod1)
image.lines.append(rod2)
image.patches.append(sphere1)
image.patches.append(sphere2)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex4_UnderactuatedManipulator.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'phi1'), (2, 'phi2')],
plotinputs=[(0, 'u')])
A.set_limits(xlim=(-0.1, 0.6), ylim=(-0.4, 0.65))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex4_UnderactuatedManipulator.gif')
# add the patches and lines to the image
image.patches.append(pendulum1)
image.patches.append(pendulum2)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod1)
image.lines.append(rod2)
# and return the image
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex8_ConstrainedDoublePendulum.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
# create Animation object
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'$x$'),(1,'$\\dot{x}$')], plotinputs=[(0,'$u$')])
xmin = np.min(S.sim_data[1][:,0])
xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 1.0, xmax + 1.0), ylim=(-1.2,1.2))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex8_ConstrainedDoublePendulum.gif')
# add the patches and lines to the image
image.patches.append(pendulum1)
image.patches.append(pendulum2)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod1)
image.lines.append(rod2)
# and return the image
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex8_ConstrainedDoublePendulum.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
# create Animation object
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, '$x$'), (1, '$\\dot{x}$')],
plotinputs=[(0, '$u$')])
xmin = np.min(S.sim_data[1][:, 0])
xmax = np.max(S.sim_data[1][:, 0])
A.set_limits(xlim=(xmin - 1.0, xmax + 1.0), ylim=(-1.2, 1.2))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
linewidth=2.0)
image.patches.append(sphere1)
image.patches.append(sphere2)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod1)
image.lines.append(rod2)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex2_InvertedDualPendulumSwingUp.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'x'), (2, 'phi1'), (4, 'phi2')],
plotinputs=[(0, 'u')])
xmin = np.min(S.sim_data[1][:, 0])
xmax = np.max(S.sim_data[1][:, 0])
A.set_limits(xlim=(xmin - 1.0, xmax + 1.0), ylim=(-0.8, 0.8))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
# joint
joint = mpl.patches.Circle((x_car, 0), 0.005, color='k')
image.lines.append(rod)
image.patches.append(sphere)
image.patches.append(cart)
image.patches.append(joint)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex1_InvertedPendulumTranslation.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'x'), (2, 'phi')],
plotinputs=[(0, 'u')])
xmin = np.min(S.sim_data[1][:, 0])
xmax = np.max(S.sim_data[1][:, 0])
A.set_limits(xlim=(xmin - 0.5, xmax + 0.5), ylim=(-0.3, 0.8))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
linewidth=2.0)
# finally we add the patches and line to the image
image.patches.append(pendulum)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod)
# and return the image
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex0_InvertedPendulumSwingUp.pcl')
# now we can create an instance of the `Animation` class
# with our draw function and the simulation results
#
# to plot the curves of some trajectories along with the picture
# we also pass the appropriate lists as arguments (see documentation)
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'x'), (2, 'phi')],
plotinputs=[(0, 'u')])
# as for now we have to explicitly set the limits of the figure
# (may involves some trial and error)
xmin = np.min(S.sim_data[1][:, 0])
rod1 = mpl.lines.Line2D([x_car,x_pendel1],[y_car,y_pendel1],color='k',zorder=1,linewidth=2.0)
rod2 = mpl.lines.Line2D([x_car,x_pendel2],[y_car,y_pendel2],color='0.3',zorder=1,linewidth=2.0)
image.patches.append(sphere1)
image.patches.append(sphere2)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod1)
image.lines.append(rod2)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex2_InvertedDualPendulumSwingUp.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'x'),(2,'phi1'),(4,'phi2')], plotinputs=[(0,'u')])
xmin = np.min(S.sim_data[1][:,0])
xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 1.0, xmax + 1.0), ylim=(-0.8,0.8))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex2_InvertedDualPendulumSwingUp.gif')
car_heigth = 0.02
x_car = x
y_car = 0
car = mpl.patches.Rectangle((x_car-0.5*car_width, y_car-car_heigth), car_width, car_heigth,
fill=True, facecolor='grey', linewidth=2.0)
image.patches.append(car)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex6_ConstrainedDoubleIntegrator.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw, simdata=S.sim_data,
plotsys=[(0,'x'), (1,'dx')],
plotinputs=[(0,'u')])
xmin = np.min(S.sim_data[1][:,0])
xmax = np.max(S.sim_data[1][:,0])
A.set_limits(xlim=(xmin - 0.1, xmax + 0.1), ylim=(-0.1,0.1))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex6_ConstrainedDoubleIntegrator.gif')
xx = S[:, 0].copy()
yy = S[:, 1].copy()
S[:, 0] = xx * cos(theta) - yy * sin(theta) + x
S[:, 1] = yy * cos(theta) + xx * sin(theta) + y
aircraft = mpl.patches.Polygon(S, closed=True, facecolor='0.75')
image.patches.append(aircraft)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex3_Aircraft.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(4, 'theta')],
plotinputs=[(0, 'F1'), (1, 'F2')])
A.set_limits(xlim=(-1, 11), ylim=(-1, 7))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
A.save('ex3_Aircraft.gif')
color='black',
zorder=1,
linewidth=2.0)
image.patches.append(pendulum)
image.patches.append(car)
image.patches.append(joint)
image.lines.append(rod)
return image
if not 'no-pickle' in sys.argv:
# here we save the simulation results so we don't have to run
# the iteration again in case the following fails
S.save(fname='ex7_ConstrainedInvertedPendulum.pcl')
if 'plot' in sys.argv or 'animate' in sys.argv:
A = Animation(drawfnc=draw,
simdata=S.sim_data,
plotsys=[(0, 'x'), (1, 'dx')],
plotinputs=[(0, 'u1')])
xmin = np.min(S.sim_data[1][:, 0])
xmax = np.max(S.sim_data[1][:, 0])
A.set_limits(xlim=(xmin - 0.5, xmax + 0.5), ylim=(-0.6, 0.6))
if 'plot' in sys.argv:
A.show(t=S.b)
if 'animate' in sys.argv:
A.animate()
# #IPS()
# else:
# print "fail"
# IPS()
# pdict = {}
# pdict['x_f'] = x
# pdict['u_f'] = u
# fname = "model_trajectory2_7.pcl"
# with open(fname, "wb") as pfile:
# pickle.dump(pdict, pfile)
# print fname, "written"
S.save(fname='pickles/model_trajectory' + str(Tb) + '.pcl')
# IPS()
# raise SystemExit
import sys
import matplotlib as mpl
from pytrajectory.visualisation import Animation
from sympy import cos, sin
N = 3
# all rods have the same length
rod_lengths = [0.5] * N
# all pendulums have the same size
pendulum_sizes = [0.015] * N