Use trac ik 7.0

.gitattributes

@@ -0,0 +1 @@
+*.onnx filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,4 @@
+build/
+install/
+log/
+.vscode/

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "src/picknik_accessories"]
+	path = src/picknik_accessories
+	url = https://github.com/PickNikRobotics/picknik_accessories.git
+[submodule "src/external_dependencies/trac_ik"]
+	path = src/external_dependencies/trac_ik
+	url = [email protected]:traclabs/trac_ik.git

.pre-commit-config.yaml

@@ -11,6 +11,7 @@
 #     pre-commit autoupdate
 #
 # See https://github.com/pre-commit/pre-commit
+exclude: ^src/external_dependencies/
 repos:
   # Standard hooks
   - repo: https://github.com/pre-commit/pre-commit-hooks

Dockerfile

@@ -10,13 +10,14 @@ ARG MOVEIT_STUDIO_BASE_IMAGE
 ARG USERNAME=studio-user
 ARG USER_UID=1000
 ARG USER_GID=1000
+ARG BASE=base
 
 ##################################################
 # Starting from the specified MoveIt Pro release #
 ##################################################
 # The image tag is specified in the argument itself.
 # hadolint ignore=DL3006
-FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base
+FROM ${MOVEIT_STUDIO_BASE_IMAGE} AS base-base
 
 # Create a non-root user
 ARG USERNAME
@@ -49,18 +50,20 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
       /home/${USERNAME}/.ros && \
     chown -R $USER_UID:$USER_GID /home/${USERNAME} /opt/overlay_ws/
 
-# IMPORTANT: Optionally install Nvidia drivers for improved simulator performance with Nvidia GPUs.
-# To do this you must
-# 1. Uncomment the ENV and RUN entries below
-# 2. Replace the 'nvidia-driver-555' apt package with the Nvidia driver version on your host, e.g. nvidia-driver-535, nvidia-driver-555. Use nvidia-smi on your host to determine the driver version.
-# After rebuilding via `moveit_pro build` verify the drivers are active in your container by running `nvidia_smi` inside of `moveit_pro shell`.
-# ENV DEBIAN_FRONTEND=noninteractive
-# RUN apt update && apt install -y software-properties-common
-# RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
-#     --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
-#    add-apt-repository ppa:graphics-drivers/ppa && \
-#    apt update && apt upgrade -y && apt install -y nvidia-driver-555
+FROM base-base AS base-nvidia
+ARG NVIDIA_DRIVER_PACKAGE
+ENV DEBIAN_FRONTEND=noninteractive
+# hadolint ignore=DL3008
+RUN apt-get update && apt-get install -y --no-install-recommends software-properties-common
+# hadolint ignore=DL3008
+RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \
+    --mount=type=cache,target=/var/lib/apt,sharing=locked \ 
+   add-apt-repository ppa:graphics-drivers/ppa && \
+   apt-get update && apt-get upgrade -y && apt-get install -y --no-install-recommends "${NVIDIA_DRIVER_PACKAGE}"
 
+ARG BASE
+# hadolint ignore=DL3006
+FROM base-${BASE} AS base
 # Install additional dependencies
 # You can also add any necessary apt-get install, pip install, etc. commands at this point.
 # NOTE: The /opt/overlay_ws folder contains MoveIt Pro binary packages and the source file.

README.md

@@ -1,7 +1,64 @@
-# MoveIt Pro Workspace
+# MoveIt Pro Example Workspace
 
