grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

feat: add release-to-move TCP gizmo control in Webots

Browse Source 
Add a native Webots marker for setting the active tool's TCP position and
orientation. Dragging edits the preview; releasing the mouse submits one
complete, checked joint trajectory with configured TCP speed limits.
Unreachable targets reset the marker to the current TCP.

Preserve coordinate, joint, and control-file commands through motion
reservations and a controller action proxy. Give the gizmo lowest priority
and finish its committed trajectory before dispatching queued regular
commands, including waits longer than the former two-second service timeout.

Place the patron TCP at its working end, 55.2 mm along local Z, and use the
flange TCP when no tool is attached. Add digital_twin.tcp_gizmo configuration,
ROS interfaces, launch integration, and regression coverage.

Document operation, configuration, tool attachment, speed limits, and
troubleshooting in both Russian and English.

Validation: 42 Python tests and 8 C++ tests passed. Webots regressions covered
one action per release, translation and rotation with and without a tool,
unreachable targets, and ordinary commands waiting for gizmo completion.
This commit is contained in:
Даниил Грабарь
2026-09-17 17:11:53 +03:00
parent 9f0f05df87
commit 3a5bc28bba
40 changed files with 2373 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_planning/CMakeLists.txt
+23
View File
@@ -9,6 +9,8 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(control_msgs REQUIRED)
find_package(rclpy REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_core REQUIRED)
@@ -16,6 +18,16 @@ find_package(moveit_ros_planning REQUIRED)
find_package(moveit_msgs REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(iiwa_msgs REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(trajectory_msgs REQUIRED)
find_package(action_msgs REQUIRED)
find_package(tf2_eigen REQUIRED)
add_executable(tcp_gizmo src/tcp_gizmo.cpp)
target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
target_include_directories(tcp_gizmo PRIVATE include)
ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
target_include_directories(motion_planning_cpp PUBLIC include)
@@ -33,6 +45,7 @@ ament_target_dependencies(motion_planning_cpp
ament_python_install_package(${PROJECT_NAME})
install(TARGETS
tcp_gizmo
motion_planning_cpp
DESTINATION lib/${PROJECT_NAME}
)
@@ -55,4 +68,14 @@ install(FILES
)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(ament_cmake_pytest REQUIRED)
ament_add_gtest(test_tcp_gizmo_policy test/test_tcp_gizmo_policy.cpp)
target_compile_features(test_tcp_gizmo_policy PUBLIC cxx_std_17)
target_include_directories(test_tcp_gizmo_policy PRIVATE include)
ament_add_pytest_test(test_motion_priority test/test_motion_priority.py
APPEND_ENV PYTHONPATH=${CMAKE_CURRENT_SOURCE_DIR})
endif()
ament_package()