diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index 82cc42d..4bf69e2 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -211,6 +211,17 @@ def _runtime_setup(context, *args, **kwargs): ], ) + move_to_pose_server = Node( + package="iiwa_planning", + executable="move_to_pose_server", + output="screen", + parameters=[ + moveit_configs.to_dict(), + {"robot_description": robot_description}, + {"use_sim_time": use_sim_time}, + ], + ) + # Rviz launch rviz_launch = Node( condition=IfCondition(LaunchConfiguration("rviz")), @@ -237,10 +248,11 @@ def _runtime_setup(context, *args, **kwargs): ) setup += [ - controllers_launch, - move_group, - rviz_launch, - shutdown_on_rviz_exit + controllers_launch, + move_group, + move_to_pose_server, + rviz_launch, + shutdown_on_rviz_exit, ] if settings.foxglove.enabled: diff --git a/src/iiwa_config/config/moveit/iiwa7.srdf b/src/iiwa_config/config/moveit/iiwa7.srdf index 43a34cb..3879ee9 100644 --- a/src/iiwa_config/config/moveit/iiwa7.srdf +++ b/src/iiwa_config/config/moveit/iiwa7.srdf @@ -10,7 +10,7 @@ - + + diff --git a/src/iiwa_config/config/moveit/joint_limits.yaml b/src/iiwa_config/config/moveit/joint_limits.yaml index b7ecf07..0546fcf 100644 --- a/src/iiwa_config/config/moveit/joint_limits.yaml +++ b/src/iiwa_config/config/moveit/joint_limits.yaml @@ -9,38 +9,66 @@ default_acceleration_scaling_factor: 0.1 # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: joint1: + has_position_limits: true + min_position: -2.97 + max_position: 2.97 has_velocity_limits: true max_velocity: 1.71 has_acceleration_limits: true max_acceleration: 8.5521 + has_jerk_limits: false joint2: + has_position_limits: true + min_position: -2.09 + max_position: 2.09 has_velocity_limits: true max_velocity: 1.71 has_acceleration_limits: true max_acceleration: 8.5521 + has_jerk_limits: false joint3: + has_position_limits: true + min_position: -2.97 + max_position: 2.97 has_velocity_limits: true max_velocity: 1.75 has_acceleration_limits: true max_acceleration: 8.7266 + has_jerk_limits: false joint4: + has_position_limits: true + min_position: -2.09 + max_position: 2.09 has_velocity_limits: true max_velocity: 2.27 has_acceleration_limits: true max_acceleration: 11.3446 + has_jerk_limits: false joint5: + has_position_limits: true + min_position: -2.97 + max_position: 2.97 has_velocity_limits: true max_velocity: 2.4399999999999999 has_acceleration_limits: true max_acceleration: 12.2173 + has_jerk_limits: false joint6: + has_position_limits: true + min_position: -2.09 + max_position: 2.09 has_velocity_limits: true max_velocity: 3.1400000000000001 has_acceleration_limits: true max_acceleration: 15.7080 + has_jerk_limits: false joint7: + has_position_limits: true + min_position: -3.05 + max_position: 3.05 has_velocity_limits: true max_velocity: 3.1400000000000001 has_acceleration_limits: true max_acceleration: 15.7080 + has_jerk_limits: false \ No newline at end of file diff --git a/src/iiwa_msgs/CMakeLists.txt b/src/iiwa_msgs/CMakeLists.txt new file mode 100644 index 0000000..78ce136 --- /dev/null +++ b/src/iiwa_msgs/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.8) +project(iiwa_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(geometry_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/MoveToPose.srv" + DEPENDENCIES geometry_msgs +) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/iiwa_msgs/package.xml b/src/iiwa_msgs/package.xml new file mode 100644 index 0000000..e342def --- /dev/null +++ b/src/iiwa_msgs/package.xml @@ -0,0 +1,21 @@ + + + + iiwa_msgs + 0.0.0 + Custom ROS2 interfaces for KUKA iiwa7 + daniel + Apache-2.0 + + ament_cmake + rosidl_default_generators + + geometry_msgs + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + + diff --git a/src/iiwa_msgs/srv/MoveToPose.srv b/src/iiwa_msgs/srv/MoveToPose.srv new file mode 100644 index 0000000..89a6f07 --- /dev/null +++ b/src/iiwa_msgs/srv/MoveToPose.srv @@ -0,0 +1,4 @@ +geometry_msgs/PoseStamped pose +--- +bool success +string message diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 4c65cc5..bec853f 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/src/iiwa_planning/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_msgs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(iiwa_msgs REQUIRED) add_executable(motion_planning_cpp src/motion_planning_cpp.cpp) target_include_directories(motion_planning_cpp PUBLIC include) @@ -37,11 +38,17 @@ install(TARGETS ) install(PROGRAMS - # scripts/motion_planning_test.py - scripts/motion_planning.py + scripts/motion_planning_test.py + # scripts/motion_planning.py DESTINATION lib/${PROJECT_NAME} RENAME motion_planning ) +install(PROGRAMS + scripts/move_to_pose_server.py + DESTINATION lib/${PROJECT_NAME} + RENAME move_to_pose_server +) + ament_package() diff --git a/src/iiwa_planning/package.xml b/src/iiwa_planning/package.xml index 730cff4..1ed08e0 100644 --- a/src/iiwa_planning/package.xml +++ b/src/iiwa_planning/package.xml @@ -19,6 +19,7 @@ geometry_msgs moveit_py tf_transformations + iiwa_msgs ament_lint_auto ament_lint_common diff --git a/src/iiwa_planning/scripts/motion_planning_test.py b/src/iiwa_planning/scripts/motion_planning_test.py index 97b1efb..21da271 100644 --- a/src/iiwa_planning/scripts/motion_planning_test.py +++ b/src/iiwa_planning/scripts/motion_planning_test.py @@ -78,7 +78,8 @@ def main(): pose_goal.pose.position.x = 0.28 pose_goal.pose.position.y = -0.2 pose_goal.pose.position.z = 0.5 - iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, pose_link="link7") + iiwa_arm.set_goal_state(pose_stamped_msg=pose_goal, + pose_link="patron") # plan to goal plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0) diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py new file mode 100644 index 0000000..a112e7e --- /dev/null +++ b/src/iiwa_planning/scripts/move_to_pose_server.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException +from moveit.planning import MoveItPy, PlanningComponent +from iiwa_msgs.srv import MoveToPose + + +POSE_LINK = "link_ee" +PLANNING_GROUP = "iiwa_arm" + + +def main(args=None): + rclpy.init(args=args) + + # MoveItPy создаёт C++ узел с именем из лонча → читает robot_description_kinematics и т.д. + moveit = MoveItPy(node_name="move_to_pose_server") + arm: PlanningComponent = moveit.get_planning_component(PLANNING_GROUP) + + # Отдельный лёгкий узел для сервиса — другое имя, нет конфликта параметров + node = Node("move_to_pose_service") + logger = node.get_logger() + + cb_group = ReentrantCallbackGroup() + + def handle(request: MoveToPose.Request, response: MoveToPose.Response): + pose = request.pose + if not pose.header.frame_id: + pose.header.frame_id = "base_link" + + arm.set_start_state_to_current_state() + arm.set_goal_state(pose_stamped_msg=pose, pose_link=POSE_LINK) + + logger.info( + f"Planning to ({pose.pose.position.x:.3f}, " + f"{pose.pose.position.y:.3f}, {pose.pose.position.z:.3f})" + ) + plan_result = arm.plan() + + if not plan_result: + response.success = False + response.message = "Planning failed: pose may be unreachable or in collision" + logger.error(response.message) + return response + + moveit.execute(plan_result.trajectory, controllers=[]) + response.success = True + response.message = "Motion executed successfully" + logger.info(response.message) + return response + + node.create_service(MoveToPose, "iiwa/move_to_pose", handle, callback_group=cb_group) + logger.info(f"MoveToPoseServer ready (pose_link={POSE_LINK})") + + try: + executor = MultiThreadedExecutor() + executor.add_node(node) + executor.spin() + except (KeyboardInterrupt, ExternalShutdownException): + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/iiwa_voice/iiwa_voice/__init__.py b/src/iiwa_voice/iiwa_voice/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/src/iiwa_voice/iiwa_voice/voice_generate.py b/src/iiwa_voice/iiwa_voice/voice_generate.py deleted file mode 100644 index 5c891a5..0000000 --- a/src/iiwa_voice/iiwa_voice/voice_generate.py +++ /dev/null @@ -1,95 +0,0 @@ -import httpx -import sounddevice as sd -import numpy as np -import io -import wave -import argparse - -ENDPOINTS = { - "customvoice": "http://localhost:8091", - "voicedesign": "http://localhost:8092", - "base": "http://localhost:8093", -} - -def synthesize_and_play( - text: str, - voice: str = "Vivian", - model_type: str = "customvoice", - language: str = "English", - instruct: str = "", - base_url: str | None = None, -): - url = (base_url or ENDPOINTS[model_type]) + "/v1/audio/speech" - - model_map = { - "customvoice": "Qwen/Qwen3-TTS-12Hz-0.6B-CustomVoice", - "voicedesign": "Qwen/Qwen3-TTS-12Hz-0.6B-VoiceDesign", - "base": "Qwen/Qwen3-TTS-12Hz-0.6B-Base", - } - - payload = { - "model": model_map[model_type], - "input": text, - "voice": voice, - "language": language, - "response_format": "wav", - } - if instruct: - payload["instruct"] = instruct - - print(f"[→] Отправка запроса к {url} ...") - - with httpx.Client(timeout=120) as client: - response = client.post(url, json=payload) - response.raise_for_status() - audio_bytes = response.content - - # Читаем WAV из памяти — без записи на диск - with wave.open(io.BytesIO(audio_bytes)) as wf: - sample_rate = wf.getframerate() - n_channels = wf.getnchannels() - sample_width = wf.getsampwidth() # байт на семпл - frames = wf.readframes(wf.getnframes()) - - # Конвертируем байты → numpy array - dtype_map = {1: np.int8, 2: np.int16, 4: np.int32} - dtype = dtype_map.get(sample_width, np.int16) - audio_np = np.frombuffer(frames, dtype=dtype) - - if n_channels > 1: - audio_np = audio_np.reshape(-1, n_channels) - - # Нормализуем до float32 [-1.0, 1.0] для sounddevice - audio_float = audio_np.astype(np.float32) / np.iinfo(dtype).max - - duration = len(audio_float) / sample_rate - print(f"[♪] Воспроизведение: {duration:.1f} сек | {sample_rate} Hz | {n_channels}ch") - - sd.play(audio_float, samplerate=sample_rate) - sd.wait() # блокируемся до конца воспроизведения - print("[✓] Готово") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Qwen3-TTS клиент") - parser.add_argument("--text", default="Hello! This is Qwen3-TTS speaking.") - parser.add_argument("--voice", default="Vivian", - help="Имя голоса (CustomVoice) или описание (VoiceDesign)") - parser.add_argument("--type", default="customvoice", - choices=["customvoice", "voicedesign", "base"]) - parser.add_argument("--language", default="English") - parser.add_argument("--instruct", default="", - help="Дополнительная инструкция: 'speak slowly', 'angry tone' и т.п.") - parser.add_argument("--url", default=None, - help="Переопределить базовый URL, напр. http://myserver:8091") - args = parser.parse_args() - - synthesize_and_play( - text=args.text, - voice=args.voice, - model_type=args.type, - language=args.language, - instruct=args.instruct, - base_url=args.url, - ) - \ No newline at end of file diff --git a/src/iiwa_voice/package.xml b/src/iiwa_voice/package.xml deleted file mode 100644 index 943f388..0000000 --- a/src/iiwa_voice/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - iiwa_voice - 0.0.0 - TODO: Package description - daniel - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/iiwa_voice/resource/.env b/src/iiwa_voice/resource/.env deleted file mode 100644 index f7cf19c..0000000 --- a/src/iiwa_voice/resource/.env +++ /dev/null @@ -1,40 +0,0 @@ -# ============================================================================= -# Qwen3-TTS — настройки окружения (.env) -# Скопируйте этот файл в .env и заполните своими значениями -# ============================================================================= - -# --- Hugging Face --------------------------------------------------------- -# Токен нужен только если модели приватные или у вас rate-limit -HF_TOKEN= - -# Путь для кэша весов моделей (рекомендуется SSD с ~20 ГБ свободного места) -HF_CACHE_DIR=./hf_cache - -# --- GPU ------------------------------------------------------------------ -# Сколько GPU выделить каждому сервису (обычно 1) -GPU_COUNT=1 - -# Видимые GPU (например "0" или "0,1" для multi-GPU) -CUDA_VISIBLE_DEVICES=0 - -# Утилизация памяти GPU (0.0–1.0). Уменьшите при OOM. -GPU_MEM_UTIL=0.90 - -# Максимальная длина контекста -MAX_MODEL_LEN=4096 - -# --- Порты ---------------------------------------------------------------- -CUSTOMVOICE_PORT=8091 -VOICEDESIGN_PORT=8092 -BASE_PORT=8093 - -# --- Модели --------------------------------------------------------------- -# По умолчанию 1.7B. Замените на 0.6B для экономии VRAM (~4 ГБ вместо ~8 ГБ) -# MODEL_SIZE=Qwen/Qwen3-TTS-12Hz-1.7B-CustomVoice -MODEL_SIZE=Qwen/Qwen3-TTS-12Hz-0.6B-CustomVoice - -# MODEL_VOICEDESIGN=Qwen/Qwen3-TTS-12Hz-1.7B-VoiceDesign -MODEL_VOICEDESIGN=Qwen/Qwen3-TTS-12Hz-0.6B-VoiceDesign - -# MODEL_BASE=Qwen/Qwen3-TTS-12Hz-1.7B-Base -MODEL_BASE=Qwen/Qwen3-TTS-12Hz-0.6B-Base \ No newline at end of file diff --git a/src/iiwa_voice/resource/docker-compose.yaml b/src/iiwa_voice/resource/docker-compose.yaml deleted file mode 100644 index 0177b4c..0000000 --- a/src/iiwa_voice/resource/docker-compose.yaml +++ /dev/null @@ -1,101 +0,0 @@ -# ============================================================================= -# Qwen3-TTS — vLLM-Omni (docker-compose.yml) -# Три варианта модели на разных портах: -# customvoice → :8091 (заранее настроенные голоса: Vivian, Ryan …) -# voicedesign → :8092 (создание голоса по текстовому описанию) -# base → :8093 (клонирование голоса из ~3 сек. аудио) -# -# Запуск всех сервисов: docker compose --profile all up -d -# Только один сервис: docker compose --profile customvoice up -d -# Логи: docker compose logs -f qwen3-tts-customvoice -# ============================================================================= - -x-tts-common: &tts-common - image: vllm/vllm-omni:v0.18.0 - ipc: host - entrypoint: ["vllm", "serve"] - ulimits: - memlock: -1 - stack: 67108864 - restart: unless-stopped - deploy: - resources: - reservations: - devices: - - driver: nvidia - count: ${GPU_COUNT:-1} - capabilities: [gpu] - volumes: - - ${HF_CACHE_DIR:-./hf_cache}:/root/.cache/huggingface - - ./output_audio:/output_audio - environment: - HF_TOKEN: ${HF_TOKEN:-} - VLLM_WORKER_MULTIPROC_METHOD: spawn - CUDA_VISIBLE_DEVICES: ${CUDA_VISIBLE_DEVICES:-0} - healthcheck: - test: ["CMD", "python3", "-c", - "import urllib.request; urllib.request.urlopen('http://localhost:8091/health')"] - interval: 30s - timeout: 10s - retries: 5 - start_period: 120s - -# ============================================================================= -services: - - # CustomVoice — готовые голоса (Vivian, Ryan и др.) - qwen3-tts-customvoice: - <<: *tts-common - container_name: qwen3-tts-customvoice - ports: - - "${CUSTOMVOICE_PORT:-8091}:8091" - command: - - "${MODEL_CUSTOMVOICE:-Qwen/Qwen3-TTS-12Hz-0.63B-CustomVoice}" - - "--omni" - - "--port" - - "8091" - - "--gpu-memory-utilization" - - "${GPU_MEM_UTIL:-0.90}" - - "--max-model-len" - - "${MAX_MODEL_LEN:-4096}" - profiles: - - customvoice - - all - - # VoiceDesign — синтез голоса по описанию на естественном языке - qwen3-tts-voicedesign: - <<: *tts-common - container_name: qwen3-tts-voicedesign - ports: - - "${VOICEDESIGN_PORT:-8092}:8091" - command: - - "${MODEL_VOICEDESIGN:-Qwen/Qwen3-TTS-12Hz-0.63B-VoiceDesign}" - - "--omni" - - "--port" - - "8091" - - "--gpu-memory-utilization" - - "${GPU_MEM_UTIL:-0.90}" - - "--max-model-len" - - "${MAX_MODEL_LEN:-4096}" - profiles: - - voicedesign - - all - - # Base — клонирование голоса из референс-аудио - qwen3-tts-base: - <<: *tts-common - container_name: qwen3-tts-base - ports: - - "${BASE_PORT:-8093}:8091" - command: - - "${MODEL_BASE:-Qwen/Qwen3-TTS-12Hz-0.63B-Base}" - - "--omni" - - "--port" - - "8091" - - "--gpu-memory-utilization" - - "${GPU_MEM_UTIL:-0.90}" - - "--max-model-len" - - "${MAX_MODEL_LEN:-4096}" - profiles: - - base - - all \ No newline at end of file diff --git a/src/iiwa_voice/resource/iiwa_voice b/src/iiwa_voice/resource/iiwa_voice deleted file mode 100644 index e69de29..0000000 diff --git a/src/iiwa_voice/setup.cfg b/src/iiwa_voice/setup.cfg deleted file mode 100644 index 9f55aaa..0000000 --- a/src/iiwa_voice/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/iiwa_voice -[install] -install_scripts=$base/lib/iiwa_voice diff --git a/src/iiwa_voice/setup.py b/src/iiwa_voice/setup.py deleted file mode 100644 index 86e7857..0000000 --- a/src/iiwa_voice/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'iiwa_voice' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='daniel', - maintainer_email='grabardm@ml-dev.ru', - description='TODO: Package description', - license='TODO: License declaration', - extras_require={ - 'test': [ - 'pytest', - ], - }, - entry_points={ - 'console_scripts': [ - ], - }, -)