-This is a minimal user workspace that can be used to build customized MoveIt Pro workspaces.
+This is fork of the [MoveIt Pro Empty Workspace](https://github.com/PickNikRobotics/moveit_pro_empty_ws).
+This workspace contains reference materials for using MoveIt Pro, including:
+- [Example base UR5e configuration](src/moveit_pro_ur_configs/picknik_ur_base_config)
+- [A physics based simulation environment with a robot on a linear rail](src/lab_sim)
+- [Mobile manipulation configuration](src/moveit_pro_mobile_manipulation/picknik_ur_mobile_config)
+- [Example behaviors](src/example_behaviors)
 
-You may fork this repository and add the MoveIt Pro configurations and ROS 2 packages of your choosing to the `src` folder.
+Since the [picknik_accessories](https://github.com/PickNikRobotics/picknik_accessories) package uses git LFS, [it cannot be added as a subtree](https://github.com/git-lfs/git-lfs/issues/854).
+Please ensure you have the submodule up to date using:
+```bash
+git submodule update --recursive --init
+```
 
-For more information, refer to the [MoveIt Pro Documentation](https://docs.picknik.ai/).
+## Working with Git Subtrees
+
+This repository was created through the combination of multiple repositories using git subtree.
+If you have no interest in manually pulling or pushing upstream changes, you can ignore the following section and treat this repository as a single repository.
+
+### Repository Structure
+
+
+The structure of this repository is as follows:
+
+<pre>
+.
+├── <a href="README.md">README.md</a>
+└── src
+    ├── <a href="https://github.com/PickNikRobotics/example_behaviors">example_behaviors</a>
+    ├── <a href="https://github.com/PickNikRobotics/lab_sim">lab_sim</a>
+    ├── <a href="https://github.com/PickNikRobotics/moveit_pro_ur_configs">moveit_pro_ur_configs</a>
+    │   ├── picknik_ur_base_config
+    │   ├── picknik_ur_mock_hw_config
+    │   ├── picknik_ur_multi_arm_config
+    │   ├── picknik_ur_sim_config
+    │   └── picknik_ur_site_config
+    ├── <a href="https://github.com/PickNikRobotics/moveit_pro_kinova_configs">moveit_pro_kinova_configs</a>
+    │   ├── kinova_gen3_base_config
+    │   ├── kinova_gen3_mujoco_config
+    │   └── moveit_studio_kinova_pstop_manager
+    ├── <a href="https://github.com/PickNikRobotics/moveit_pro_mobile_manipulation">moveit_pro_mobile_manipulation</a>
+    │   ├── mobile_manipulation_config
+    │   └── picknik_ur_mobile_config
+    ├── <a href="https://github.com/PickNikRobotics/fanuc_sim">fanuc_sim</a>
+    ├── <a href="https://github.com/PickNikRobotics/picknik_accessories">picknik_accessories</a> (submodule)
+    └── external_dependencies
+        ├── <a href="https://github.com/sjahr/ridgeback/tree/ros2">ridgeback</a>
+        ├── <a href="https://github.com/PickNikRobotics/ros2_robotiq_gripper">ros2_robotiq_gripper</a>
+        └── <a href="https://github.com/tylerjw/serial/tree/ros2">serial</a>
+</pre>
+
+This repository contains a **copy** of the git repositories that were added as subtrees.
+File changes and commits are treated as if they happen only in this repository.
+If you update the contents of a subtree, you can merge the latest `main` branch of [lab_sim](https://github.com/PickNikRobotics/lab_sim) using the following command:
+```bash
+git subtree pull --prefix src/lab_sim https://github.com/PickNikRobotics/lab_sim main --squash
+```
+
+To pull the upstream changes to all subtrees and submodules, a convenience script is provided.
+From the top level, you can execute:
+```bash
+./sync_subtrees.sh
+```

src/example_behaviors/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.22)
+project(example_behaviors CXX)
+
+find_package(moveit_studio_common REQUIRED)
+find_package(example_interfaces REQUIRED)
+moveit_studio_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior moveit_studio_behavior_interface pluginlib 
+moveit_studio_vision
+moveit_studio_vision_msgs
+PCL
+pcl_conversions
+pcl_ros
+example_interfaces)
+foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${package} REQUIRED)
+endforeach()
+find_package(moveit_pro_ml REQUIRED)
+
+add_library(
+  example_behaviors
+  SHARED 
+  src/example_add_two_ints_service_client.cpp
+  src/example_convert_mtc_solution_to_joint_trajectory.cpp
+  src/example_delayed_message.cpp
+  src/example_get_string_from_topic.cpp
+  src/example_fibonacci_action_client.cpp
+  src/example_hello_world.cpp
+  src/example_publish_color_rgba.cpp
+  src/example_setup_mtc_pick_from_pose.cpp
+  src/example_setup_mtc_place_from_pose.cpp
+  src/example_setup_mtc_wave_hand.cpp
+  src/example_ndt_registration.cpp
+  src/example_ransac_registration.cpp
+  src/example_sam2_segmentation.cpp
+  src/register_behaviors.cpp)
+target_include_directories(
+  example_behaviors
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  PRIVATE ${PCL_INCLUDE_DIRS})
+ament_target_dependencies(example_behaviors
+                          ${THIS_PACKAGE_INCLUDE_DEPENDS})
+target_link_libraries(example_behaviors onnx_sam2)
+
+# Install Libraries
+install(
+  TARGETS example_behaviors
+  EXPORT example_behaviorsTargets
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES
+  DESTINATION include)
+
+install(DIRECTORY config models DESTINATION share/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  moveit_pro_behavior_test(example_behaviors)
+endif()
+
+# Export the behavior plugins defined in this package so they are available to
+# plugin loaders that load the behavior base class library from the
+# moveit_studio_behavior package.
+pluginlib_export_plugin_description_file(
+  moveit_studio_behavior_interface example_behaviors_plugin_description.xml)
+
+ament_export_targets(example_behaviorsTargets HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+ament_package()

src/example_behaviors/README.md

@@ -0,0 +1 @@
+# example_behaviors

src/example_behaviors/behavior_plugin.yaml

@@ -0,0 +1,4 @@
+objectives:
+  behavior_loader_plugins:
+    example_behaviors:
+      - "example_behaviors::ExampleBehaviorsLoader"

src/example_behaviors/config/tree_nodes_model.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<root>
+  <TreeNodesModel>
+    <!-- Include additional SubTree metadata in this file. -->
+  </TreeNodesModel>
+</root>

src/example_behaviors/example_behaviors_plugin_description.xml

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="utf-8" ?>
+<library path="example_behaviors">
+  <class
+    type="example_behaviors::ExampleBehaviorsLoader"
+    base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
+  />
+</library>

src/example_behaviors/include/example_behaviors/example_add_two_ints_service_client.hpp

@@ -0,0 +1,51 @@
+#pragma once
+
+#include <moveit_studio_behavior_interface/service_client_behavior_base.hpp>
+#include <example_interfaces/srv/add_two_ints.hpp>
+
+using moveit_studio::behaviors::BehaviorContext;
+using moveit_studio::behaviors::ServiceClientBehaviorBase;
+using AddTwoInts = example_interfaces::srv::AddTwoInts;
+
+namespace example_behaviors
+{
+class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
+{
+public:
+  ExampleAddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
+                          const std::shared_ptr<BehaviorContext>& shared_resources);
+
+  /** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */
+  static BT::PortsList providedPorts();
+
+  /**
+   * @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
+   * subcategory, in the MoveIt Studio Developer Tool.
+   * @return A BT::KeyValueVector containing the Behavior metadata.
+   */
+  static BT::KeyValueVector metadata();
+
+private:
+  /** @brief User-provided function to get the name of the service when initializing the service client. */
+  tl::expected<std::string, std::string> getServiceName() override;
+
+  /**
+   * @brief User-provided function to create the service request.
+   * @return Returns a service request message. If not successful, returns an error message. Note that the criteria for
+   * success or failure is defined by the user's implementation of this function.
+   */
+  tl::expected<AddTwoInts::Request, std::string> createRequest() override;
+
+  /** @brief Optional user-provided function to process the service response after the service has finished. */
+  tl::expected<bool, std::string> processResponse(const AddTwoInts::Response& response) override;
+
+  /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
+  std::shared_future<tl::expected<bool, std::string>>& getFuture() override
+  {
+    return future_;
+  }
+
+  /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
+  std::shared_future<tl::expected<bool, std::string>> future_;
+};
+}  // namespace example_behaviors

src/example_behaviors/include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <Eigen/Core>
+#include <behaviortree_cpp/action_node.h>
+#include <behaviortree_cpp/bt_factory.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <moveit_studio_behavior/utils/trajectory_utils.hpp>
+#include <moveit_studio_behavior_interface/behavior_context.hpp>
+#include <moveit_studio_behavior_interface/check_for_error.hpp>
+#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
+#include <moveit_task_constructor_msgs/msg/solution.hpp>
+#include <spdlog/spdlog.h>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
+#include <yaml-cpp/yaml.h>
+
+namespace example_behaviors
+{
+/**
+ * @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory.
+ *
+ * @details
+ * | Data Port Name    | Port Type     | Object Type                                             |
+ * | ----------------- |---------------|---------------------------------------------------------|
+ * | solution          | Input         | moveit_task_constructor_msgs::msg::Solution             |
+ * | joint_group       | Input         | std::string                                             |
+ * | velocity_scaling_factor | Input   | double                                                  |
+ * | acceleration_scaling_factor | Input | double                                               |
+ * | sampling_rate     | Input         | int                                                     |
+ * | joint_trajectory  | Output        | trajectory_msgs::msg::JointTrajectory                   |
+ */
+class ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
+{
+public:
+  ExampleConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config,
+                                      const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
+
+  static BT::PortsList providedPorts();
+
+  static BT::KeyValueVector metadata();
+
+  BT::NodeStatus tick() override;
+
+private:
+  std::unique_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_;
+
+  const std::vector<Eigen::VectorXd>& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution);
+};
+}  // namespace example_behaviors