diff --git a/lbr_bringup/launch/move_group.launch.py b/lbr_bringup/launch/move_group.launch.py index b39ad7c3..29f68bb3 100644 --- a/lbr_bringup/launch/move_group.launch.py +++ b/lbr_bringup/launch/move_group.launch.py @@ -21,7 +21,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: robot_name=model, package_name=f"{model}_moveit_config", ) - movegroup_params = LBRMoveGroupMixin.params_move_group() + move_group_params = LBRMoveGroupMixin.params_move_group() # MoveGroup: # - requires world frame @@ -45,7 +45,7 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: LBRMoveGroupMixin.node_move_group( parameters=[ moveit_configs_builder.to_dict(), - movegroup_params, + move_group_params, {"use_sim_time": LaunchConfiguration("sim")}, ], namespace=robot_name, diff --git a/lbr_bringup/launch/real.launch.py b/lbr_bringup/launch/real.launch.py index 6e7ac7f1..bc7f0c04 100644 --- a/lbr_bringup/launch/real.launch.py +++ b/lbr_bringup/launch/real.launch.py @@ -88,13 +88,13 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: robot_name=model, package_name=f"{model}_moveit_config", ) - movegroup_params = LBRMoveGroupMixin.params_move_group() + move_group_params = LBRMoveGroupMixin.params_move_group() ld.add_action( LBRMoveGroupMixin.node_move_group( parameters=[ moveit_configs_builder.to_dict(), - movegroup_params, + move_group_params, {"use_sim_time": False}, ], condition=IfCondition(LaunchConfiguration("moveit")), diff --git a/lbr_bringup/launch/sim.launch.py b/lbr_bringup/launch/sim.launch.py index 9342396a..edac639d 100644 --- a/lbr_bringup/launch/sim.launch.py +++ b/lbr_bringup/launch/sim.launch.py @@ -70,13 +70,13 @@ def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: robot_name=model, package_name=f"{model}_moveit_config", ) - movegroup_params = LBRMoveGroupMixin.params_move_group() + move_group_params = LBRMoveGroupMixin.params_move_group() ld.add_action( LBRMoveGroupMixin.node_move_group( parameters=[ moveit_configs_builder.to_dict(), - movegroup_params, + move_group_params, {"use_sim_time": True}, ], condition=IfCondition(LaunchConfiguration("moveit")), diff --git a/lbr_bringup/launch_mixins/lbr_bringup.py b/lbr_bringup/launch_mixins/lbr_bringup.py index afe38589..ef0b4682 100644 --- a/lbr_bringup/launch_mixins/lbr_bringup.py +++ b/lbr_bringup/launch_mixins/lbr_bringup.py @@ -67,7 +67,7 @@ def moveit_configs_builder( f"urdf/{robot_name}/{robot_name}.xacro", ), ) - .planning_pipelines(default_planning_pipeline="ompl", pipelines=["ompl"]) + .planning_pipelines(default_planning_pipeline="ompl") ) @staticmethod diff --git a/lbr_demos/doc/lbr_demos.rst b/lbr_demos/doc/lbr_demos.rst index 329fd1f0..7bcbf9e9 100644 --- a/lbr_demos/doc/lbr_demos.rst +++ b/lbr_demos/doc/lbr_demos.rst @@ -30,6 +30,7 @@ MoveIt :titlesonly: lbr_moveit2_py <../lbr_moveit_py/doc/lbr_moveit_py.rst> + lbr_moveit2_cpp <../lbr_moveit_cpp/doc/lbr_moveit_cpp.rst> Integration ----------- diff --git a/lbr_demos/lbr_moveit_cpp/CMakeLists.txt b/lbr_demos/lbr_moveit_cpp/CMakeLists.txt new file mode 100644 index 00000000..624f7fa9 --- /dev/null +++ b/lbr_demos/lbr_moveit_cpp/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.22) +project(lbr_moveit_cpp) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(moveit_ros_planning_interface REQUIRED) +find_package(rclcpp REQUIRED) + +add_executable(hello_moveit src/hello_moveit.cpp) + +ament_target_dependencies(hello_moveit + geometry_msgs + moveit_ros_planning_interface + rclcpp +) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +install(TARGETS hello_moveit + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst new file mode 100644 index 00000000..bf0fc249 --- /dev/null +++ b/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst @@ -0,0 +1,86 @@ +lbr_moveit_cpp +============== +.. note:: + + Also refer to the official `MoveIt `_:octicon:`link-external` documentation. + +.. contents:: Table of Contents + :depth: 2 + :local: + :backlinks: none + +MoveIt via C++ API +------------------ +Simulation +~~~~~~~~~~ +#. Run the robot driver: + + .. code-block:: bash + + ros2 launch lbr_bringup bringup.launch.py \ + moveit:=true \ + sim:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +#. Run the `hello_moveit `_:octicon:`link-external` node: + + .. code-block:: bash + + ros2 launch lbr_moveit_cpp hello_moveit.launch.py \ + sim:=true \ + model:=iiwa7 # [iiwa7, iiwa14, med7, med14] + +Hardware +~~~~~~~~ +#. Client side configurations: + + #. Configure the ``client_command_mode`` to ``position`` in `lbr_system_parameters.yaml `_:octicon:`link-external` + #. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml `_:octicon:`link-external` + +#. Remote side configurations: + + #. .. dropdown:: Launch the ``LBRServer`` application on the ``KUKA smartPAD`` + + .. thumbnail:: ../../doc/img/applications_lbr_server.png + + #. Select + + - ``FRI send period``: ``10 ms`` + - ``IP address``: ``your configuration`` + - ``FRI control mode``: ``POSITION_CONTROL`` or ``JOINT_IMPEDANCE_CONTROL`` + - ``FRI client command mode``: ``POSITION`` + +#. Proceed with steps 1 and 2 from `Simulation`_ but with ``sim:=false``. + +Examining the Code +~~~~~~~~~~~~~~~~~~ +The source code for this demo is available on `GitHub `_:octicon:`link-external`. The demo vastly follows the official `MoveIt `_:octicon:`link-external` demo. + +Differently, this repository puts the ``MoveGroup`` under a namespace. The ``MoveGroup`` is thus created as follows: + +.. code-block:: cpp + + // Create MoveGroupInterface (lives inside robot_name namespace) + auto move_group_interface = moveit::planning_interface::MoveGroupInterface( + node_ptr, moveit::planning_interface::MoveGroupInterface::Options("arm", "robot_description", + robot_name)); + +The ``MoveGroup`` configurations are parsed conveniently through a mixin: + +.. code-block:: python + + from launch_mixins.lbr_bringup import LBRMoveGroupMixin + + ... + + model = LaunchConfiguration("model").perform(context) + + # generate moveit configs + moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( + robot_name=model, + package_name=f"{model}_moveit_config", + ) + +.. note:: + + The MoveIt configurations might vary depending the user's configurations. diff --git a/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py new file mode 100644 index 00000000..685b7014 --- /dev/null +++ b/lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py @@ -0,0 +1,46 @@ +from typing import List + +from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity +from launch.actions import OpaqueFunction +from launch.substitutions import LaunchConfiguration +from launch_mixins.lbr_bringup import LBRMoveGroupMixin +from launch_mixins.lbr_description import LBRDescriptionMixin +from launch_ros.actions import Node + + +def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]: + ld = LaunchDescription() + + model = LaunchConfiguration("model").perform(context) + + # generate moveit configs + moveit_configs = LBRMoveGroupMixin.moveit_configs_builder( + robot_name=model, + package_name=f"{model}_moveit_config", + ) + + # launch demo node + ld.add_action( + Node( + package="lbr_moveit_cpp", + executable="hello_moveit", + parameters=[ + moveit_configs.to_dict(), + {"use_sim_time": LaunchConfiguration("sim")}, + LBRDescriptionMixin.param_robot_name(), + ], + ) + ) + return ld.entities + + +def generate_launch_description() -> LaunchDescription: + ld = LaunchDescription() + + ld.add_action(LBRDescriptionMixin.arg_model()) + ld.add_action(LBRDescriptionMixin.arg_robot_name()) + ld.add_action(LBRDescriptionMixin.arg_sim()) + + ld.add_action(OpaqueFunction(function=launch_setup)) + + return ld diff --git a/lbr_demos/lbr_moveit_cpp/package.xml b/lbr_demos/lbr_moveit_cpp/package.xml new file mode 100644 index 00000000..8d5f2804 --- /dev/null +++ b/lbr_demos/lbr_moveit_cpp/package.xml @@ -0,0 +1,22 @@ + + + + lbr_moveit_cpp + 2.0.0 + Demo for using MoveIt C++ API. + mhubii + Apache-2.0 + + ament_cmake + + geometry_msgs + moveit_ros_planning_interface + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + \ No newline at end of file diff --git a/lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp b/lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp new file mode 100644 index 00000000..d77a2341 --- /dev/null +++ b/lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp @@ -0,0 +1,39 @@ +#include "geometry_msgs/msg/pose.hpp" +#include "moveit/move_group_interface/move_group_interface.h" +#include "rclcpp/rclcpp.hpp" + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + // Configure node + auto node_ptr = rclcpp::Node::make_shared("hello_moveit"); + node_ptr->declare_parameter("robot_name", "lbr"); + auto robot_name = node_ptr->get_parameter("robot_name").as_string(); + + // Create MoveGroupInterface (lives inside robot_name namespace) + auto move_group_interface = moveit::planning_interface::MoveGroupInterface( + node_ptr, moveit::planning_interface::MoveGroupInterface::Options("arm", "robot_description", + robot_name)); + + // Set a target pose + geometry_msgs::msg::Pose target_pose; + target_pose.orientation.w = 1.0; + target_pose.position.x = -0.4; + target_pose.position.y = 0.0; + target_pose.position.z = 0.9; + move_group_interface.setPoseTarget(target_pose); + + // Create a plan to that target pose + moveit::planning_interface::MoveGroupInterface::Plan plan; + auto error_code = move_group_interface.plan(plan); + + if (error_code == moveit::core::MoveItErrorCode::SUCCESS) { + // Execute the plan + move_group_interface.execute(plan); + } else { + RCLCPP_ERROR(node_ptr->get_logger(), "Failed to plan to target pose"); + } + + rclcpp::shutdown(); + return 0; +}