def compute_trajectory(self, x0, tf=10, n=10001, solver='ode'):
"""
Simulation of time evolution of the system
------------------------------------------------
x0 : initial time
tf : final time
"""
self.sim = simulation.CLosedLoopSimulation(self, tf, n, solver)
self.sim.x0 = x0
self.sim.compute()
def plot_phase_plane_trajectory(self, x0, tf=10, x_axis=0, y_axis=1):
"""
Simulates the system and plot the trajectory in the Phase Plane
------------------------------------------------
x0 : initial time
tf : final time
x_axis : index of state on x axis
y_axis : index of state on y axis
"""
self.sim = simulation.CLosedLoopSimulation(self, tf)
self.sim.x0 = x0
self.sim.compute()
self.sim.phase_plane_trajectory_closed_loop(x_axis, y_axis)
from pyro.dynamic import pendulum
from pyro.control import robotcontrollers
from pyro.analysis import simulation
###############################################################################
sys = pendulum.SinglePendulum()
dof = 1
kp = 2 # 2,4
kd = 1 # 1
ki = 1
ctl = robotcontrollers.JointPID(dof, kp, ki, kd)
# Set Point
q_target = np.array([3.14])
ctl.rbar = q_target
# New cl-dynamic
cl_sys = ctl + sys
# Simultation
x_start = np.array([0, 0])
cl_sys.sim = simulation.CLosedLoopSimulation(cl_sys, 20, 20001, 'euler')
cl_sys.sim.x0 = x_start
cl_sys.sim.compute()
cl_sys.sim.phase_plane_trajectory(0, 1)
cl_sys.sim.plot('xu')
cl_sys.animate_simulation()
###############################################################################
# Controller selection
ctl = p_ctl
#ctl = pd_ctl
#ctl = pid_ctl
#ctl = ctc_ctl
#ctl = sld_ctl
#ctl = traj_ctl
#ctl = traj_ctc_ctl
#ctl = traj_sld_ctl
###############################################################################
# New cl-dynamic
cl_sys = ctl + sys
# Simultation
q0 = 0
tf = 10
cl_sys.sim = simulation.CLosedLoopSimulation(cl_sys, tf, tf * 1000 + 1,
'euler')
cl_sys.sim.x0 = np.array([q0, 0])
cl_sys.sim.compute()
cl_sys.sim.plot('xu')
cl_sys.animate_simulation()
cl_sys.sim.phase_plane_trajectory(0, 1)
sys.plot_phase_plane()