Robotique de manipulation avec Sawyer

La robotique de manipulation regroupe la manipulation d’objets avec des robots : des bras articulés à 5 ou 6 axes, les robots SCARA (Selective Compliance Assembly Robot Arm), les robots cartésiens (linéaires), les robots parallèles … Dans ce TP nous utilisons un robot Sawyer du fabriquant Rethink Robotics.

Prérequis

Diapositives

Alternative text - include a link to the PDF!

1. Documentation

1.1. Les liens

1.2. Le plus important

Voici les quelques fonctions les plus importantes traduites depuis la documentation.

1.2.1. Déclarer un commandeur de robot

commander = MoveGroupCommander("right_arm")

Ce commandeur est celui qui identifie notre robot et permettra d’en prendre le contrôle.

1.2.2. Exécuter un mouvement vers une cible

Définir une cible, dans l’espace des joints :

commander.set_joint_value_target([-1.0, -2.0, 2.8, 1.5, 0.1, -0.3, 3.0])

Les 7 valeurs sont les angles cibles des 7 joints en radians Attention : les cibles dans l’espace des joints n’auront pas d’évitement de collisions

Ou bien définir une cible dans l’espace cartésien :

commander.set_pose_target([0.5, 0.05, 1.1, 0, 0, 0, 1])

Les 7 valeurs sont la position et l'orientation [x, y, z, qx, qy, qz, qw] cible de l’effecteur dans le repère base

1.2.3. Planifier & exécuter le mouvement vers la cible

La fonction “go” déclenche le calcul de trajectoire et l’exécution instantanée si le calcul a réussi :

commander.go()

1.2.4. Exécuter une trajectoire cartésienne

Par opposition à la cible cartésienne, dans cet exemple au lieu de ne définir qu’une cible finale, on demande à MoveIt de suivre une trajectoire rectiligne dans l’espace cartésien. Cette trajectoire est précalculée en entier grâce à la fonction commander.compute_cartesian_path([pose1, pose2]), resolution, jump) où :

La fonction commander.compute_cartesian_path(...) renvoie :

Par exemple, étant données 2 points p1 et p2 de type geometry_msgs.Pose, cet appel est valide :

trajectory, ratio = commander.compute_cartesian_path([pose1, pose2]), 0.01, 3.14)

Enfin, exécuter la trajectoire, uniquement si le ratio indique au moins 95% de succès :

commander.execute(trajectory)

1.2.5. Définir des objets de collision

Lorsqu’on ajoute des objets de collision, les appels à go() dans l’espace cartésien planifieront, si possible, une trajectoire en évitant les objets déclarés comme objets de collision.

La scène de planification est notre interface pour ajouter ou supprimer des objets de collision :

scene = PlanningSceneInterface()

On peut ensuite effectuer les ajouts ou suppressions. Par exemple, on ajoute un objet de collision cubique de taille 10x8x2 cm à la position [1.2, 0.5, 0.55] et avec l’orientation [0, 0, 0, 1] (= rotation identité) dans le repère base:

ps = PoseStamped()
ps.header.frame_id = "base"
ps.pose.position.x = 1.2
ps.pose.position.y = 0.5
ps.pose.position.z = 0.55
ps.pose.orientation.x = 0
ps.pose.orientation.y = 0
ps.pose.orientation.z = 0
ps.pose.orientation.w = 1
scene.add_box("ma_boite", list_to_pose_stamped2([[1.2, 0.5, 0.55], [0, 0, 0, 1]]), (0.10, 0.08, 0.02))

Les objets de collision apparaissent en vert dans RViz s’ils sont définis correctement.

Note: après une modification de la scène, est généralement utile de faire une pause rospy.sleep(1) afin de laisser le temps à la scène d’être mise à jour sur tout le système avant toute nouvelle planification de trajectoire

2. Travaux pratiques

2.1. Utiliser MoveIt dans le visualisateur Rviz

