From 9aef77094fe24ac5a76f0e52c36faab4c6b8207c Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 24 Feb 2026 15:00:20 -0700 Subject: [PATCH 1/2] Add lab_sim_behaviors package with ComputeTrayPlacePositionsUsingAprilTags - New lab_sim_behaviors package providing custom Behavior plugins for lab_sim - ComputeTrayPlacePositionsUsingAprilTags: computes grid-based place positions relative to a tray AprilTag for bottle placement workflows - Fix tray center direction and reverse row fill order - Register the behavior loader plugin in lab_sim config.yaml - Add lab_sim_behaviors exec_depend to lab_sim package.xml Co-Authored-By: Claude Opus 4.6 --- src/lab_sim/config/config.yaml | 2 + src/lab_sim/package.xml | 1 + src/lab_sim_behaviors/CMakeLists.txt | 46 +++ ...e_tray_place_positions_using_apriltags.hpp | 62 ++++ .../lab_sim_behaviors_plugin_description.xml | 7 + src/lab_sim_behaviors/package.xml | 30 ++ ...e_tray_place_positions_using_apriltags.cpp | 272 ++++++++++++++++++ .../src/register_behaviors.cpp | 29 ++ src/lab_sim_behaviors/test/CMakeLists.txt | 4 + .../test/test_behavior_plugins.cpp | 42 +++ 10 files changed, 495 insertions(+) create mode 100644 src/lab_sim_behaviors/CMakeLists.txt create mode 100644 src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp create mode 100644 src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml create mode 100644 src/lab_sim_behaviors/package.xml create mode 100644 src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp create mode 100644 src/lab_sim_behaviors/src/register_behaviors.cpp create mode 100644 src/lab_sim_behaviors/test/CMakeLists.txt create mode 100644 src/lab_sim_behaviors/test/test_behavior_plugins.cpp diff --git a/src/lab_sim/config/config.yaml b/src/lab_sim/config/config.yaml index ca1ceeb2..e78f0500 100644 --- a/src/lab_sim/config/config.yaml +++ b/src/lab_sim/config/config.yaml @@ -48,6 +48,8 @@ objectives: - "moveit_pro::behaviors::VisionBehaviorsLoader" - "moveit_pro::behaviors::ConverterBehaviorsLoader" - "moveit_pro::behaviors::MujocoBehaviorsLoader" + lab_sim: + - "lab_sim_behaviors::LabSimBehaviorsLoader" # Specify source folder for objectives # [Required] objective_library_paths: diff --git a/src/lab_sim/package.xml b/src/lab_sim/package.xml index 86451190..6bddc807 100644 --- a/src/lab_sim/package.xml +++ b/src/lab_sim/package.xml @@ -28,6 +28,7 @@ robotiq_description ur_description velocity_force_controller + lab_sim_behaviors ament_lint_auto diff --git a/src/lab_sim_behaviors/CMakeLists.txt b/src/lab_sim_behaviors/CMakeLists.txt new file mode 100644 index 00000000..0558bd92 --- /dev/null +++ b/src/lab_sim_behaviors/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.22) +project(lab_sim_behaviors CXX) + +find_package(moveit_studio_common REQUIRED) +moveit_studio_package() + +set(THIS_PACKAGE_INCLUDE_DEPENDS + moveit_pro_behavior + moveit_pro_behavior_interface + moveit_studio_vision + moveit_studio_vision_msgs + pluginlib) +foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${package} REQUIRED) +endforeach() + +add_library( + lab_sim_behaviors + SHARED + src/compute_tray_place_positions_using_apriltags.cpp + src/register_behaviors.cpp) +target_include_directories( + lab_sim_behaviors + PUBLIC $ + $) +ament_target_dependencies(lab_sim_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +install( + TARGETS lab_sim_behaviors + EXPORT lab_sim_behaviorsTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include) + +if(BUILD_TESTING) + moveit_pro_behavior_test(lab_sim_behaviors) +endif() + +pluginlib_export_plugin_description_file( + moveit_pro_behavior_interface lab_sim_behaviors_plugin_description.xml) + +ament_export_targets(lab_sim_behaviorsTargets HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_package() diff --git a/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp b/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp new file mode 100644 index 00000000..84999b08 --- /dev/null +++ b/src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp @@ -0,0 +1,62 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#pragma once + +#include +#include +#include +#include + +namespace lab_sim_behaviors +{ +/** + * @brief Computes a grid of place positions within a tray detected via its AprilTag. + * + * @details Given an array of AprilTag detections and camera intrinsics, this Behavior finds the + * tray's AprilTag by ID, uses its 3D pose plus a known offset to locate the tray center, then + * generates a grid of place positions (multiple columns per row). Rows fill from the far end + * of the tray toward the tag (top-to-bottom). Positions are transformed from the camera frame + * to the world frame with a fixed gripper-down orientation. + * + * If an input image is provided, the Behavior annotates it with green circles sized to match + * the bottle diameter at each computed place position and publishes the result. + * + * | Data Port Name | Port Type | Object Type | + * | ----------------------- | --------- | --------------------------------------------------- | + * | detections | input | moveit_studio_vision_msgs::msg::ObjectDetectionArray | + * | camera_info | input | sensor_msgs::msg::CameraInfo | + * | input_image | input | sensor_msgs::msg::Image (optional) | + * | tray_apriltag_id | input | int | + * | num_rows | input | int | + * | columns_per_row | input | int | + * | row_spacing | input | double | + * | column_spacing | input | double | + * | tag_to_tray_center | input | double | + * | bottle_diameter | input | double | + * | visualization_topic | input | std::string | + * | place_positions | output | std::vector | + */ +class ComputeTrayPlacePositionsUsingAprilTags final + : public moveit_pro::behaviors::SharedResourcesNode +{ +public: + ComputeTrayPlacePositionsUsingAprilTags( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources, + std::unique_ptr ros_publisher_interface = + std::make_unique()); + + static BT::PortsList providedPorts(); + + static BT::KeyValueVector metadata(); + + BT::NodeStatus tick() override; + +private: + std::unique_ptr ros_publisher_interface_; +}; +} // namespace lab_sim_behaviors diff --git a/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml b/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml new file mode 100644 index 00000000..bbfc828d --- /dev/null +++ b/src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml @@ -0,0 +1,7 @@ + + + + diff --git a/src/lab_sim_behaviors/package.xml b/src/lab_sim_behaviors/package.xml new file mode 100644 index 00000000..c06505fe --- /dev/null +++ b/src/lab_sim_behaviors/package.xml @@ -0,0 +1,30 @@ + + + lab_sim_behaviors + 6.5.0 + Lab sim specific behavior plugins for MoveIt Pro + + MoveIt Pro Maintainer + MoveIt Pro Maintainer + + BSD-3-Clause + + ament_cmake + + moveit_studio_common + + moveit_pro_behavior + moveit_pro_behavior_interface + moveit_studio_vision + moveit_studio_vision_msgs + pluginlib + + ament_lint_auto + ament_cmake_gtest + ament_clang_format + ament_clang_tidy + + + ament_cmake + + diff --git a/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp b/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp new file mode 100644 index 00000000..1b8ca52a --- /dev/null +++ b/src/lab_sim_behaviors/src/compute_tray_place_positions_using_apriltags.cpp @@ -0,0 +1,272 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace +{ +inline constexpr auto kDescriptionComputeTrayPlacePositionsUsingAprilTags = R"( +

