Comment contrôler un robot avec Python

PyBullet est une plateforme de simulation open source créée par Facebook et conçue pour former des agents physiques (tels que des robots) dans un environnement 3D. Il fournit un moteur physique pour les corps rigides et mous. Il est couramment utilisé pour la robotique, l’intelligence artificielle et le développement de jeux.

Bras robotiques sont très populaires en raison de leur rapidité, de leur précision et de leur capacité à travailler dans des environnements dangereux. Ils sont utilisés pour des tâches telles que le soudage, l’assemblage et la manutention de matériaux, en particulier dans les milieux industriels (comme la fabrication).

Un robot peut effectuer une tâche de deux manières :

• Contrôle manuel – nécessite qu’un humain programme explicitement chaque action. Mieux adapté aux tâches fixes, mais aux prises avec l’incertitude et nécessite un réglage fastidieux des paramètres pour chaque nouveau scénario.
• Intelligence artificielle – permet à un robot d’apprendre les meilleures actions par essais et erreurs pour atteindre un objectif. Ainsi, il peut s’adapter à des environnements changeants en apprenant des récompenses et des pénalités sans plan prédéfini (Apprentissage par renforcement).

Dans cet article, je vais montrer comment créer un environnement 3D avec PyBullet pour contrôler manuellement un bras robotique. Je présenterai du code Python utile qui peut être facilement appliqué dans d’autres cas similaires (il suffit de copier, coller, exécuter) et de parcourir chaque ligne de code avec des commentaires afin que vous puissiez reproduire cet exemple.

Installation

PyBullet peut facilement être installé avec l’une des commandes suivantes (si pépin échoue, conda devrait certainement fonctionner) :

pip install pybullet

conda install -c conda-forge pybullet

PyBullet est livré avec une collection de préréglages Fichiers URDF (Unified Robot Description Format), qui sont des schémas XML décrivant la structure physique des objets dans l’environnement 3D (c’est-à-dire cube, sphère, robot).

import pybullet as p
import pybullet_data
import time

## setup
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(gravX=0, gravY=0, gravZ=-9.8)
p.setAdditionalSearchPath(path=pybullet_data.getDataPath())

## load URDF
plane = p.loadURDF("plane.urdf")

cube = p.loadURDF("cube.urdf", basePosition=[0,0,0.1], globalScaling=0.5, useFixedBase=True)

obj = p.loadURDF("cube_small.urdf", basePosition=[1,1,0.1], globalScaling=1.5)
p.changeVisualShape(objectUniqueId=obj, linkIndex=-1, rgbaColor=[1,0,0,1]) #red

while p.isConnected():
    p.setRealTimeSimulation(True)

Passons en revue le code ci-dessus :

• lorsque vous pouvez vous connecter au moteur physique, vous devez préciser si vous souhaitez ouvrir le interface graphique (p.GUI) ou non (p.DIRECT).
• L’espace cartésien a 3 dimensions: axe x (avant/arrière), axe y (gauche/droite), axe z (haut/bas).
• Il est courant de définir le pesanteur à (x=0, y=0, z=-9.8) simulation La gravité terrestremais on peut le changer en fonction de l’objectif de la simulation.
• Habituellement, vous devez importer un avion pour créer un sol, sinon les objets tomberaient indéfiniment. Si vous souhaitez qu’un objet soit fixé au sol, alors réglez useFixedBase=True (c’est FAUX par défaut). J’ai importé les objets avec basePosition=[0,0,0.1] ce qui signifie qu’ils sont à 10 cm du sol.
• La simulation peut être rendue dans en temps réel avec p.setRealTimeSimulation(True) ou plus rapide (temps CPU) avec p.stepSimulation(). Une autre façon de définir le temps réel est :

import time

for _ in range(240):   #240 timestep commonly used in videogame development
    p.stepSimulation() #add a physics step (CPU speed = 0.1 second)
    time.sleep(1/240)  #slow down to real-time (240 steps × 1/240 second sleep = 1 second)

Robot

Actuellement, notre environnement 3D est constitué d’un petit objet (petit cube rouge), et d’une table (gros cube) fixée au sol (avion). j’ajouterai un bras robotique avec pour tâche de ramasser le plus petit objet et de le poser sur la table.

PyBullet a un bras robotique par défaut modélisé d’après le Robot Franka Panda.

robo = p.loadURDF("franka_panda/panda.urdf", 
                   basePosition=[1,0,0.1], useFixedBase=True)

La première chose à faire est d’analyser links (les parties solides) et articulations (liaisons entre deux parties rigides) du robot. Pour cela, vous pouvez simplement utiliser p.DIRECT car il n’y a pas besoin d’interface graphique.

