# Fine traj optimization
n = 100
dt = tf / n
planner2 = trajectoryoptimisation.DirectCollocationTrajectoryOptimisation(
sys, dt, n)
planner2.x_start = np.array([-3.14, 0, 0, 0])
planner2.x_goal = np.array([0, 0, 0, 0])
planner2.set_initial_trajectory_guest(planner.traj)
planner2.maxiter = 500
planner2.compute_optimal_trajectory()
planner2.show_solution()
planner2.save_solution('double_pendulum_directcollocation_hires.npy')
# Controller
ctl = nonlinear.ComputedTorqueController(sys, planner2.traj)
ctl.rbar = np.array([0, 0])
ctl.w0 = 5
ctl.zeta = 1
# New cl-dynamic
cl_sys = ctl + sys
# Simultation
cl_sys.x0 = np.array([-3.14, 0, 0, 0])
cl_sys.plot_trajectory('xu')
cl_sys.animate_simulation()
"""
###############################################################################
from pathlib import Path
import numpy as np
###############################################################################
from pyro.dynamic import pendulum
from pyro.control import nonlinear
from pyro.planning import plan
###############################################################################
sys = pendulum.DoublePendulum()
# Load pre-computed trajectory from file
this_file_dir = Path(__file__).parent
traj_file = this_file_dir.joinpath(Path('double_pendulum_rrt.npy'))
traj = plan.load_trajectory(str(traj_file))
# Controller
ctl = nonlinear.ComputedTorqueController(sys, traj)
# goal
ctl.rbar = np.array([0, 0])
# New cl-dynamic
cl_sys = ctl + sys
# Simultation
x_start = np.array([-3.14, 0, 0, 0])
cl_sys.plot_phase_plane_trajectory(x_start)
cl_sys.sim.plot('xu')
cl_sys.animate_simulation()
###############################################################################
import numpy as np
import matplotlib.pyplot as plt
###############################################################################
from asimov import Asimov
from pyro.control import nonlinear
###############################################################################
asimov = Asimov() # Asimov
x0 = np.array([-np.pi / 4, -3 * np.pi / 4, np.pi / 2, 0, 0,
0]) # Position initiale
ctl = nonlinear.ComputedTorqueController(asimov) # Déclaration du controlleur
ctl.rbar = np.array([0.5, 0.25, 0]) # Cible
ctl.w0 = 2
ctl.zeta = 1
closed_loop_robot = ctl + asimov # Système boucle fermé
closed_loop_robot.plot_trajectory(x0, 5) # Calcul de la trajectoire
closed_loop_robot.sim.plot('x') # Affichage des états
closed_loop_robot.sim.plot('u') # Affichage des commandes
closed_loop_robot.animate_simulation(1, True) # Animation et enregistrement
plt.show()
