Skip to main content

3.0.0

· 6 min read

Changes in This Release

Major version increase.

  • Updated MoveIt version to include planning pipeline refactor and per-joint limit margins in MoveIt Servo.
  • Added new Parameter Manager node to store and retrieve joint state and pose parameters set from the UI for manual control applications.
  • New Behaviors:
    • ExtractGraspableObjectPose - Given a Graspable Object, returns its pose to an output port.
      • RetrieveJointStateParameter / RetrievePoseParameter - Retrieve joint states and poses that are set from the UI for manual control applications.
      • PublishStaticFrame - Publishes a static transform to the tf2 buffer.
      • PublishString - Publishes a string to a specified ROS topic.
  • Behavior Changes:
    • AveragePoseStamped - No longer an experimental Behavior. Can be used multiple times in a single Objective, improved orientation averaging, and configurable for finite-time vs. continuously running operation.
      • Pick AprilTag Labeled Object tutorial updated with new pose averaging capabilities.
      • GetJointStateFromUser removed in favor of RetrieveJointStateParameter.
      • SetupMTCMoveAlongFrameAxis - Get all input parameters from individual ports instead of a YAML file.
      • YAML read/write Behaviors:
        • New LoadFromYaml, LoadMultipleFromYaml, and SaveToYaml base classes for authoring custom Behaviors.
          • All Behaviors now support absolute paths and relative paths to the configuration folder (${HOME}/.config/moveit_pro/<config_package_name>/objectives) folder.
          • SaveJointTrajectoryToYaml / SavePoseToYaml - Refactored to reuse generic implementation.
          • WritePoseToYaml removed in favor of SavePoseToYaml.
      • Behavior interface for developers:
        • Replaced all uses of fp::Result with tl::expected.
          • ActionClientBehaviorBase base class now provides a processFeedback function to insert user code for handling action feedback.
  • moveit_studio_agent_msgs package moved to the public moveit_studio_sdk repository.
  • Added integration testing reference package to moveit_studio_ws repository, including CI support.
  • UI Updates:
    • New debug overlay when planning fails with PlanMTCTask Behavior.
      • Added debugging resume button to be used along with BreakpointSubscriber Behaviors.
      • Allow streaming images from arbitrary ROS topics.
      • Show pre- and post-conditions in Behavior Tree editor.
      • Trajectory previewing: Display planning algorithms, replaced stepped with simplified approval view.
      • Removed Logs tab.

Migration Guide

MoveIt Configuration Changes

MoveIt Studio now includes the updated planning pipeline interface introduced in moveit2#2429 and moveit2#2457.

As such, any MoveIt configuration files in your MoveIt Studio configuration packages must be updated.

Previous:

planning_plugin: ompl_interface/OMPLPlanner
request_adapters: >-
default_planner_request_adapters/AddTimeOptimalParameterization
default_planner_request_adapters/ResolveConstraintFrames
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints
start_state_max_bounds_error: 0.1

Updated:

planning_plugins:
- ompl_interface/OMPLPlanner
# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
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

These changes mean that you must fully specify configuration files for any of the supported planners (OMPL, Pilz, and STOMP) you would like to use, as follows:

ompl_planning:
package: "picknik_ur_base_config"
path: "config/moveit/ompl_planning.yaml"
pilz_planning:
package: "picknik_ur_base_config"
path: "config/moveit/pilz_industrial_motion_planner_planning.yaml"
stomp_planning:
package: "picknik_ur_base_config"
path: "config/moveit/stomp_planning.yaml"

Additionally, MoveIt Servo configuration files now use per-joint limit margins. For example, for a 6-DOF arm you must make the following changes to your MoveIt Servo configuration file.

Previous:

joint_limit_margin: 0.1

Updated:

joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

Replace fp::Result with tl::expected

Replaced expected types in Behaviors from fp::Result (from the fp repo) with tl::expected (from the tl repo).

All Behaviors (including your custom Behaviors) that previously used fp::Result must have their syntax updated as follows.

Previous:

fp::Result<bool> MyBehavior::doWork()
{
// other code goes here...
return tl::make_unexpected(fp::Internal("Some failure reason"));
}

