Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add a MoveIt C++ API demo #197

Merged
merged 2 commits into from
Jul 24, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions lbr_bringup/launch/move_group.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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,
Expand Down
4 changes: 2 additions & 2 deletions lbr_bringup/launch/real.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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")),
Expand Down
4 changes: 2 additions & 2 deletions lbr_bringup/launch/sim.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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")),
Expand Down
2 changes: 1 addition & 1 deletion lbr_bringup/launch_mixins/lbr_bringup.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions lbr_demos/doc/lbr_demos.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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
-----------
Expand Down
34 changes: 34 additions & 0 deletions lbr_demos/lbr_moveit_cpp/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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()
86 changes: 86 additions & 0 deletions lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.rst
Original file line number Diff line number Diff line change
@@ -0,0 +1,86 @@
lbr_moveit_cpp
==============
.. note::

Also refer to the official `MoveIt <https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html>`_: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 <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp>`_: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 <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/lbr_system_parameters.yaml>`_:octicon:`link-external`
#. Set the ``update_rate`` to ``100`` in `lbr_controllers.yaml <https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_ros2_control/config/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 <https://github.com/lbr-stack/lbr_fri_ros2_stack/tree/humble/lbr_demos/lbr_moveit_cpp>`_:octicon:`link-external`. The demo vastly follows the official `MoveIt <https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html>`_: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.
46 changes: 46 additions & 0 deletions lbr_demos/lbr_moveit_cpp/launch/hello_moveit.launch.py
Original file line number Diff line number Diff line change
@@ -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
22 changes: 22 additions & 0 deletions lbr_demos/lbr_moveit_cpp/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>lbr_moveit_cpp</name>
<version>2.0.0</version>
<description>Demo for using MoveIt C++ API.</description>
<maintainer email="[email protected]">mhubii</maintainer>
<license>Apache-2.0</license>

<buildtool_depend>ament_cmake</buildtool_depend>

<depend>geometry_msgs</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>rclcpp</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
39 changes: 39 additions & 0 deletions lbr_demos/lbr_moveit_cpp/src/hello_moveit.cpp
Original file line number Diff line number Diff line change
@@ -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;
}
Loading