MoveIt & pick and place

Dans la partie précédente, vous pilotiez le bras joint par joint. Ici MoveIt 2 fait le travail : vous donnez un objectif, il calcule une trajectoire articulaire sans collision. On planifie d’abord dans RViz, puis on code un pick & place complet qui saisit un cube et le dépose dans une zone cible.

Objectifs

• Comprendre ce qu’apporte MoveIt 2 (IK, planners, planning scene) et le rôle de move_group.
• Planifier et exécuter une trajectoire depuis RViz (demo.launch.py).
• Coder un pick & place en moveit_py et le tester en simulation.

1. Ce que MoveIt apporte

MoveIt 2 est la stack ROS 2 de manipulation. Plutôt que de calculer vos trajectoires à la main, vous lui donnez un objectif et il renvoie une trajectoire collision-free. Le nœud central move_group orchestre tout :

• Cinématique inverse (IK) — « quels angles donnent cette pose de pince ? » (KDL).
• Planification (OMPL) — « comment aller de A à B sans collision ? ».
• Planning Scene — la représentation du monde (obstacles, objets attachés).
• Exécution — il envoie la trajectoire du bras au arm_controller ; la pince est pilotée séparément par le gripper_controller.

move_group consomme URDF + SRDF, planifie, puis délègue l’exécution aux contrôleurs.

L’URDF décrit la géométrie ; le SRDF (déjà fourni dans so101_moveit_config, rien à générer) ajoute la sémantique : groupes de joints et poses nommées qu’on utilisera en code.

Groupe	Poses nommées	Chaîne
arm	home, ready	base_link → gripper_link
gripper	gripper_open, gripper_close	joint gripper

Deux repères de pince à distinguer

gripper_link est le bout de la chaîne du groupe arm. Mais le point de préhension réel (TCP) est gripper_frame_link, ~9,8 cm plus loin (joint fixe dans l’URDF) : c’est lui qu’on placera sur l’objet pour que les doigts soient au bon endroit, pas le corps de la pince.

2. Planifier dans RViz

ros2 launch so101_moveit_config demo.launch.py world:=pick_place.sdf

demo.launch.py est le tout-en-un : Gazebo (avec le cube et la zone de dépôt du monde pick_place.sdf) + move_group + RViz avec le panneau MotionPlanning.

1. Dans les Options (panneau MotionPlanning), cochez Approx IK Solutions.
2. Onglet Planning → glissez la sphère interactive autour de la pince pour une cible cartésienne, ou choisissez une pose nommée dans Goal State.
3. Plan : MoveIt cherche une trajectoire.
4. Execute : elle est jouée sur le bras dans Gazebo.

Pourquoi cocher « Approx IK Solutions »

Le SO-101 n’a que 5 DOF : il ne peut pas atteindre une pose 6D (position + orientation) arbitraire. Sans cette case, l’IK exacte échoue souvent et le marqueur devient rouge / le Plan échoue. Approx IK Solutions autorise une solution approchée (position respectée, orientation au plus près) — c’est le pendant RViz du « on planifie en espace des joints » qu’on code à la partie suivante.

Locale fr_FR

MoveIt 2 (Kilted) a une régression : en locale fr_FR, le séparateur décimal « , » casse le parsing de l’URDF. demo.launch.py force déjà LC_NUMERIC=C.UTF-8. Si vous lancez un nœud à la main, exportez-la : export LC_NUMERIC=C.UTF-8.

3. Pick & place en code (moveit_py)

RViz est idéal pour explorer une cible à la souris. Mais un pick & place doit ensuite s’enchaîner tout seul, sans intervention — on passe donc au code, avec moveit_py. On commence par le minimum : se connecter à MoveIt, récupérer les deux groupes (arm et gripper), et écrire deux petits helpers réutilisables — l’un planifie puis exécute, l’autre vise une pose nommée.

import time
import rclpy
from rclpy.logging import get_logger
from geometry_msgs.msg import PoseStamped
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState

ARM = "arm"
EE_LINK = "gripper_frame_link"        # TCP de préhension
GRASP_DOWN = (0.0, 1.0, 0.0, 0.0)     # quaternion pince-vers-le-bas


def plan_and_execute(robot, component, logger):
    result = component.plan()
    if not result:
        logger.error("Échec de planification.")
        return False
    robot.execute(result.trajectory, controllers=[])
    time.sleep(0.5)
    return True


def move_arm_to_named(moveit, arm, name, logger):       # home / ready
    arm.set_start_state_to_current_state()
    arm.set_goal_state(configuration_name=name)
    return plan_and_execute(moveit, arm, logger)


def set_gripper(moveit, gripper, name, logger):         # gripper_open / gripper_close
    gripper.set_start_state_to_current_state()
    gripper.set_goal_state(configuration_name=name)
    return plan_and_execute(moveit, gripper, logger)

3.1 Premier essai — poses nommées (sans cube)

