Projet Asimov fait par Bruno-Pier Busque

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')
+

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()

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()

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()

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()