import pybullet as p
import pybullet_data

## setup
p.connect(p.DIRECT)
p.resetSimulation()
p.setGravity(gravX=0, gravY=0, gravZ=-9.8)
p.setAdditionalSearchPath(path=pybullet_data.getDataPath())

## load URDF
robo = p.loadURDF("franka_panda/panda.urdf", 
                  basePosition=[1,0,0.1], useFixedBase=True)

## joints
dic_info = {
    0:"joint Index",  #starts at 0
    1:"joint Name",
    2:"joint Type",  #0=revolute (rotational), 1=prismatic (sliding), 4=fixed
    3:"state vectorIndex",
    4:"velocity vectorIndex",
    5:"flags",  #nvm always 0
    6:"joint Damping",  
    7:"joint Friction",  #coefficient
    8:"joint lowerLimit",  #min angle
    9:"joint upperLimit",  #max angle
    10:"joint maxForce",  #max force allowed
    11:"joint maxVelocity",  #max speed
    12:"link Name",  #child link connected to this joint
    13:"joint Axis",
    14:"parent FramePos",  #position
    15:"parent FrameOrn",  #orientation
    16:"parent Index"  #−1 = base
}
for i in range(p.getNumJoints(bodyUniqueId=robo)):
    joint_info = p.getJointInfo(bodyUniqueId=robo, jointIndex=i)
    print(f"--- JOINT {i} ---")
    print({dic_info[k]:joint_info[k] for k in dic_info.keys()})

## links
for i in range(p.getNumJoints(robo)):
    link_name = p.getJointInfo(robo, i)[12].decode('utf-8')  #field 12="link Name"
    dyn = p.getDynamicsInfo(robo, i)
    pos, orn, *_ = p.getLinkState(robo, i)
    dic_info = {"Mass":dyn[0], "Friction":dyn[1], "Position":pos, "Orientation":orn}
    print(f"--- LINK {i}: {link_name} ---")
    print(dic_info)

Chaque robot a un « base« , la partie racine de son corps qui relie tout (comme notre colonne vertébrale squelette). En regardant le résultat du code ci-dessus, nous savons que le bras robotique a 12 articulations et 12 liens. Ils sont connectés comme ceci :

Contrôle des mouvements

Pour faire faire quelque chose à un robot, il faut bouger ses articulations. Il existe 3 principaux types de contrôle, qui sont tous des applications de Les lois du mouvement de Newton:

• Contrôle de position: en gros, vous dites au robot « va ici ». Techniquement, vous définissez un position ciblepuis appliquez une force pour déplacer progressivement l’articulation de sa position actuelle vers la cible. À mesure qu’elle se rapproche, la force appliquée diminue pour éviter tout dépassement et finit par s’équilibrer parfaitement contre les forces de résistance (c’est-à-dire friction, gravité) pour maintenir l’articulation stable en place.
• Vitesse contrôle: en gros, vous dites au robot « bougez à cette vitesse ». Techniquement, vous définissez un vitesse cibleet appliquez une force pour amener progressivement la vitesse de zéro à la cible, puis maintenez-la en équilibrant la force appliquée et les forces de résistance (c’est-à-dire friction, gravité).
• Force/Couple contrôle: en gros, vous « poussez le robot et voyez ce qui se passe ». Techniquement, vous définissez directement la force appliquée au niveau de l’articulation, et le mouvement résultant dépend uniquement de la masse, de l’inertie et de l’environnement de l’objet. En passant, en physique, le mot «forcer » est utilisé pour le mouvement linéaire (pousser/tirer), tandis que « couple » indique un mouvement de rotation (torsion/tour).

Nous pouvons utiliser p.setJointMotorControl2() pour déplacer une seule articulation, et p.setJointMotorControlArray() pour appliquer une force sur plusieurs articulations en même temps. Par exemple, j’effectuerai un contrôle de position en donnant une cible aléatoire pour chaque articulation du bras.

## setup
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(gravX=0, gravY=0, gravZ=-9.8)
p.setAdditionalSearchPath(path=pybullet_data.getDataPath())

## load URDF
plane = p.loadURDF("plane.urdf")
cube = p.loadURDF("cube.urdf", basePosition=[0,0,0.1], globalScaling=0.5, useFixedBase=True)
robo = p.loadURDF("franka_panda/panda.urdf", basePosition=[1,0,0.1], useFixedBase=True)
obj = p.loadURDF("cube_small.urdf", basePosition=[1,1,0.1], globalScaling=1.5)
p.changeVisualShape(objectUniqueId=obj, linkIndex=-1, rgbaColor=[1,0,0,1]) #red