Avant de saisir quoi que ce soit, vérifiez que la chaîne code → MoveIt → robot fonctionne, avec le mouvement le plus simple : enchaîner des poses nommées du SRDF. Aucun objet, aucune IK — juste home → ready → home.

def main():
    rclpy.init()
    logger = get_logger("essai")
    moveit = MoveItPy(node_name="essai")
    arm = moveit.get_planning_component("arm")

    for name in ["home", "ready", "home"]:
        move_arm_to_named(moveit, arm, name, logger)

    moveit.shutdown()
    rclpy.shutdown()

Le bras doit faire l’aller-retour dans Gazebo. Si c’est le cas, toute la boucle est opérationnelle — on peut passer à la saisie.

3.2 Le piège : ne visez pas une pose cartésienne 6D

Pour atteindre un point précis (l’objet, le dépôt), le réflexe est de passer une PoseStamped à set_goal_state(pose_stamped_msg=…). Sur le SO-101, ça échoue presque toujours.

5 DOF + IK numérique = but cartésien instable

Avec un but en pose, OMPL l’échantillonne en relançant l’IK avec des graines aléatoires. Le SO-101 n’a que 5 DOF et son solveur KDL est numérique (sensible à la graine) : la plupart des tirages échouent → Unable to sample any valid states for goal tree.

Mesuré (5 poses du TP, départ home) :

Type de but	Réussite de plan()
pose 6D (pose_stamped_msg)	0/5
configuration articulaire (robot_state)	5/5

Solution : résoudre l’IK une seule fois vers une configuration articulaire, puis planifier vers ce but articulaire. C’est la traduction concrète du « on planifie en espace des joints » vu en slides.

def make_pose(x, y, z):
    """PoseStamped dans base_link, orientation pince-vers-le-bas."""
    pose = PoseStamped()
    pose.header.frame_id = "base_link"
    pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = map(float, (x, y, z))
    qx, qy, qz, qw = GRASP_DOWN
    pose.pose.orientation.x, pose.pose.orientation.y = qx, qy
    pose.pose.orientation.z, pose.pose.orientation.w = qz, qw
    return pose


def move_arm_to_pose(moveit, arm, model, pose, logger):
    """Amène le TCP sur `pose` via un BUT ARTICULAIRE (robuste sur 5 DOF)."""
    rs = RobotState(model)
    rs.set_to_default_values(ARM, "home")              # graine de l'IK
    if not rs.set_from_ik(ARM, pose.pose, EE_LINK, timeout=0.2):
        logger.error("IK introuvable.")
        return False
    rs.update()
    arm.set_start_state_to_current_state()
    arm.set_goal_state(robot_state=rs)                 # but = la config, pas la pose
    return plan_and_execute(moveit, arm, logger)

Volume atteignable du SO-101

Portée ~0,31 m. En pince-vers-le-bas, le bras ne peut pas lever haut : gardez des hauteurs faibles (lever ~5 cm). Valeurs validées sur pick_place.sdf : cube à (0.21, 0, 0.0125), dépôt à (0.0, 0.20, 0.02).

3.3 La séquence complète

On enchaîne les briques. La pince tient le cube par friction ; un petit GRASP_OFFSET recale le TCP sur le centre des mâchoires (réglé en regardant la sim), et une remontée après le dépôt évite de pousser le cube en repartant.

OBJECT_XYZ = (0.21, 0.0, 0.0125)   # cube (monde pick_place.sdf)
PLACE_XYZ  = (0.0, 0.20, 0.02)     # zone de dépôt
APPROACH, RETREAT = 0.05, 0.05     # au-dessus / lever (limite du bras)
GRASP_OFFSET = (-0.02, 0.0, 0.0)   # TCP -> centre des mâchoires (dx, dy, dz)


def main():
    rclpy.init()
    logger = get_logger("pick_and_place")
    moveit = MoveItPy(node_name="pick_and_place")
    arm = moveit.get_planning_component("arm")
    gripper = moveit.get_planning_component("gripper")
    model = moveit.get_robot_model()

    ox, oy, oz = OBJECT_XYZ
    px, py, pz = PLACE_XYZ
    gx, gy, gz = GRASP_OFFSET
    pose = lambda x, y, z: make_pose(x + gx, y + gy, z + gz)

    steps = [
        ("home",        lambda: move_arm_to_named(moveit, arm, "home", logger)),
        ("ouvrir",      lambda: set_gripper(moveit, gripper, "gripper_open", logger)),
        ("approche",    lambda: move_arm_to_pose(moveit, arm, model, pose(ox, oy, oz + APPROACH), logger)),
        ("descente",    lambda: move_arm_to_pose(moveit, arm, model, pose(ox, oy, oz), logger)),
        ("préhension",  lambda: set_gripper(moveit, gripper, "gripper_close", logger)),
        ("retrait",     lambda: move_arm_to_pose(moveit, arm, model, pose(ox, oy, oz + RETREAT), logger)),
        ("dépôt haut",  lambda: move_arm_to_pose(moveit, arm, model, pose(px, py, pz + APPROACH), logger)),
        ("dépôt",       lambda: move_arm_to_pose(moveit, arm, model, pose(px, py, pz), logger)),
        ("relâcher",    lambda: set_gripper(moveit, gripper, "gripper_open", logger)),
        ("remontée",    lambda: move_arm_to_pose(moveit, arm, model, pose(px, py, pz + RETREAT), logger)),
        ("home",        lambda: move_arm_to_named(moveit, arm, "home", logger)),
    ]
    for label, step in steps:
        logger.info(f"→ {label}")
        if not step():
            logger.error(f"Interrompu à : {label}.")
            break
    moveit.shutdown()
    rclpy.shutdown()

