diff --git a/projects/asimov/asimov.py b/projects/asimov/asimov.py new file mode 100644 index 00000000..715309da --- /dev/null +++ b/projects/asimov/asimov.py @@ -0,0 +1,113 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon July 22 2019 + +@author: Bruno-Pier Busque +""" + +############################################################################### +import numpy as np +############################################################################### +from pyro.dynamic import manipulator +############################################################################### + + +############################################################################### +# 3D Asimov +############################################################################### + +class Asimov( manipulator.ThreeLinkManipulator3D ): + """ + Asimov Manipulator Class + ------------------------------- + """ + + ############################ + def __init__(self): + """ """ + + # initialize standard params + manipulator.ThreeLinkManipulator3D.__init__(self) + + # Name + self.name = 'Asimov Manipulator' + + # Kinematic params + self.l1 = 0.5 # [m] + self.baseradius = 0.3 # [m] + self.armradius = 0.05 # [m] + self.l2 = 0.525 # [m] + self.l3 = 0.375 # [m] + self.lc1 = self.l1/2 # [m] + self.lc2 = self.l2/2 # [m] + self.lc3 = self.l3/2 # [m] + self.lw = (self.l1 + self.l2 + self.l3) # Total length + + # Inertia params + self.m1 = 3.703 # [kg] + self.mbras = 0.915 # [kg] + self.mcoude = 0.832 # [kg] + self.m2 = self.mbras + self.mcoude # [kg] + self.m3 = 0.576 # [kg] + + self.I1z = self.m1*(self.baseradius**2)/2 + + self.I2x = (self.mbras*self.l2**2)/3 + self.mcoude*self.l2**2 + self.I2y = self.I2x + self.I2z = self.m2*(self.armradius**2)/2 + + self.I3x = (self.m3*self.l3**2)/3 + self.I3y = self.I3x + self.I3z = self.m3*(self.armradius**2)/2 + + # Labels, bounds and units + self.x_ub = np.array([np.pi/2, -np.pi/4, np.pi/2]) + self.x_lb = np.array([-np.pi/2, -3*np.pi/4, -np.pi/2]) + + +############################################################################### +# 2D Asimov +############################################################################### + +class Asimov2D( manipulator.TwoLinkManipulator ): + """ + Asimov 2D Manipulator Class + ------------------------------- + A model of Asimov without the rotating base + """ + + ############################ + def __init__(self): + """ """ + + # initialize standard params + manipulator.TwoLinkManipulator.__init__(self) + + # Name + self.name = 'Asimov 2D Manipulator' + + self.l1 = 0.525 # [m] + self.l2 = 0.375 + self.lc1 = self.l1/2 + self.lc2 = self.l2/2 + + self.mbras = 0.915 # [kg] + self.mcoude = 0.832 # [kg] + self.m1 = self.mbras + self.mcoude # [kg] + self.m2 = 0.576 # [kg] + self.I1 = (self.mbras*self.l1**2)/3 + self.mcoude*self.l1**2 + self.I2 = (self.m2*self.l2**2)/3 + + +if __name__ == "__main__": + """ MAIN TEST """ + + sys = Asimov() + dsys = manipulator.SpeedControlledManipulator(sys) + dsys.ubar = np.array([1, 1, 1]) + dsys.show3([0.1, 0.1, 0.1]) + x02 = np.array([0, 1, 1]) # Position initiale + + dsys.plot_animation(x02) + dsys.sim.plot('xu') + diff --git a/projects/asimov/asimov_computed_torque_controller.py b/projects/asimov/asimov_computed_torque_controller.py new file mode 100644 index 00000000..6735d521 --- /dev/null +++ b/projects/asimov/asimov_computed_torque_controller.py @@ -0,0 +1,24 @@ +############################################################################### +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() diff --git a/projects/asimov/asimov_endeffector_pid_controller.py b/projects/asimov/asimov_endeffector_pid_controller.py new file mode 100644 index 00000000..ae0c485d --- /dev/null +++ b/projects/asimov/asimov_endeffector_pid_controller.py @@ -0,0 +1,29 @@ +############################################################################### +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() diff --git a/projects/asimov/asimov_joint_pid_controller.py b/projects/asimov/asimov_joint_pid_controller.py new file mode 100644 index 00000000..9ea87cc7 --- /dev/null +++ b/projects/asimov/asimov_joint_pid_controller.py @@ -0,0 +1,28 @@ +############################################################################### +import numpy as np +import matplotlib.pyplot as plt +############################################################################### +from asimov import Asimov +from pyro.control import robotcontrollers +############################################################################### +kp = 40 +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 + +ctl = robotcontrollers.JointPID(3, kp, ki, kd) # Déclaration du controlleur +ctl.rbar = qd +ctl.kd = np.array([5, 8, 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 + +plt.show() diff --git a/projects/asimov/asimov_kinematic_controller.py b/projects/asimov/asimov_kinematic_controller.py new file mode 100644 index 00000000..662f7ff7 --- /dev/null +++ b/projects/asimov/asimov_kinematic_controller.py @@ -0,0 +1,23 @@ +############################################################################### +import numpy as np +import matplotlib.pyplot as plt +############################################################################### +from pyro.control import robotcontrollers +from pyro.dynamic import manipulator +from asimov import Asimov +############################################################################### +gain = 1 # Gain du controlleur + +asimov = Asimov() # Asimov +sc_asimov = manipulator.SpeedControlledManipulator(asimov) # Asimov controllé en vitesse +x0 = np.array([3*np.pi/4, -np.pi/2, np.pi/2]) # Position initiale [q1, q2, q3] + +ctl = robotcontrollers.EndEffectorKinematicController(sc_asimov, gain) # Déclaration du controlleur +ctl.rbar = np.array([0.5, 0.25, 0]) # Cible + +closed_loop_robot = ctl + sc_asimov # Système boucle fermé + +closed_loop_robot.plot_trajectory(x0, 5) # Calcul de la trajectoire +closed_loop_robot.animate_simulation(1.0, True ) # Animation + +plt.show()