## move arm
joints = [0,1,2,3,4,5,6]
target_positions = [1,1,1,1,1,1,1] #<--random numbers
p.setJointMotorControlArray(bodyUniqueId=robo, jointIndices=joints,
                            controlMode=p.POSITION_CONTROL,
                            targetPositions=target_positions,
                            forces=[50]*len(joints))
for _ in range(240*3):
    p.stepSimulation()
    time.sleep(1/240)

La vraie question est : «quelle est la bonne position cible pour chaque articulation ? » La réponse est Cinématique inversele processus mathématique de calcul des paramètres nécessaires pour placer un robot dans une position et une orientation données par rapport à un point de départ. Chaque joint définit un angle et vous ne voulez pas deviner plusieurs angles de joint à la main. Alors, nous demanderons PyBullet déterminer les positions cibles dans l’espace cartésien avec p.calculateInverseKinematics().

obj_position, _ = p.getBasePositionAndOrientation(obj)
obj_position = list(obj_position)

target_positions = p.calculateInverseKinematics(
    bodyUniqueId=robo,
    endEffectorLinkIndex=11, #grasp-target link
    targetPosition=[obj_position[0], obj_position[1], obj_position[2]+0.25], #25cm above object
    targetOrientation=p.getQuaternionFromEuler([0,-3.14,0]) #[roll,pitch,yaw]=[0,-π,0] -> hand pointing down
)

arm_joints = [0,1,2,3,4,5,6]
p.setJointMotorControlArray(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                            jointIndices=arm_joints,
                            targetPositions=target_positions[:len(arm_joints)],
                            forces=[50]*len(arm_joints))

Veuillez noter que j’ai utilisé p.getQuaternionFromEuler() à convertir les angles 3D (faciles à comprendre pour les humains) en 4Dcomme « quaternions« sont plus faciles à calculer pour les moteurs physiques. Si vous voulez devenir technique, de manière quaternion (x, y, z, w)les trois premières composantes décrivent l’axe de rotation, tandis que la quatrième dimension code la quantité de rotation (parce que/péché).

Le code ci-dessus commande au robot de déplacer sa main vers une position spécifique au-dessus d’un objet à l’aide de la cinématique inverse. Nous saisissons où se trouve le petit cube rouge dans le monde avec p.getBasePositionAndOrientation()et nous utilisons les informations pour calculer la position cible des articulations.

Interagir avec des objets

Le bras du robot est doté d’une main (« pince »), qui peut donc être ouverte et fermée pour saisir des objets. De l’analyse précédente, nous savons que les « doigts » peuvent se déplacer à l’intérieur (0-0,04). Ainsi, vous pouvez définir la position cible comme limite inférieure (main fermée) ou la limite supérieure (main ouverte).

De plus, je veux m’assurer que le bras tient le petit cube rouge tout en se déplaçant. Vous pouvez utiliser p.createConstraint() faire un lien physique temporaire entre la pince du robot et l’objet, afin qu’il se déplace avec la main du robot. Dans le monde réel, la pince appliquerait une force par friction et contact pour presser l’objet afin qu’il ne tombe pas. Mais, puisque PyBullet est un simulateur très simple, je vais juste prendre ce raccourci.

## close hand
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=9, #finger_joint1
                        targetPosition=0, #lower limit for finger_joint1
                        force=force)
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=10, #finger_joint2
                        targetPosition=0, #lower limit for finger_joint2
                        force=force)

## hold the object
constraint = p.createConstraint(
    parentBodyUniqueId=robo,
    parentLinkIndex=11,
    childBodyUniqueId=obj,
    childLinkIndex=-1,
    jointType=p.JOINT_FIXED,
    jointAxis=[0,0,0],
    parentFramePosition=[0,0,0],
    childFramePosition=[0,0,0,1]
)

Après cela, nous pouvons déplacer le bras vers la table, en utilisant la même stratégie que précédemment : Cinématique inverse -> position cible -> contrôle de position.

Enfin, lorsque le robot atteint la position cible dans l’espace cartésien, on peut ouvrir la main et libérer la contrainte entre l’objet et le bras.

## close hand
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=9, #finger_joint1
                        targetPosition=0.04, #upper limit for finger_joint1
                        force=force)
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=10, #finger_joint2
                        targetPosition=0.04, #upper limit for finger_joint2
                        force=force)

## drop the obj
p.removeConstraint(constraint)

Contrôle manuel complet

Dans PyBulletvous devez restituer la simulation pour chaque action effectuée par le robot. Par conséquent, il est pratique d’avoir une fonction utilitaire capable d’accélérer (c’est-à-dire seconde=0,1) ou ralentir (c’est-à-dire seconde=2) le temps réel (seconde=1).

import pybullet as p
import time

