UP | HOME

MoveWhat? Activity

Introduction

  1. MoveIt 2 is a massive package, with many features, but it is still under development and is under-documented.
  2. To make our robot move, without using C++, we will need to do some detective work
  3. This guide is designed as a tutorial to introduce you to MoveIt and prepare your group for Homework 3

Preliminaries

  1. Setup the franka workspace on your computer.

First Steps

  1. To start the franka robot ros2 launch franka_fer_moveit_config demo.launch.py
    • This command will start a Franka MoveIt setup, with the MoveIt GUI in rviz
    • Rather than connecting to the actual robot or a simulation, a mock controller is used that takes joint positions as inputs and returns fake joint states as sensor data.

Rviz2

  1. There are two new Rviz widgets (which show up in the left-pane) that have been added by the setup when you run these launch files
    • PlanningScene, which keeps track of obstacles in the environment
    • MotionPlanning, which is a gui interface to move the robot
  2. There will be an interactive marker displayed near the robot's end-effector that you can drag to set the target location for the motion plan.

Motion Plan

  1. You should see an orange overlay on the robot.
    • The orange robot represents the planning group, the collection of joints (and there associated links) that are currently being controlled
    • In the MotionPlanning GUI, under the Planning Tab, you can change the planning group to, for example, control the gripper instead of the arm
    • While you can plan a path for the gripper the execution is simulated by simply snapping it to the commanded position immediately.
      • On the real robot there are multiple control modes for the gripper, accessible through the gripper_action server.
      • Not all gripper control modes are position controlled and are therefore not accessible via MoveIt
  2. An interactive marker, which can be dragged to move the arm to various positions
    • This marker controls the goal state, which is the target location of the robot's end-effector
    • As you drag the robot around, some of the links may turn red, which indicates a (possibly self) collision or otherwise invalid state
    • Try it!
  3. There are also pre-set positions that the arm can be moved to
    • The gripper has no interactive marker and can only be moved to set positions via the GUI (though it can be moved to arbitrary positions in code).
  4. It is also possible to plan from a goal state that is not the current location of the robot by setting the Start State
  5. In the Planning Tab, press plan to generate a motion plan
    • You should see an animation of the arm moving from start to goal
    • Press Execute to run the plan, and watch the arm move (Try It!)
    • If you press Plan & Execute you will see the robot start to move, with the plan happening slightly ahead of robot motion (Try It!)
    • You can change the speed of the motion by selecting Velocity Scaling and Accel Scaling
      • On the real robot be very careful with these, the robot can move fast!
    • There are also many options that are available for motion planning that are accessible via the GUI

Joints

  1. The Joints tab allows you to plan to a specific joint configuration rather than an end-effector configuration
  2. Notice that if you move the marker at the end-effector, the joint states move as well indicating that inverse kinematics is happening here!
    • In other words, planning to an end-effector position means computing inverse kinematics to find the corresponding joint positions

Planning Scene

  1. Under the Scene Objects tab, you can manipulate obstacles in the environment
    1. Click the plus sign next to the word box to add a box into the scene
      • Move it to somewhere where the robot can potentially hit it
    2. Click "publish" to set the box as part of the planning scene
    3. It is also possible to attach objects to the end-effector (e.g., so that collision detection accounts for a grabbed object)
  2. With the scene set, drag the end-effector into the box
    • What happens? (The links that collide should turn red)
    • What happens if you click plan? (The plan will fail)
    • Check "collision-aware IK" and move the end-effector into the box and hold it
      • You should see the IK solver try to keep moving the joints to avoid the collision
        • If the end-effector is colliding with the box, no IK solution can succeed at avoiding it
        • But it is possible if another link is hitting the box (maybe another arm orienation does not have that same collision)
  3. Objects in the planning scene can be attached to the end-effector (e.g., to indicate that the robot has picked up the object).
    • This enables them to be checked for collision during planning