3.4 Lancer le node

moveit_py a besoin de la config MoveIt injectée par un launch (MoveItConfigsBuilder). Deux pièges à connaître, gérés par le launch :

• use_sim_time=True sur le node — sinon il lit /joint_states en horloge murale et l’exécution n’est jamais validée (« couldn’t receive full current joint state »).
• un moveit_cpp.yaml qui définit planning_pipelines.pipeline_names: ["ompl"] — sinon « Failed to load any planning pipelines ».
• le pipeline OMPL au format Kilted (planning_plugins en liste, adapters en listes) — sinon « expected [string array] got [string] » au démarrage.

launch/pick_and_place.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
    cfg = (MoveItConfigsBuilder("so101", package_name="so101_moveit_config")
           .moveit_cpp(file_path="<votre_pkg>/config/moveit_cpp.yaml")
           .to_moveit_configs())
    return LaunchDescription([Node(
        package="<votre_pkg>", executable="pick_and_place", output="screen",
        parameters=[cfg.to_dict(), {"use_sim_time": True}],
        additional_env={"LC_NUMERIC": "C.UTF-8"},
    )])

Le moveit_cpp.yaml correspondant (config/moveit_cpp.yaml de votre package) :

# config/moveit_cpp.yaml — paramètres MoveItCpp pour moveit_py.
# MoveItCpp lit la forme IMBRIQUÉE planning_pipelines.pipeline_names (≠ liste plate de move_group).
planning_pipelines:
  pipeline_names: ["ompl"]

# Pipeline OMPL — FORMAT KILTED : planning_plugins est une LISTE (ex-planning_plugin string)
# et les adapters sont des listes. C'est ce qui corrige « expected [string array] got [string] ».
ompl:
  planning_plugins:
    - ompl_interface/OMPLPlanner
  request_adapters:
    - default_planning_request_adapters/ResolveConstraintFrames
    - default_planning_request_adapters/ValidateWorkspaceBounds
    - default_planning_request_adapters/CheckStartStateBounds
    - default_planning_request_adapters/CheckStartStateCollision
  response_adapters:
    - default_planning_response_adapters/AddTimeOptimalParameterization
    - default_planning_response_adapters/ValidateSolution
    - default_planning_response_adapters/DisplayMotionPath

# Params par défaut de PlanningComponent.plan() appelé sans argument.
plan_request_params:
  planning_attempts: 10
  planning_pipeline: ompl
  max_velocity_scaling_factor: 1.0
  max_acceleration_scaling_factor: 1.0
  planning_time: 5.0

OMPL déjà chargé par so101_moveit_config ?

Si so101_moveit_config fournit déjà un ompl_planning.yaml au format Kilted, le bloc ompl: ci-dessus est inutile (gardez seulement planning_pipelines et plan_request_params). Ne le dupliquez que si l’erreur « expected string array » persiste — signe que l’OMPL chargé est encore à l’ancien format.

Puis, dans deux terminaux :

# Terminal 1 — sim (cube + dépôt) + move_group + RViz
ros2 launch so101_moveit_config demo.launch.py world:=pick_place.sdf

# Terminal 2 — votre séquence
ros2 launch <votre_pkg> pick_and_place.launch.py

Le bras enchaîne la séquence dans Gazebo et le cube est transporté dans le carré de dépôt.

Saisie en simulation

Ici la pince tient le cube par friction (cube léger de 2,5 cm). C’est suffisant pour la démo, mais fragile : sur un vrai robot, c’est la force des moteurs qui tient l’objet. Pour une saisie déterministe en sim (objet solidaire de la pince), on attache l’objet — côté MoveIt (attach_collision_object, pour que le planificateur en tienne compte) et/ou côté physique. C’est l’objet de la théorie de la préhension (voir slides).

Pour aller plus loin — descente en ligne droite

Avec un but articulaire, MoveIt choisit librement le chemin. Pour une descente strictement verticale au-dessus de l’objet, on préfère parfois une trajectoire cartésienne (compute_cartesian_path, interpolation entre waypoints) : elle conserve l’orientation pendant la descente. Remplacez la descente du §3.3 par cette version et comparez la trajectoire obtenue avec le « pose à pose ».

Prochaine étape

Retour au sommaire J3, ou enchaînez sur Jour 4 — Vision.