Integrating MoveIt

​

Why MoveIt isn’t in the SDK

​

Run MoveIt as a sidecar

• a moveit compose service on the cyberwaveos/ros2-moveit image, in the twin’s ROS namespace;
• the plant (your driver or its vendor node) publishes robot_description, the TF tree, joint states and the MoveIt config package for this arm;
• trajectory bridges expose the arm’s FollowJointTrajectory action, so MoveIt executes through the same controller as everything else.

​

Write a MotionBackend

• name and planning_frame;
• available() and unavailable_refusal(): cheap checks that the action servers and services are ready, and the reason when they aren’t;
• validate_goal(goal) and plan_and_execute(goal, on_update, on_done): rclpy action clients for sequence_move_group (Pilz PTP and LIN) and move_action (OMPL). A goal is a MotionGoal (one pose, PTP or LIN) or a GraspGoal (ranked grasp candidates). Both arrive in your planning frame. A backend without a grasp search refuses a GraspGoal in validate_goal;
• apply_scene(request, diff), attach(request) and detach(object_id): the apply_planning_scene and get_planning_scene services, sent as diffs;
• cancel(): cancel the active goal, fire and forget.

from cyberwave.driver.motion import (
    MotionOutcome,
    MotionResult,
    MotionUpdate,
    backend_unavailable,
)


class MoveItBackend:
    name = "moveit"
    planning_frame = "base_link"

    def __init__(self, node):
        self._node = node
        self._clients = None
        self._error = ""

    def _connect(self):
        if self._clients is None and not self._error:
            try:
                from moveit_msgs.action import MoveGroup, MoveGroupSequence
                from rclpy.action import ActionClient
            except ImportError as exc:
                self._error = f"moveit_msgs is not installed ({exc})"
                return None
            ns = self._node.namespaced
            self._clients = (
                ActionClient(self._node, MoveGroupSequence, ns("sequence_move_group")),
                ActionClient(self._node, MoveGroup, ns("move_action")),
            )
        return self._clients

    def available(self):
        clients = self._connect()
        return clients is not None and all(c.server_is_ready() for c in clients)

    def unavailable_refusal(self):
        return backend_unavailable(
            self.name, self._error or "move_group action servers are not ready"
        )

    def validate_goal(self, goal):
        return None  # PTP, LIN and grasp sequences all exist on MoveIt

    def plan_and_execute(self, goal, on_update, on_done):
        on_update(MotionUpdate(planner="pilz_ptp"))
        # Send a MoveGroupSequence goal. On "MONITOR" feedback, report
        # on_update(MotionUpdate(executing=True)). On the result, map the
        # MoveItErrorCodes value to an outcome and call on_done exactly once:
        on_done(MotionResult(MotionOutcome.SUCCEEDED, planner="pilz_ptp"))

    def apply_scene(self, request, diff): ...
    def attach(self, request): ...
    def detach(self, object_id): ...
    def cancel(self): ...

​

Register it and switch from config

class MyArmDriver(PlannedMotionMixin, WorldObstaclesMixin, ArmCapabilityMixin, BaseROS2Driver):
    kinematics_backend = "pinocchio"

    def _motion_backend_factories(self):
        return {**super()._motion_backend_factories(), "moveit": self._build_moveit_backend}

    def _build_moveit_backend(self):
        from my_driver.motion.moveit import MoveItBackend

        return MoveItBackend(self)

    def configure(self):
        ...
        self.kinematics_backend = "moveit" if self._config.use_moveit else "pinocchio"

​

Status and refusals

• state/motion carries kinematics_backend (the driver’s default backend), backend_available, backend (the one answering the current motion), and planner, the planner name your backend reports (for example pilz_ptp, pilz_lin or ompl);
• when available() is false, every planned-motion command is refused with backend_unavailable. Build the refusal with backend_unavailable(name, detail) so the detail names the backend;
• a failed plan is planning_failed, a controller that didn’t follow is execution_failed, and a stopped goal is cancelled;
• guards, their order, scene revisions and the attached payload are handled by the SDK before your backend is called. A backend first used after a scene_set is given that scene, and any carried payload, before its first goal.

​

Test with fakes

​

Reference implementations

• The Piper driver’s MoveIt backend, cw_piper/motion/moveit/ in cyberwave-edge-nodes/piper/public: configuration, the planner chain (Pilz PTP, then OMPL on failures it can route around), the grasp as Pilz sequences, MoveItErrorCodes classification and the rclpy client, with its tests.
• The Kinova Gen3 driver’s bridge, cyberwave_kinova_bringup/scripts/cyberwave_bridge.py: a guarded moveit_msgs import and a MoveGroupSequence client for Cartesian waypoint missions.