diff --git a/doc/examples/realtime_servo/realtime_servo_tutorial.rst b/doc/examples/realtime_servo/realtime_servo_tutorial.rst index 151ea6dc56..04a8e1de6b 100644 --- a/doc/examples/realtime_servo/realtime_servo_tutorial.rst +++ b/doc/examples/realtime_servo/realtime_servo_tutorial.rst @@ -28,7 +28,7 @@ Design overview --------------- MoveIt Servo consists of two main parts: The core implementation ``Servo`` which provides a C++ interface, and the ``ServoNode`` which -wraps the C++ interface and provides a ROS interface.The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml ` +wraps the C++ interface and provides a ROS interface. The configuration of Servo is done through ROS parameters specified in :moveit_codedir:`servo_parameters.yaml `. In addition to the servoing capability, MoveIt Servo has some convenient features such as: @@ -40,7 +40,10 @@ In addition to the servoing capability, MoveIt Servo has some convenient feature Singularity checking and collision checking are safety features that scale down the velocities when approaching singularities or collisions (self collision or collision with other objects). The collision checking and smoothing are optional features that can be disabled using the ``check_collisions`` parameter and the ``use_smoothing`` parameters respectively. -The inverse kinematics is handled through either the inverse Jacobain or the robot's IK solver if one was provided. +The inverse kinematics is handled through either the inverse Jacobian or the robot's IK solver if one was provided. + +If your robot has subgroups (for example, a dual-arm setup), you can switch which subgroup +Servo actuates at runtime using the ``active_subgroup`` parameter. Inverse Kinematics in Servo @@ -74,7 +77,7 @@ Once your :code:`kinematics.yaml` file has been populated, include it with the R ) -The above excerpt is taken from :moveit_codedir:`servo_example.launch.py ` in MoveIt. +The above excerpt is taken from :moveit_codedir:`demo_ros_api.launch.py ` in MoveIt. In the above example, the :code:`kinematics.yaml` file is taken from the :moveit_resources_codedir:`moveit_resources ` repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`. The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics..`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`. @@ -117,6 +120,7 @@ The first step is to create a ``Servo`` instance. // Import the Servo headers. #include #include + #include // The node to be used by Servo. rclcpp::Node::SharedPtr node = std::make_shared("servo_tutorial"); @@ -134,8 +138,20 @@ The first step is to create a ``Servo`` instance. // Create a Servo instance. Servo servo = Servo(node, servo_param_listener, planning_scene_monitor); + // Create a publisher for sending trajectory commands to the robot controller. + auto trajectory_publisher = node->create_publisher( + servo_params.command_out_topic, rclcpp::SystemDefaultsQoS()); + + // Get the current robot state. This is needed by getNextJointState(). + auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = + robot_state->getJointModelGroup(servo_params.move_group_name); + +Once you have a ``Servo`` instance and a ``robot_state``, you can send commands. +Each command type is shown below. Using the JointJogCommand +""""""""""""""""""""""""" .. code-block:: c++ @@ -143,34 +159,35 @@ Using the JointJogCommand // Create the command. JointJogCommand command; - command.joint_names = {"panda_link7"}; - command.velocities = {0.1}; + command.names = {"panda_joint7"}; + command.velocities = {0.4}; // Set JointJogCommand as the input type. servo.setCommandType(CommandType::JOINT_JOG); // Get the joint states required to follow the command. // This is generally run in a loop. - KinematicState next_joint_state = servo.getNextJointState(command); + KinematicState next_joint_state = servo.getNextJointState(robot_state, command); Using the TwistCommand +"""""""""""""""""""""" .. code-block:: c++ using namespace moveit_servo; - // Create the command. - TwistCommand command{"panda_link0", {0.1, 0.0, 0.0, 0.0, 0.0, 0.0}; + // Create the command: frame_id and {linear_x, linear_y, linear_z, angular_x, angular_y, angular_z}. + TwistCommand command{"panda_link0", {0.0, 0.0, 0.05, 0.0, 0.0, 0.4}}; // Set the command type. servo.setCommandType(CommandType::TWIST); // Get the joint states required to follow the command. // This is generally run in a loop. - KinematicState next_joint_state = servo.getNextJointState(command); - + KinematicState next_joint_state = servo.getNextJointState(robot_state, command); Using the PoseCommand +""""""""""""""""""""" .. code-block:: c++ @@ -185,17 +202,52 @@ Using the PoseCommand // Get the joint states required to follow the command. // This is generally run in a loop. - KinematicState next_joint_state = servo.getNextJointState(command); + KinematicState next_joint_state = servo.getNextJointState(robot_state, command); + +Publishing the Output +""""""""""""""""""""" + +The ``next_joint_state`` result must be published to the robot controller. Servo provides +utility functions for composing trajectory messages and maintaining a sliding window of +recent states: + +.. code-block:: c++ + + using namespace moveit_servo; + + std::deque joint_cmd_rolling_window; + + // In your control loop (using the command from one of the sections above): + KinematicState next_joint_state = servo.getNextJointState(robot_state, command); + StatusCode status = servo.getStatus(); -The ``next_joint_state`` result can then be used for further steps in the control pipeline. + if (status != StatusCode::INVALID) + { + updateSlidingWindow(next_joint_state, joint_cmd_rolling_window, + servo_params.max_expected_latency, node->now()); + if (const auto msg = composeTrajectoryMessage(servo_params, joint_cmd_rolling_window)) + { + trajectory_publisher->publish(msg.value()); + } + // Update the robot state for the next iteration. + if (!joint_cmd_rolling_window.empty()) + { + robot_state->setJointGroupPositions(joint_model_group, + joint_cmd_rolling_window.back().positions); + robot_state->setJointGroupVelocities(joint_model_group, + joint_cmd_rolling_window.back().velocities); + } + } The status of MoveIt Servo resulting from the last command can be obtained by: .. code-block:: c++ StatusCode status = servo.getStatus(); + std::string status_msg = servo.getStatusMessage(); -The user can use status for higher-level decision making. +Use the status for higher-level decision making — for example, to stop sending commands +when the arm reaches a singularity or collision. See :moveit_codedir:`moveit_servo/demos ` for complete examples of using the C++ interface. The demos can be launched using the launch files found in :moveit_codedir:`moveit_servo/launch `. @@ -215,7 +267,7 @@ To use MoveIt Servo through the ROS interface, it must be launched as a ``Node`` When using MoveIt Servo with the ROS interface the commands are ROS messages of the following types published to respective topics specified by the Servo parameters. 1. ``control_msgs::msg::JointJog`` on the topic specified by the ``joint_command_in_topic`` parameter. - 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. For now, the twist message must be in the planning frame of the robot. (This will be updated soon.) + 2. ``geometry_msgs::msg::TwistStamped`` on the topic specified by the ``cartesian_command_in_topic`` parameter. 3. ``geometry_msgs::msg::PoseStamped`` on the topic specified by the ``pose_command_in_topic`` parameter. Twist and Pose commands require that the ``header.frame_id`` is always specified. diff --git a/doc/how_to_guides/controller_teleoperation/controller_teleoperation.rst b/doc/how_to_guides/controller_teleoperation/controller_teleoperation.rst index 2845e42582..217c73974b 100644 --- a/doc/how_to_guides/controller_teleoperation/controller_teleoperation.rst +++ b/doc/how_to_guides/controller_teleoperation/controller_teleoperation.rst @@ -1,89 +1,189 @@ -How to Teleoperate a Robotic Arm with a Gamepad -=============================================== +How to Teleoperate a Robotic Arm +================================ -This guide will introduce you to using a gamepad to move the panda arm. +This guide shows you how to control the panda arm in real time using MoveIt Servo. +MoveIt Servo accepts velocity commands and handles the inverse kinematics, collision checking, +and singularity management for you. + +You can teleoperate the arm with the built-in keyboard interface or connect your own input +device (such as a gamepad) through the ROS API. Prerequisites ------------- -Make sure your workspace has the required packages installed and that you -have a gamepad supported by ROS2 joy. This can be tested by launching a -``joy`` node and then running ``ros2 topic echo /joy`` to ensure your -gamepad is detected by ``joy``. - -Requirements ------------- -- Ubuntu 22.04 -- ROS 2 Humble -- MoveIt 2 -- MoveIt 2 Tutorials -- `ROS2 joy `_ +Make sure you have completed the :doc:`Getting Started ` tutorial +and can successfully launch the MoveIt demo for the panda arm. + +For the gamepad section, you also need a controller supported by +`ROS 2 joy `_. Test it by running +``ros2 run joy joy_node`` and then ``ros2 topic echo /joy`` to confirm your +gamepad is detected. + +Keyboard Teleoperation +---------------------- + +MoveIt Servo ships with a keyboard teleoperation demo that works out of the box. + +1. Build the MoveIt 2 workspace. + + ``cd`` to the root of the workspace (``~/ws_moveit/`` if you followed Getting Started), + then run: + + .. code-block:: bash + + colcon build + source install/setup.bash + +2. Launch the servo demo. + + .. code-block:: bash + + ros2 launch moveit_servo demo_ros_api.launch.py + +3. In a second terminal, start the keyboard input node. + + .. code-block:: bash + + ros2 run moveit_servo servo_keyboard_input + +4. Use the following keys to move the arm: + + ================== ===================================================== + Key Action + ================== ===================================================== + Arrow keys Cartesian motion (linear X / Y) + ``.`` / ``;`` Cartesian motion (linear Z down / up) + ``1`` – ``7`` Jog individual joints (panda_joint1 through panda_joint7) + ``r`` Reverse joint jog direction + ``t`` Switch to Twist (Cartesian) command mode + ``j`` Switch to JointJog command mode + ``w`` Set reference frame to planning frame + ``e`` Set reference frame to end-effector frame + ``q`` Quit + ================== ===================================================== + +Gamepad Teleoperation +--------------------- + +The previous built-in ``JoyToServoPub`` node was removed during the MoveIt Servo refactoring. +Gamepad teleoperation now works through the same ROS topics that the keyboard demo uses — +you just need a small node that translates ``sensor_msgs/msg/Joy`` messages into the +command messages that Servo expects. + +.. image:: xboxcontroller.png + :width: 600px + +The image above shows a reference controller layout from the previous ``JoyToServoPub`` node. +Your actual button-to-axis mapping depends on your translator code (see the Python example below). +Run ``ros2 topic echo /joy`` to identify which axes and buttons correspond to your gamepad's controls. +The drawio source for the image is `here `__. + +How It Works +^^^^^^^^^^^^ + +MoveIt Servo's ``ServoNode`` subscribes to three command topics and exposes a service for +switching between them: + +- ``~/delta_twist_cmds`` — ``geometry_msgs/msg/TwistStamped`` for Cartesian velocity commands. +- ``~/delta_joint_cmds`` — ``control_msgs/msg/JointJog`` for direct joint velocity commands. +- ``~/pose_target_cmds`` — ``geometry_msgs/msg/PoseStamped`` for target pose commands. + +The active command type is selected via the ``~/switch_command_type`` service +(``moveit_msgs/srv/ServoCommandType``). Servo only processes one command type at a time. + +The ``~/`` prefix means these names are relative to the node's name. In ``demo_ros_api.launch.py`` +the node is named ``servo_node``, so the topics resolve to ``/servo_node/delta_twist_cmds``, etc. +If you launch under a different name or namespace, adjust the topic names accordingly. + +Any input device that can publish to these topics will work. For a gamepad, the +typical approach is: + +1. Run the ``joy`` node to publish raw gamepad state on ``/joy``. +2. Write a translator node that subscribes to ``/joy`` and publishes + ``TwistStamped`` or ``JointJog`` messages to the Servo topics. +3. Use the ``switch_command_type`` service to select which command type Servo processes. Steps ------ +^^^^^ -1. Build the MoveIt 2 workspace +1. Launch the servo demo: - First, ``cd`` to the root directory of the moveit2 workspace. (if you followed the :doc:`Getting Started ` tutorial, this will be ``~/ws_moveit/``). + .. code-block:: bash - Then, run ``colcon build``. + ros2 launch moveit_servo demo_ros_api.launch.py -2. Plug in your gamepad. -3. Source the install script and run the ``moveit_servo`` example file. +2. In a second terminal, launch the ``joy`` node: - Run ``source install/setup.bash``, then ``ros2 launch moveit_servo servo_example.launch.py`` + .. code-block:: bash -4. Move the arm around, using the below image as a guide. + ros2 run joy joy_node - .. image:: xboxcontroller.png - :width: 600px +3. Switch Servo to Twist command mode (it may not be the default): -The drawio document can be seen `here `__. + .. code-block:: bash -Explanation ------------ + ros2 service call /servo_node/switch_command_type \ + moveit_msgs/srv/ServoCommandType "{command_type: 1}" -This section explains the launch file and the node that translates gamepad inputs to motion commands. +4. In a third terminal, run your translator node. A minimal Python example: -Launch File -^^^^^^^^^^^ + .. code-block:: python -The file that launches this example is -``ws_moveit2/src/moveit2/moveit_ros/moveit_servo/launch/servo_example.launch.py`` + import rclpy + from rclpy.node import Node + from sensor_msgs.msg import Joy + from geometry_msgs.msg import TwistStamped -This launch file launches everything needed for the panda arm planning, and also launches the ``joy`` node and the ``JoyToServoPub`` node (which is explained below). + class JoyToServo(Node): + # Scaling factors: axes are dimensionless [-1, 1], these convert to m/s and rad/s. + # Adjust to suit your robot — start low and increase gradually. + LINEAR_SCALE = 0.2 # m/s at full stick deflection + ANGULAR_SCALE = 0.5 # rad/s at full stick deflection -Of primary interest is the section of code that launches the joy and ``JoyToServoPub`` nodes. -They are both created as ``ComposableNode``\s. More information about ``ComposableNode``\s can be found `here `__ and `here `__. + def __init__(self): + super().__init__("joy_to_servo") + self.pub = self.create_publisher( + TwistStamped, "/servo_node/delta_twist_cmds", 10 + ) + self.create_subscription(Joy, "/joy", self.joy_cb, 10) -.. code-block:: python + def joy_cb(self, msg): + twist = TwistStamped() + twist.header.stamp = self.get_clock().now().to_msg() + twist.header.frame_id = "panda_link0" # Change to your robot's planning frame + # Map left stick to linear X/Y, right stick to linear Z and angular Z + twist.twist.linear.x = msg.axes[1] * self.LINEAR_SCALE + twist.twist.linear.y = msg.axes[0] * self.LINEAR_SCALE + twist.twist.linear.z = msg.axes[4] * self.LINEAR_SCALE + twist.twist.angular.z = msg.axes[3] * self.ANGULAR_SCALE + self.pub.publish(twist) - ComposableNode( - package="moveit_servo", - plugin="moveit_servo::JoyToServoPub", - name="controller_to_servo_node", - ), - ComposableNode( - package="joy", - plugin="joy::Joy", - name="joy_node", - ) + rclpy.init() + node = JoyToServo() + try: + rclpy.spin(node) + finally: + node.destroy_node() + rclpy.shutdown() -JoyToServoPub -^^^^^^^^^^^^^ + Adjust the axis indices and scaling to match your gamepad. Run + ``ros2 topic echo /joy`` to see which axes correspond to which sticks. -The node that translates gamepad inputs to motion commands is -``ws_moveit2/src/moveit2/moveit_ros/moveit_servo/src/teleop_demo/joystick_servo_example.cpp`` +Explanation +----------- -This node subscribes to the joy node (which publishes messages giving the state of the gamepad). It publishes ``TwistStamped`` messages, ``JointJog`` messages, and ``PlanningScene`` messages. +The keyboard and gamepad approaches both work the same way under the hood. +``ServoNode`` processes incoming velocity commands regardless of where they +come from. It handles: -The ``PlanningScene`` message is only published once, when the JoyToServoPub is first constructed. It simply adds some obstacles into the planning scene. +- **Inverse kinematics** — converting Cartesian velocity commands into joint motions. +- **Collision checking** — scaling down velocities when the arm is near obstacles or itself. +- **Singularity management** — decelerating when the arm approaches a kinematic singularity. +- **Joint limits** — enforcing position and velocity bounds. -The difference between the ``JointJog`` and ``TwistStamped`` messages is -that the inverse kinematic solver moves the joints to achieve the end -effector motions defined by the ``TwistStamped`` messages, while the -``JointJog`` messages directly move individual joints. +``JointJog`` messages directly specify joint velocities, while ``TwistStamped`` +messages specify the desired end-effector velocity and let Servo solve for the +joint motions. If you need to move individual joints precisely, use ``JointJog``. +For intuitive Cartesian control, use ``TwistStamped``. -The ``joyCB`` function is called when a message is published to the ``joy`` -topic, and translates the button presses from the gamepad into commands -for the arm. If both ``JointJog`` and ``TwistStamped`` messages would be -published by the inputs, only ``JointJog`` messages are published. +For a deeper look at MoveIt Servo's architecture, command types, and configuration, +see the :doc:`Realtime Servo ` tutorial.