Avec roslaunch, lancer sawyer_moveit.launch provenant du package sawyer_moveit_config:

roslaunch sawyer_moveit_config sawyer_moveit.launch

Via l’interface graphique, changer l’orientation et la position de l’effecteur puis demander à MoveIt de planifier et exécuter une trajectoire pour l’atteindre.

2.2. Utiliser MoveIt via son client Python

Dans le package ros4pro, ouvrir le nœud manipulate.py. Repérez les 3 exemples :

Durant la suite du TP, nous démarrerons notre programme de manipulation avec le launchfile manipulate.launch du package ros4pro qui fonctionne par défaut en mode simulé, à savoir :

roslaunch ros4pro manipulate.launch

Celui-ci démarre automatiquement manipulate.py, il est donc inutile de le démarrer par un autre moyen.

2.2.1. Modifier la scène de planification

La scène représente tout ce qui rentre en compte dans les mouvements du robot et qui n’est pas le robot lui-même : les obstacles et/ou les objets à attraper. Ces éléments sont déclarés à MoveIt comme des objets de collision (détachés du robot ou attachés c’est-à-dire qu’ils bougent avec lui).

2.2.2. Effectuer un pick-and-place avec un cube simulé (optionnel)

Nous considérons un cube situé à la pose ᵇᵃˢᵉP꜀ᵤ₆ₑ ᵇᵃˢᵉP꜀ᵤ₆ₑ_ᵥₑᵣₜ = [[0.32, 0.52, 0.32], [1, 0, 0, 0]], ce qui correspond exactement à l’emplacement entouré en vert sur le feeder.

Pour l’approche, on positionnera le gripper 18cm au dessus du cube le long de son axe z.

Ensuite, dans votre code :

2.2.3. Générer les 4 trajectoires du pick-and-place

Pour rappel, voici les 4 étapes d’un pick pour attraper et relacher le cube simulé : 1. trajectoire d’approche : aller sans collision à ᵇᵃˢᵉP꜀ᵤ₆ₑ_ᵥₑᵣₜ c’est à dire 18cm au dessus du cube sur son axe z (axe bleu) 2. trajectoire de descente : suivre une trajectoire cartésienne de 50 points descendant le long de l’axe z pour atteindre ᵇᵃˢᵉP꜀ᵤ₆ₑ_ᵥₑᵣₜ avec l’effecteur right_gripper_tip. Puis fermer l’effecteur. 3. trajectoire de retraite : retourner au point d’approche par une trajectoire cartésienne 4. trajectoire de dépose : si le cube a bel-et-bien été attrapé avec succès, aller sans collision au point de dépose ᵇᵃˢᵉP꜀ₔₑₚₒₛₑ = [[0.5, 0, 0.1], [0.707, 0.707, 0, 0]]

2.3. Exécutez le pick-and-place sur le Sawyer réel

export ROS_MASTER_URI=http://021608CP00013.local:11311
roslaunch ros4pro manipulate.launch simulate:=false

Vous devriez constater que votre pick-and-place fonctionne parfois et qu’il échoue dans certaines situations.

2.4. Préparer le pipeline du scenario final (optionnel)

Ajoutez à manipulate.py le code nécessaire pour votre scenario final :

2.5. Prévenir les échecs de planification

Le path-planning étant réalisé pendant l’exécution et non précalculé, il est possible que MoveIt ne trouve aucune solution de mouvement. Pour remédier à cela, plusieurs pistes s’offrent à nous :

MoveIt possède un système de calcul de la géométrie directe et inverse, respectivement via les services /compute_fk et /compute_ik. Observer les types de service (rosservice info) et le contenu de la requête (rossrv show) puis appeler ces deux services sur ces deux exemples :

Réexécutez le même calcul d’IK sur la même position d’effecteur, une seconde puis une troisième fois. Observez que le résultat est différent. Pourquoi ?