How It Works

  1. The Moveit Documentation provides an overview, particularly the Move Group diagram.
  2. The moveit_msgs documentation explains how to use the ROS API for the /move_group node.
  3. Take a look at the rqt_graph (be sure to refresh)
    • The key node for planning is the /move_group node
  4. Run ros2 node info /move_group to see it's ROS API
    • You can of course get info on these interface with the ros2 command line ros2 service/action/topic and ros2 interface
    • The key actions here, however, are
      • /move_action, which is used to plan the path
      • /execute_trajectory, which does exactly what it sounds like
    • The key service is /compute_ik which computes the inverse kinematics
  5. If you use ros2 interface on the types (which you should do) you can learn how these actions work a bit more

The Plan

  1. Let's use our working interface to the move-group node (that is, Rviz) to help us better understand how the ROS API for the /move_group node works
    • Make sure the MotionPlanning panel in Rviz is selected and activated
  2. To be as thorough as possible we would want to capture all the data that rviz sends to the move_group node
    • That is, all move_group service servers, action servers, and subscriptions that rviz2 connects to
    • We can find these by comparing ros2 node info /move_group to ros2 node info /rviz2
  3. Capturing actions and services is not as straightforward as subscribing because there can only be one service or action server with a given name.
    • Instead the approach is to write a new node that has an action server that prints out the requests
    • The node could also forward traffic to the actual underlying action with an action client.

Basic Action Inspection

  1. Make a new package to inspect actions
  2. Create a node and an action Server for /viz/move_action:
    • The server, for now, can simply print out the request.

  3. Write a launchfile that

    • Launches your node
    • Launches rviz
    • Remaps the /move_action to /viz/move_action
    • Here is an example. Note that the MoveIt rviz gui requires that rviz have some parameters to be setup for moveit
    from moveit_configs_utils import MoveItConfigsBuilder
from launch import LaunchDescription
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution

def generate_launch_description():
    moveit_config = MoveItConfigsBuilder("fer", package_name="franka_fer_moveit_config").to_moveit_configs()

    return LaunchDescription([
        Node(package="rviz2",
            executable="rviz2",
            output="log",
            arguments=["-d", PathJoinSubstitution([FindPackageShare("franka_fer_moveit_config"), "config", "moveit.rviz"])],
            parameters = [ moveit_config.planning_pipelines, moveit_config.robot_description_kinematics],
            # Remapping actions not implemented, must remap individual topics/services
            # https://github.com/ros2/ros2/issues/1312 Accessed on 10/30/2024
            remappings = [("/move_action/_action/feedback", "/viz/move_action/_action/feedback"),
                        ("/move_action/_action/status", "/viz/move_action/_action/status"),
                        ("/move_action/_action/cancel_goal", "/viz/move_action/_action/cancel_goal"),
                        ("/move_action/_action/get_result", "/viz/move_action/_action/get_result"),
                        ("/move_action/_action/send_goal", "/viz/move_action/_action/send_goal")
                        ]
            ),
        Node(package="inspector",
            executable="inspector"
            )
        ])
  4. If you want more visibility, you can have your action forward the request to the actual /move_action, and forward the responses back. This can get complicated for actions because there are several methods of feedback, but it is relatively straightforward for services.
  5. You can now run demo.launch.py for the franka, kill rviz and launch your own rviz
    • Now planning requests from rviz can be printed.
  6. Other actions to try are /execute_trajectory
  7. The same can be done with services:
    • Some services to try are /compute_ik.
  8. The planning scene is published on a topic, so subscribing and printing out that topic can help
  9. By doing these exercises you will learn how the move_it rviz plugin uses the move_group
    • Try to discover what sequence of events happens to plan a motion and then execute it
    • Try to discover the difference between Plan, Execute, and Plan & Execute
    • Try to discover what features of the planning message are used and which are not
  10. The goal of this exploration is to be able to call an action manually that causes a path to be planned and executed

Documentation

  • It is not necessary to fully reverse engineer the action service. Though it can be helpful because rviz provides a working example of using the ROS API.
  • The API documentation for moveit_msgs (available on https://index.ros.org) is very helpful for understanding the capabilities of the /move_action
  • It is also possible to read the source code for moveit though it can be quite complicated.
Answers
  1. Here is a repository (https://github.com/m-elwin/me495_inspector) that intercepts and forwards actions from rviz to the move_group node.
    • It is a good exercise to attempt this task yourself before looking at the answer.

Author: Matthew Elwin.