import numpy as np
import matplotlib.pyplot as plt
###############################################################################
from asimov import Asimov
from pyro.control import robotcontrollers
###############################################################################
kp = 80
ki = 0
kd = 0
asimov = Asimov() # Asimov
x0 = np.array([-np.pi / 4, -3 * np.pi / 4, np.pi / 2, 0, 0,
0]) # Position initiale
qd = np.array([0.5, -np.pi / 4, 0.5]) # Cible au joint
rd = asimov.forward_kinematic_effector(qd)
ctl = robotcontrollers.EndEffectorPID(asimov, kp, ki,
kd) # Déclaration du controlleur
ctl.rbar = rd # Cible
ctl.kd = np.array([10, 30, 30])
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()
###############################################################################
from pyro.dynamic import manipulator
from pyro.control import robotcontrollers
###############################################################################
# Model
torque_controlled_robot = manipulator.ThreeLinkManipulator3D()
# Target
r_desired = np.array([0.5, 1, 2])
# Effector pid
model = torque_controlled_robot
effector_pid = robotcontrollers.EndEffectorPID(model)
effector_pid.rbar = r_desired
effector_pid.kp = np.array([100, 100, 100])
effector_pid.kd = np.array([30, 30, 30])
effector_pid.ki = np.array([90, 90, 90])
# Closed-loops
robot_with_effector_pid = effector_pid + torque_controlled_robot
# Simulation
robot_with_effector_pid.x0 = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0])
robot_with_effector_pid.compute_trajectory(10)
robot_with_effector_pid.plot_trajectory('x')
robot_with_effector_pid.plot_internal_controller_states()
robot_with_effector_pid.plot_end_effector_trajectory()
###############################################################################
from pyro.control import robotcontrollers
from pyro.dynamic import manipulator
###############################################################################
torque_controlled_robot = manipulator.TwoLinkManipulator()
# Target
q_desired = np.array([0.5, 0.5])
r_desired = torque_controlled_robot.forward_kinematic_effector(q_desired)
# effector PID
dof = 2
effector_pid = robotcontrollers.EndEffectorPID(torque_controlled_robot)
effector_pid.rbar = r_desired
effector_pid.kp = np.array([100, 100])
effector_pid.kd = np.array([0, 0])
effector_pid.ki = np.array([50, 50])
# Closed-loops
robot_with_effector_pid = effector_pid + torque_controlled_robot
# Simulations
tf = 20
robot_with_effector_pid.x0 = np.array([0, 0, 0, 0, 0, 0])
robot_with_effector_pid.compute_trajectory(tf)
robot_with_effector_pid.plot_trajectory('xu')
robot_with_effector_pid.plot_internal_controller_states()