MoveIt Notebook Tutorial

MoveIt Notebook Tutorial#

Welcome to this tutorial on using jupyter notebooks with MoveIt 2. A great benefit of being able to interact with MoveIt via a Python notebook is the ability to rapidly prototype code. We hope you find this interface intuitive and that you gain value from using MoveIt via Python notebooks.

In this tutorial we will cover the following:

  • The required imports to run the notebook

  • A motion planning example

  • A teleoperation example

If you have suggestions or feedback for this tutorial please post an issue on GitHub (ros-planning/moveit2_tutorials) and tag @peterdavidfagan.

Imports#

Note: to launch this notebook and the nodes it depends on you must first specify a launch file. Details are provided earlier in this tutorial.

import os
import sys
import yaml
import rclpy
import numpy as np

# message libraries
from geometry_msgs.msg import PoseStamped, Pose

# moveit_py
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState

# config file libraries
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python.packages import get_package_share_directory


# we need to specify our moveit_py config at the top of each notebook we use. 
# this is since we will start spinning a moveit_py node within this notebook.

moveit_config = (
        MoveItConfigsBuilder(robot_name="panda", package_name="moveit_resources_panda_moveit_config")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .moveit_cpp(
            file_path=os.path.join(
                get_package_share_directory("moveit2_tutorials"),
                "config",
                "jupyter_notebook_prototyping.yaml"
        )
    )
    .to_moveit_configs()
    ).to_dict()

Setup#

# initialise rclpy (only for logging purposes)
rclpy.init()

# instantiate moveit_py instance and a planning component for the panda_arm
panda = MoveItPy(node_name="moveit_py", config_dict=moveit_config)
panda_arm = panda.get_planning_component("panda_arm")

Motion Planning Example#

def plan_and_execute(
    robot,
    planning_component,
    single_plan_parameters=None,
    multi_plan_parameters=None,
):
    """A helper function to plan and execute a motion."""
    # plan to goal
    if multi_plan_parameters is not None:
        plan_result = planning_component.plan(
            multi_plan_parameters=multi_plan_parameters
        )
    elif single_plan_parameters is not None:
        plan_result = planning_component.plan(
            single_plan_parameters=single_plan_parameters
        )
    else:
        plan_result = planning_component.plan()

    # execute the plan
    if plan_result:
        robot_trajectory = plan_result.trajectory
        robot.execute(robot_trajectory, controllers=[])
    else:
        print("Planning failed")

# set plan start state using predefined state
panda_arm.set_start_state("ready")

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name = "extended")

# plan to goal
plan_and_execute(panda, panda_arm)

We can perform motion planning interactively (see the motion planning tutorial for further details of the motion planning API). Suppose we are developing our code and we make a mistake such as follows:

# set plan start state using predefined state
panda_arm.set_start_state("ready") # This conflicts with the current robot configuration and will cause an error

# set goal using a pose message this time
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg = pose_goal, pose_link = "panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm)

Since we are using a notebook this mistake is easy to rectify without having to fix the bug and recompile files. Simply edit the above notebook to match the below and rerun the cell.

# set plan start state using predefined state
panda_arm.set_start_state_to_current_state()

# set goal using a pose message this time
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg = pose_goal, pose_link = "panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm)

Teleoperation Example#

One may also want to perform live teleoperation of their robot. With the Python API it is possible to interactively start/stop teleoperation without shutting down and subsequently relaunching all processes. In this example, we are going to show how this is possible with notebooks through a motivating example of teleoperating the robot, performing motion planning and teleoperating the robot again.

For this section you will need a device that support teleoperation with moveit_py, in this case we leverage the PS4 dualshock controller.

To start teleoperating the robot we instantiate the PS4 dualshock controller as a teleop device.

from moveit.servo_client.devices.ps4_dualshock import PS4DualShockTeleop

# instantiate the teleoperating device
ps4 = PS4DualShockTeleop(ee_frame_name="panda_link8")

# start teleloperating the robot
ps4.start_teleop()

If we want to perform motion planning to bring the robot back to its default configuration, we simply stop teleoperating the robot and leverage the existing motion planning API as demonstrated below:

# stop teleoperating the robot
ps4.stop_teleop()

# plan and execute
# set plan start state using predefined state
panda_arm.set_start_state_to_current_state()

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name = "ready")

# plan to goal
plan_and_execute(panda, panda_arm)

Ok great now we are back at our default configuration, lets start teleoperating the robot again.

ps4.start_teleop()
Contents