-
Notifications
You must be signed in to change notification settings - Fork 43
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
9 changed files
with
196 additions
and
7 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,48 @@ | ||
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. | ||
|
||
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``. |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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; | ||
} |