def render(sec=1):
    for _ in range(int(240*sec)):
        p.stepSimulation()
        time.sleep(1/240)

Voici le code complet de la simulation que nous avons réalisée dans cet article.

import pybullet_data

## setup
p.connect(p.GUI)
p.resetSimulation()
p.setGravity(gravX=0, gravY=0, gravZ=-9.8)
p.setAdditionalSearchPath(path=pybullet_data.getDataPath())

plane = p.loadURDF("plane.urdf")
cube = p.loadURDF("cube.urdf", basePosition=[0,0,0.1], globalScaling=0.5, useFixedBase=True)
robo = p.loadURDF("franka_panda/panda.urdf", basePosition=[1,0,0.1], globalScaling=1.3, useFixedBase=True)
obj = p.loadURDF("cube_small.urdf", basePosition=[1,1,0.1], globalScaling=1.5)
p.changeVisualShape(objectUniqueId=obj, linkIndex=-1, rgbaColor=[1,0,0,1])

render(0.1)
force = 100


## open hand
print("### open hand ###")
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=9, #finger_joint1
                        targetPosition=0.04, #upper limit for finger_joint1
                        force=force)
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=10, #finger_joint2
                        targetPosition=0.04, #upper limit for finger_joint2
                        force=force)
render(0.1)
print(" ")


## move arm
print("### move arm ### ")
obj_position, _ = p.getBasePositionAndOrientation(obj)
obj_position = list(obj_position)

target_positions = p.calculateInverseKinematics(
    bodyUniqueId=robo,
    endEffectorLinkIndex=11, #grasp-target link
    targetPosition=[obj_position[0], obj_position[1], obj_position[2]+0.25], #25cm above object
    targetOrientation=p.getQuaternionFromEuler([0,-3.14,0]) #[roll,pitch,yaw]=[0,-π,0] -> hand pointing down
)
print("target position:", target_positions)

arm_joints = [0,1,2,3,4,5,6]
p.setJointMotorControlArray(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                            jointIndices=arm_joints,
                            targetPositions=target_positions[:len(arm_joints)],
                            forces=[force/3]*len(arm_joints))

render(0.5)
print(" ")


## close hand
print("### close hand ###")
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=9, #finger_joint1
                        targetPosition=0, #lower limit for finger_joint1
                        force=force)
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=10, #finger_joint2
                        targetPosition=0, #lower limit for finger_joint2
                        force=force)
render(0.1)
print(" ")


## hold the object
print("### hold the object ###")
constraint = p.createConstraint(
    parentBodyUniqueId=robo,
    parentLinkIndex=11,
    childBodyUniqueId=obj,
    childLinkIndex=-1,
    jointType=p.JOINT_FIXED,
    jointAxis=[0,0,0],
    parentFramePosition=[0,0,0],
    childFramePosition=[0,0,0,1]
)
render(0.1)
print(" ")


## move towards the table
print("### move towards the table ###")
cube_position, _ = p.getBasePositionAndOrientation(cube)
cube_position = list(cube_position)

target_positions = p.calculateInverseKinematics(
    bodyUniqueId=robo,
    endEffectorLinkIndex=11, #grasp-target link
    targetPosition=[cube_position[0], cube_position[1], cube_position[2]+0.80], #80cm above the table
    targetOrientation=p.getQuaternionFromEuler([0,-3.14,0]) #[roll,pitch,yaw]=[0,-π,0] -> hand pointing down
)
print("target position:", target_positions)

arm_joints = [0,1,2,3,4,5,6]
p.setJointMotorControlArray(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                            jointIndices=arm_joints,
                            targetPositions=target_positions[:len(arm_joints)],
                            forces=[force*3]*len(arm_joints))
render()
print(" ")


## open hand and drop the obj
print("### open hand and drop the obj ###")
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=9, #finger_joint1
                        targetPosition=0.04, #upper limit for finger_joint1
                        force=force)
p.setJointMotorControl2(bodyUniqueId=robo, controlMode=p.POSITION_CONTROL,
                        jointIndex=10, #finger_joint2
                        targetPosition=0.04, #upper limit for finger_joint2
                        force=force)
p.removeConstraint(constraint)
render()

Conclusion

Cet article a été un tutoriel sur la façon de contrôler manuellement un bras robotique. Nous avons appris le contrôle du mouvement, la cinématique inverse, la saisie et le déplacement d’objets. De nouveaux tutoriels avec des robots plus avancés arriveront.

Code complet pour cet article : GitHub

J’espère que vous l’avez apprécié ! N’hésitez pas à me contacter pour des questions et des commentaires, ou simplement pour partager vos projets intéressants.

👉 Connectons-nous 👈

_{^{(Toutes les images sont de l’auteur, sauf indication contraire)}}