+ Computes a grid of place positions within a tray detected by its AprilTag. + Finds the tray tag by ID, uses its 3D pose plus a known offset to define the + tray center, then generates a grid of positions with the specified number of + columns per row and row spacing. Rows fill from near the tag toward + the far end of the tray (top-to-bottom). Positions are transformed to the world frame with a + fixed gripper-down orientation. If an input image is provided, annotates it + with green circles matching the bottle diameter at each position. +

+ )"; + +constexpr auto kPortIDDetections = "detections"; +constexpr auto kPortIDCameraInfo = "camera_info"; +constexpr auto kPortIDInputImage = "input_image"; +constexpr auto kPortIDTrayApriltagId = "tray_apriltag_id"; +constexpr auto kPortIDNumRows = "num_rows"; +constexpr auto kPortIDColumnsPerRow = "columns_per_row"; +constexpr auto kPortIDRowSpacing = "row_spacing"; +constexpr auto kPortIDColumnSpacing = "column_spacing"; +constexpr auto kPortIDTagToTrayCenter = "tag_to_tray_center"; +constexpr auto kPortIDBottleDiameter = "bottle_diameter"; +constexpr auto kPortIDVisualizationTopic = "visualization_topic"; +constexpr auto kPortIDPlacePositions = "place_positions"; +constexpr auto kDefaultVisualizationTopic = "/apriltag_detections"; +constexpr auto kMarkerThickness = 2; +} // namespace + +namespace lab_sim_behaviors +{ +ComputeTrayPlacePositionsUsingAprilTags::ComputeTrayPlacePositionsUsingAprilTags( + const std::string& name, const BT::NodeConfiguration& config, + const std::shared_ptr& shared_resources, + std::unique_ptr ros_publisher_interface) + : SharedResourcesNode(name, config, shared_resources) + , ros_publisher_interface_{ std::move(ros_publisher_interface) } +{ +} + +BT::PortsList ComputeTrayPlacePositionsUsingAprilTags::providedPorts() +{ + return { + BT::InputPort(kPortIDDetections, "{detections}", + "Array of AprilTag detections from " + "DetectAprilTags. Must include the tray tag."), + BT::InputPort(kPortIDCameraInfo, "{camera_info}", + "Camera intrinsics used to project 3D positions to pixel coordinates " + "for visualization."), + BT::InputPort( + kPortIDInputImage, "", "Optional camera image to annotate with green circles at each computed place position."), + BT::InputPort(kPortIDTrayApriltagId, 1, + "AprilTag ID assigned to the tray. Used to find the tray among all detections."), + BT::InputPort(kPortIDNumRows, 3, "Number of rows of place positions along the tray's Y axis."), + BT::InputPort(kPortIDColumnsPerRow, 2, "Number of columns per row along the tray's X axis."), + BT::InputPort(kPortIDRowSpacing, 0.055, "Distance in meters between rows along the tray's Y axis."), + BT::InputPort(kPortIDColumnSpacing, 0.05, "Distance in meters between columns along the tray's X axis."), + BT::InputPort(kPortIDTagToTrayCenter, 0.15, + "Distance in meters from the tray tag to the tray center along the tray's +Y axis."), + BT::InputPort(kPortIDBottleDiameter, 0.04, + "Bottle diameter in meters. Used to size the visualization circles on the annotated image."), + BT::InputPort(kPortIDVisualizationTopic, kDefaultVisualizationTopic, + "ROS image topic to publish the annotated image to. Only used when input_image is " + "provided."), + BT::OutputPort>(kPortIDPlacePositions, "{place_positions}", + "Computed world-frame poses for placing objects in " + "the tray, with gripper-down orientation."), + }; +} + +BT::KeyValueVector ComputeTrayPlacePositionsUsingAprilTags::metadata() +{ + return { { moveit_pro::behaviors::kSubcategoryMetadataKey, "Vision" }, + { moveit_pro::behaviors::kDescriptionMetadataKey, kDescriptionComputeTrayPlacePositionsUsingAprilTags } }; +} + +BT::NodeStatus ComputeTrayPlacePositionsUsingAprilTags::tick() +{ + const auto ports = moveit_pro::behaviors::getRequiredInputs( + getInput(kPortIDDetections), + getInput(kPortIDCameraInfo)); + + if (!ports.has_value()) + { + shared_resources_->logger->publishFailureMessage(name(), "Failed to get required values from input data ports: " + + ports.error()); + return BT::NodeStatus::FAILURE; + } + + const auto& [detections_array, camera_info] = ports.value(); + + const int tray_apriltag_id = getInput(kPortIDTrayApriltagId).value(); + const int num_rows = getInput(kPortIDNumRows).value(); + const int columns_per_row = getInput(kPortIDColumnsPerRow).value(); + const double row_spacing = getInput(kPortIDRowSpacing).value(); + const double column_spacing = getInput(kPortIDColumnSpacing).value(); + const double tag_to_tray_center = getInput(kPortIDTagToTrayCenter).value(); + const double bottle_diameter = getInput(kPortIDBottleDiameter).value(); + + // Find the tray AprilTag by matching tray_apriltag_id + const auto tray_it = + std::find_if(detections_array.detections.begin(), detections_array.detections.end(), + [tray_apriltag_id](const auto& detection) { return detection.id == tray_apriltag_id; }); + + if (tray_it == detections_array.detections.end()) + { + shared_resources_->logger->publishFailureMessage( + name(), fmt::format("Tray AprilTag with ID {} not found in detections.", tray_apriltag_id)); + return BT::NodeStatus::FAILURE; + } + + const auto& tray_pose = tray_it->pose.pose; + const std::string& camera_frame = tray_it->pose.header.frame_id; + + // Get the tray tag's 3D pose in camera frame + const Eigen::Quaterniond tag_rotation(tray_pose.orientation.w, tray_pose.orientation.x, tray_pose.orientation.y, + tray_pose.orientation.z); + const Eigen::Vector3d tag_position(tray_pose.position.x, tray_pose.position.y, tray_pose.position.z); + + // Define tray local axes in camera frame + const Eigen::Vector3d tray_x = tag_rotation * Eigen::Vector3d::UnitX(); + const Eigen::Vector3d tray_y = tag_rotation * Eigen::Vector3d::UnitY(); + + // Compute tray center from tag position. The tag's +Y points away from the tray + // interior, so we move in the -Y direction to reach the tray center. + const Eigen::Vector3d tray_center = tag_position - tray_y * tag_to_tray_center; + + // Generate grid positions. Rows fill from near the tag (-Y) toward the far end (+Y). + // Within each row, columns are centered around X=0. + const int num_positions = num_rows * columns_per_row; + std::vector positions_camera; + positions_camera.reserve(num_positions); + + for (int row = 0; row < num_rows; ++row) + { + // Row 0 is closest to the tag (-Y), last row is at the far end (+Y) + const double row_offset = (row - (num_rows - 1) / 2.0) * row_spacing; + + for (int col = 0; col < columns_per_row; ++col) + { + const double col_offset = (col - (columns_per_row - 1) / 2.0) * column_spacing; + positions_camera.push_back(tray_center + tray_y * row_offset + tray_x * col_offset); + } + } + + // Look up camera_frame -> "world" transform + if (!shared_resources_->transform_buffer_ptr->canTransform("world", camera_frame, tf2::TimePointZero)) + { + shared_resources_->logger->publishFailureMessage(name(), fmt::format("Cannot transform between '{}' and 'world'.", + camera_frame)); + return BT::NodeStatus::FAILURE; + } + const geometry_msgs::msg::TransformStamped camera_to_world_tf = + shared_resources_->transform_buffer_ptr->lookupTransform("world", camera_frame, tf2::TimePointZero); + const Eigen::Isometry3d camera_to_world = tf2::transformToEigen(camera_to_world_tf); + + // Transform all positions to world frame with gripper-down orientation (x=1, y=0, z=0, w=0) + std::vector place_positions; + place_positions.reserve(num_positions); + for (const auto& pos_camera : positions_camera) + { + const Eigen::Vector3d pos_world = camera_to_world * pos_camera; + + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = "world"; + pose.pose.position.x = pos_world.x(); + pose.pose.position.y = pos_world.y(); + pose.pose.position.z = pos_world.z(); + pose.pose.orientation.x = 1.0; + pose.pose.orientation.y = 0.0; + pose.pose.orientation.z = 0.0; + pose.pose.orientation.w = 0.0; + place_positions.push_back(pose); + } + + // Visualization: annotate the camera image with place position markers + const auto maybe_image = getInput(kPortIDInputImage); + if (!maybe_image.has_value()) + { + shared_resources_->logger->publishInfoMessage(name(), "No input_image provided; skipping visualization."); + } + else + { + try + { + const auto viz_topic = getInput(kPortIDVisualizationTopic).value(); + ros_publisher_interface_->init(viz_topic, shared_resources_); + + const double fx = camera_info.k[0]; + const double fy = camera_info.k[4]; + const double cx = camera_info.k[2]; + const double cy = camera_info.k[5]; + + const cv_bridge::CvImagePtr cv_image_ptr = + cv_bridge::toCvCopy(maybe_image.value(), sensor_msgs::image_encodings::RGB8); + cv::Mat& cv_image = cv_image_ptr->image; + + const cv::Scalar green(0, 255, 0); + + for (std::size_t i = 0; i < positions_camera.size(); ++i) + { + const auto& p = positions_camera[i]; + if (p.z() <= 0.0) + { + continue; + } + + const int u = static_cast(fx * p.x() / p.z() + cx); + const int v = static_cast(fy * p.y() / p.z() + cy); + + // Skip positions that project outside the image bounds + if (u < 0 || u >= cv_image.cols || v < 0 || v >= cv_image.rows) + { + continue; + } + + // Compute pixel radius from bottle diameter using pinhole projection + const double radius_3d = bottle_diameter / 2.0; + const int pixel_radius = std::max(1, static_cast(fx * radius_3d / p.z())); + + // Draw a green circle matching the bottle diameter and a cross at each place position + cv::circle(cv_image, cv::Point(u, v), pixel_radius, green, kMarkerThickness); + cv::line(cv_image, cv::Point(u - pixel_radius, v), cv::Point(u + pixel_radius, v), green, kMarkerThickness); + cv::line(cv_image, cv::Point(u, v - pixel_radius), cv::Point(u, v + pixel_radius), green, kMarkerThickness); + + const std::string label = fmt::format("P{}", i); + const cv::Rect bbox(u - pixel_radius, v - pixel_radius, pixel_radius * 2, pixel_radius * 2); + moveit_studio::vision::overlayText(label, bbox, green, cv_image); + } + + sensor_msgs::msg::Image annotated_image; + cv_image_ptr->toImageMsg(annotated_image); + annotated_image.header = maybe_image.value().header; + + ros_publisher_interface_->publish_image(annotated_image); + } + catch (const cv_bridge::Exception& e) + { + shared_resources_->logger->publishFailureMessage(name(), fmt::format("cv_bridge error during visualization: {}", + e.what())); + } + } + + setOutput(kPortIDPlacePositions, place_positions); + + return BT::NodeStatus::SUCCESS; +} +} // namespace lab_sim_behaviors diff --git a/src/lab_sim_behaviors/src/register_behaviors.cpp b/src/lab_sim_behaviors/src/register_behaviors.cpp new file mode 100644 index 00000000..b68a72d4 --- /dev/null +++ b/src/lab_sim_behaviors/src/register_behaviors.cpp @@ -0,0 +1,29 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include +#include +#include + +#include + +#include + +namespace lab_sim_behaviors +{ +class LabSimBehaviorsLoader : public moveit_pro::behaviors::SharedResourcesNodeLoaderBase +{ +public: + void registerBehaviors(BT::BehaviorTreeFactory& factory, + const std::shared_ptr& shared_resources) override + { + moveit_pro::behaviors::registerBehavior( + factory, "ComputeTrayPlacePositionsUsingAprilTags", shared_resources); + } +}; +} // namespace lab_sim_behaviors + +PLUGINLIB_EXPORT_CLASS(lab_sim_behaviors::LabSimBehaviorsLoader, moveit_pro::behaviors::SharedResourcesNodeLoaderBase); diff --git a/src/lab_sim_behaviors/test/CMakeLists.txt b/src/lab_sim_behaviors/test/CMakeLists.txt new file mode 100644 index 00000000..8db41205 --- /dev/null +++ b/src/lab_sim_behaviors/test/CMakeLists.txt @@ -0,0 +1,4 @@ +find_package(ament_cmake_gtest REQUIRED) + +ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp) +ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/src/lab_sim_behaviors/test/test_behavior_plugins.cpp b/src/lab_sim_behaviors/test/test_behavior_plugins.cpp new file mode 100644 index 00000000..741998f0 --- /dev/null +++ b/src/lab_sim_behaviors/test/test_behavior_plugins.cpp @@ -0,0 +1,42 @@ +// Copyright 2026 PickNik Inc. +// All rights reserved. +// +// Unauthorized copying of this code base via any medium is strictly prohibited. +// Proprietary and confidential. + +#include + +#include +#include +#include +#include + +/** + * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and + * instantiated by the behavior tree factory. + */ +TEST(BehaviorTests, test_load_behavior_plugins) +{ + pluginlib::ClassLoader class_loader( + "moveit_pro_behavior_interface", "moveit_pro::behaviors::SharedResourcesNodeLoaderBase"); + + auto node = std::make_shared("test_node"); + auto shared_resources = std::make_shared(node); + + BT::BehaviorTreeFactory factory; + { + auto plugin_instance = class_loader.createUniqueInstance("lab_sim_behaviors::LabSimBehaviorsLoader"); + ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); + } + // Test that ClassLoader is able to find and instantiate each Behavior using the package's plugin description info. + EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_behavior_name", "ComputeTrayPlacePositionsUsingAprilTags", + BT::NodeConfiguration())); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From e8b10ac6f2ae69f99a965e0d580742f5d322edfe Mon Sep 17 00:00:00 2001 From: Dave Coleman Date: Tue, 24 Feb 2026 15:04:26 -0700 Subject: [PATCH 2/2] Add lab desk objects, rework constrained pick-and-place, and clean up lab_sim Co-Authored-By: Claude Opus 4.6 --- .gitignore | 1 + CLAUDE.md | 30 + .../control/picknik_ur.ros2_control.yaml | 11 +- src/lab_sim/description/keyboard-texture.png | 3 + src/lab_sim/description/laptop-screen.png | 3 + src/lab_sim/description/picknik_ur.xacro | 12 +- src/lab_sim/description/scene.xml | 1028 ++++++++++++++++- .../objectives/3_waypoint_pick_and_place.xml | 18 +- src/lab_sim/objectives/_scan_scene.xml | 34 - src/lab_sim/objectives/close_gripper.xml | 4 +- ...nt.xml => computelinkposefromwaypoint.xml} | 10 +- .../constrained_pick_and_place_subtree.xml | 329 ++++-- .../objectives/constrained_pick_place.xml | 30 +- .../objectives/cycle_between_waypoints.xml | 2 +- .../get_candidate_grasps_subtree.xml | 1 - src/lab_sim/objectives/grasp_planning.xml | 2 +- .../load_mesh_as_green_poincloud.xml | 40 - .../load_mesh_as_red_pointcloud.xml | 39 - src/lab_sim/objectives/move_along_square.xml | 2 +- src/lab_sim/objectives/mpc_pose_tracking.xml | 2 +- ...th_static_sphere_point_cloud_avoidance.xml | 2 +- src/lab_sim/objectives/open_gripper.xml | 6 +- ...tle.xml => pick_1_pill_bottle_with_ml.xml} | 12 +- .../pick_all_bottles_with_apriltags.xml | 573 +++++++++ .../objectives/pick_and_place_example.xml | 44 - .../pick_april_tag_object_with_approval.xml | 4 +- src/lab_sim/objectives/pick_from_pose.xml | 2 +- .../pick_from_pose_with_approval.xml | 2 +- src/lab_sim/objectives/pick_up_cube.xml | 10 +- src/lab_sim/objectives/place_at_pose.xml | 2 +- .../place_at_pose_with_approval.xml | 2 +- src/lab_sim/objectives/plan_move_to_pose.xml | 11 +- src/lab_sim/objectives/register_cad_part.xml | 44 +- .../objectives/register_cad_part_subtree.xml | 40 +- .../objectives/scan_multiple_views.xml | 26 - .../scan_scene_-_multiple_point_clouds.xml | 121 ++ .../objectives/stack_blocks_with_icp.xml | 6 +- ...rasp_link.xml => visualize_grasp_link.xml} | 6 +- src/lab_sim/waypoints/ur_waypoints.yaml | 622 ++-------- src/picknik_accessories | 2 +- 40 files changed, 2237 insertions(+), 901 deletions(-) create mode 100644 CLAUDE.md create mode 100644 src/lab_sim/description/keyboard-texture.png create mode 100644 src/lab_sim/description/laptop-screen.png delete mode 100644 src/lab_sim/objectives/_scan_scene.xml rename src/lab_sim/objectives/{compute_linkpose_from_waypoint.xml => computelinkposefromwaypoint.xml} (88%) delete mode 100644 src/lab_sim/objectives/load_mesh_as_green_poincloud.xml delete mode 100644 src/lab_sim/objectives/load_mesh_as_red_pointcloud.xml rename src/lab_sim/objectives/{pick_1_pill_bottle.xml => pick_1_pill_bottle_with_ml.xml} (91%) create mode 100644 src/lab_sim/objectives/pick_all_bottles_with_apriltags.xml delete mode 100644 src/lab_sim/objectives/pick_and_place_example.xml delete mode 100644 src/lab_sim/objectives/scan_multiple_views.xml create mode 100644 src/lab_sim/objectives/scan_scene_-_multiple_point_clouds.xml rename src/lab_sim/objectives/{show_grasp_link.xml => visualize_grasp_link.xml} (82%) diff --git a/.gitignore b/.gitignore index ab974948..70b72a5e 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,4 @@ build/ install/ log/ .vscode/ +MUJOCO_LOG.TXT diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 00000000..fd619580 --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,30 @@ +# AI Code Assistant Instructions for MoveIt Pro Example Workspace + +## MuJoCo Scene Files + +### Keyframe qpos must match model DOF count + +When editing `scene.xml` files (adding/removing bodies with joints), the `` section's `qpos` attribute must have exactly the number of values matching the model's total degrees of freedom. A mismatch causes `ros2_control_node` to crash with: + +``` +Error: keyframe 0: invalid qpos size, expected length +``` + +Each joint type contributes to qpos: +- **freejoint**: 7 values (x, y, z, qw, qx, qy, qz) +- **hinge/slide**: 1 value each +- **ball**: 4 values (quaternion) + +After adding or removing bodies with joints, either: +1. **Remove the keyframe** and let MuJoCo use body `pos=` attributes for initial positions +2. **Regenerate the keyframe** from the MuJoCo Interactive Viewer (Ctrl+Shift+K) once the scene is stable + +### MuJoCo documentation + +Refer to [docs.picknik.ai](https://docs.picknik.ai) for MuJoCo configuration guides: + +- [Physics Simulator Setup](https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/) — creating scene.xml from URDF, camera/sensor setup, mesh conversion, MuJoCo Interactive Viewer +- [config.yaml Reference](https://docs.picknik.ai/how_to/configuration_tutorials/config_yaml_reference/) — `hardware` section for `picknik_mujoco_ros/MujocoSystem` plugin configuration +- [Simulator Keyframes Setup](https://docs.picknik.ai/how_to/configuration_tutorials/configure_keyframes/) — defining keyframes in scene.xml, `ResetMujocoKeyframe` Behavior +- [Optimize Model Meshes](https://docs.picknik.ai/how_to/configuration_tutorials/optimizing_robot_model_meshes/) — MuJoCo enforces 1-200,000 faces per STL +- [Simulation Troubleshooting](https://docs.picknik.ai/troubleshooting/Simulation%20Troubleshooting/) — physics parameters, grip stability, mass/inertia errors, rendering issues diff --git a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml index 022396ad..4638d966 100644 --- a/src/lab_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/lab_sim/config/control/picknik_ur.ros2_control.yaml @@ -64,18 +64,25 @@ joint_trajectory_controller: stopped_velocity_tolerance: 0.0 goal_time: 0.0 linear_rail_joint: + trajectory: 0.5 goal: 0.05 shoulder_pan_joint: + trajectory: 0.5 goal: 0.05 shoulder_lift_joint: + trajectory: 0.5 goal: 0.05 elbow_joint: + trajectory: 0.5 goal: 0.05 wrist_1_joint: + trajectory: 0.5 goal: 0.05 wrist_2_joint: + trajectory: 0.5 goal: 0.05 wrist_3_joint: + trajectory: 0.5 goal: 0.05 acceleration_limits: linear_rail_joint: 10.0 @@ -91,8 +98,8 @@ robotiq_gripper_controller: default: true joint: robotiq_85_left_knuckle_joint allow_stalling: true - stall_timeout: 0.05 - goal_tolerance: 0.02 + stall_timeout: 0.3 + goal_tolerance: 0.3 force_torque_sensor_broadcaster: ros__parameters: diff --git a/src/lab_sim/description/keyboard-texture.png b/src/lab_sim/description/keyboard-texture.png new file mode 100644 index 00000000..9e685eab --- /dev/null +++ b/src/lab_sim/description/keyboard-texture.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f0b212c56658ff5e5936f82867413fb357322e432664609c6b496c190925d20c +size 170370 diff --git a/src/lab_sim/description/laptop-screen.png b/src/lab_sim/description/laptop-screen.png new file mode 100644 index 00000000..67982b9f --- /dev/null +++ b/src/lab_sim/description/laptop-screen.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4045f46ccec667d8c317cde63758d9ab826f8990ebcdee163ca10a0241d52b09 +size 215006 diff --git a/src/lab_sim/description/picknik_ur.xacro b/src/lab_sim/description/picknik_ur.xacro index fe73e3e8..98d1a434 100644 --- a/src/lab_sim/description/picknik_ur.xacro +++ b/src/lab_sim/description/picknik_ur.xacro @@ -663,7 +663,7 @@ - + @@ -672,7 +672,7 @@ - + @@ -681,7 +681,7 @@ - + @@ -690,7 +690,7 @@ - + @@ -699,7 +699,7 @@ - + @@ -708,7 +708,7 @@ - + diff --git a/src/lab_sim/description/scene.xml b/src/lab_sim/description/scene.xml index 7d896c29..18e75e90 100644 --- a/src/lab_sim/description/scene.xml +++ b/src/lab_sim/description/scene.xml @@ -6,12 +6,87 @@ -