Updated:

tl::expected<bool, std::string> MyBehavior::doWork()
{
// other code goes here...
return tl::make_unexpected("Some failure reason");
}

SetupMTCMoveAlongFrameAxis Port Changes

This Behavior no longer uses a YAML file to contain its parameters, and instead relies on individual input ports containing each parameter.

Previous:

<Action ID="SetupMTCMoveAlongFrameAxis"
axis="z" max_distance="0.2" min_distance="0.05"
parameters="{parameters}" task="{push_along_axis_task}"
/>

Updated:

<Action ID="SetupMTCMoveAlongFrameAxis"
task="{push_along_axis_task}" hand_frame="manual_grasp_link"
axis_frame="world" axis_x="0.0" axis_y="0.0" axis_z="1.0"
max_distance="0.2" min_distance="0.05" planning_group_name="manipulator"
velocity_scale="0.05" acceleration_scale="0.05"
/>

Changes to Behaviors that Read from and Write to YAML

All simple Behaviors that interact with YAML files have been updated to inherit from LoadFromYaml, LoadMultipleFromYaml, and SaveToYaml base classes, respectively. You can use these classes yourself to register specialized Behaviors for new ROS message types with just a few lines of code.

#include <moveit_studio_behavior_interface/load_from_yaml.hpp>
#include <moveit_studio_behavior_interface/save_to_yaml.hpp>

void MyBehaviorsLoader::registerBehaviors(BT::BehaviorTreeFactory& factory,
const std::shared_ptr<BehaviorContext>& shared_resources)
{
registerBehavior<LoadFromYaml<my_msgs::msg::MyMessage>>(factory, "LoadMyMessageFromYaml", shared_resources);
registerBehavior<LoadMultipleFromYaml<my_msgs::msg::MyMessage>>(factory, "LoadMyMessageVectorFromYaml", shared_resources);
registerBehavior<SaveToYaml<my_msgs::msg::MyMessage>>(factory, "SaveMyMessageToYaml", shared_resources);
}

Changes to Getting Pose and Joint State Parameters from UI

In releases prior to 3.0.0, some Objectives used GetJointStateFromUser and GetPoseFromUser to connect button clicks from manual control in the UI to motion planning targets.

Starting with release 3.0.0, these have been replaced with a more formal Parameter Manager node that is launched with MoveIt Studio. This node can store and retrieve joint states and poses using ROS services, which will support more generic capabilities in the future.

As a result, Objectives that previously used these Behaviors can be updated to remove fallback ROS messages and instead wait for parameters to be retrieved with a configurable timeout. For an example, see the changes in Move to Pose:

Previous:

<Action ID="GetPoseFromUser"
parameter_name="move_to_pose.target_pose"
parameter_value="{target_pose}"
/>

Updated:

<!-- Block indefinitely -->
<Action ID="RetrievePoseParameter" timeout_sec="" pose="{target_pose}"/>

<!-- Do not wait at all -->
<Action ID="RetrievePoseParameter" timeout_sec="0" pose="{target_pose}"/>

<!-- Wait for some finite amount of time -->
<Action ID="RetrievePoseParameter" timeout_sec="2.5" pose="{target_pose}"/>

With these changes, the Objectives in our Universal Robots example workspace has been updated as follows:

  • Interpolate to Joint State and Move to Joint State - now uses RetrieveJointStateParameter.
  • Move to Known Pose - removed, the UI now extracts a joint state from the waypoint name and uses Move to Joint State directly.
  • Move to Pose - now uses RetrievePoseParameter.

Removed Logs tab in the UI

If you were previously using the Logs tab to view output from your nodes, you can still access this information in several ways.

  • Launch MoveIt Studio in verbose mode (./moveit_studio run -v) and view the output in your console.
  • View the log files which are mounted on your host to the (${HOME}/.ros/log_moveit_studio) folder.
    • You can use the command tail -f ~/.ros/log_moveit_studio/* on your host system to monitor files while MoveIt Studio is running.
    • You can also follow the logs using docker compose logs -f.