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

Add motion planning actions and services for iiwa robot

Browse Source 
- Implemented action servers for MoveToPose and MoveToJoints.
- Added service for MoveToNamedPose and stop action.
- Updated README with usage examples for new actions.
- Enhanced planning configurations in YAML and Python files.
- Removed deprecated motion planning scripts.
This commit is contained in:
Даниил Грабарь
2026-05-05 17:15:08 +03:00
parent b3f4bc458b
commit e4d6bdc1af
16 changed files with 392 additions and 276 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+16
View File
@@ -46,6 +46,22 @@ LC_ALL=C ros2 launch iiwa_moveit demo...
sudo pip3 install transforms3d --break-system-packages
```
Отправка робота в точку:
```
ros2 action send_goal /iiwa/move_to_pose iiwa_msgs/action/MoveToPose \
"{x: 0.5, y: 0.0, z: 0.5, a: 3.14, b: 0, c: 0, speed: 0.1, planner: 'ptp'}"
ros2 action send_goal --feedback /iiwa/move_to_joints iiwa_msgs/action/MoveToJoints \
"{joints: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0], speed: 0.4}"
ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
"{name: 'home', speed: 0.5}"
ros2 service call /iiwa/move_to_named iiwa_msgs/srv/MoveToNamedPose \
"{name: 'work', speed: 0.3}"
ros2 service call /iiwa/stop std_srvs/srv/Trigger "{}"
```
Спавн объекта:
```bash
src/iiwa_bringup/launch/iiwa.launch.py
+7
View File
@@ -219,6 +219,13 @@ def _runtime_setup(context, *args, **kwargs):
moveit_configs.to_dict(),
{"robot_description": robot_description},
{"use_sim_time": use_sim_time},
{
"pose_link": settings.planning.pose_link,
"planning_group": settings.planning.planning_group,
"default_frame": settings.planning.default_frame,
"default_planner": settings.planning.default_planner,
"planning_attempts": settings.planning.planning_attempts,
},
],
)
src/iiwa_config/config/moveit/kinematics.yaml
+3 -2
View File
@@ -1,4 +1,5 @@
iiwa_arm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.0050000000000000001
kinematics_solver_timeout: 0.0050000000000000001
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
src/iiwa_config/config/moveit/moveit_cpp.yaml
+10 -1
View File
@@ -25,7 +25,7 @@ ompl_rrtc: # Namespace for individual plan request
max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning
planning_time: 1.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned
pilz_lin:
pilz_ptp:
plan_request_params:
planning_attempts: 1
planning_pipeline: pilz_industrial_motion_planner
@@ -34,6 +34,15 @@ pilz_lin:
max_acceleration_scaling_factor: 1.0
planning_time: 0.8
pilz_lin:
plan_request_params:
planning_attempts: 1
planning_pipeline: pilz_industrial_motion_planner
planner_id: "LIN"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 0.8
chomp_planner:
plan_request_params:
planning_attempts: 1
src/iiwa_config/config/setting.yaml
+7
View File
@@ -33,6 +33,13 @@ controller:
moveit_cpp: pkg://iiwa_config/config/moveit/moveit_cpp.yaml
planning:
pose_link: "link_ee" # TCP-линк для декартовых целей
planning_group: "iiwa_arm" # Группа планирования из SRDF
default_frame: "base_link" # Система отсчёта по умолчанию
default_planner: "ompl" # Планировщик по умолчанию
planning_attempts: 3 # Число попыток планирования
foxglove:
enabled: true # Запускать ли foxglove_bridge вместе с роботом
port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)
src/iiwa_msgs/CMakeLists.txt
+5 -1
View File
@@ -4,10 +4,14 @@ project(iiwa_msgs)
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(action_msgs REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"srv/MoveToPose.srv"
DEPENDENCIES geometry_msgs
"srv/MoveToNamedPose.srv"
"action/MoveToPose.action"
"action/MoveToJoints.action"
DEPENDENCIES geometry_msgs action_msgs
)
ament_export_dependencies(rosidl_default_runtime)
src/iiwa_msgs/action/MoveToJoints.action
+11
View File
@@ -0,0 +1,11 @@
# Goal — 7 joint positions in radians
float64[7] joints
# Velocity scaling 0.0–1.0
float32 speed
---
# Result
bool success
string message
---
# Feedback
string state # "planning" | "executing"
src/iiwa_msgs/action/MoveToPose.action
+21
View File
@@ -0,0 +1,21 @@
# Goal
float64 x
float64 y
float64 z
# ZYX Euler in radians (A=yaw, B=pitch, C=roll — KUKA ABC convention)
float64 a
float64 b
float64 c
# Velocity scaling 0.0–1.0
float32 speed
# Planner: ompl | ptp | lin | circ | chomp (default: ompl)
string planner
# Reference frame (default: base_link)
string frame_id
---
# Result
bool success
string message
---
# Feedback
string state # "planning" | "executing"
src/iiwa_msgs/package.xml
+1
View File
@@ -11,6 +11,7 @@
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>geometry_msgs</depend>
<depend>action_msgs</depend>
<depend>rosidl_default_runtime</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
src/iiwa_msgs/srv/MoveToNamedPose.srv
+7
View File
@@ -0,0 +1,7 @@
# Named state from SRDF (e.g. "home", "work")
string name
# Velocity scaling 0.0–1.0
float32 speed
---
bool success
string message
src/iiwa_msgs/srv/MoveToPose.srv
+14 -1
View File
@@ -1,4 +1,17 @@
geometry_msgs/PoseStamped pose
# Position in metres
float64 x
float64 y
float64 z
# Orientation as ZYX Euler angles in degrees (KUKA ABC convention: A=yaw, B=pitch, C=roll)
float64 a
float64 b
float64 c
# Velocity scaling factor 0–100 (%)
float32 speed
# Planner: "ompl" | "ptp" | "lin" | "chomp" (default: "ompl")
string planner
# Reference frame, defaults to "base_link" if empty
string frame_id
---
bool success
string message
src/iiwa_planning/CMakeLists.txt
-7
View File
@@ -37,13 +37,6 @@ install(TARGETS
DESTINATION lib/${PROJECT_NAME}
)
install(PROGRAMS
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}
src/iiwa_planning/scripts/motion_planning.py
-129
View File
@@ -1,129 +0,0 @@
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from moveit.core.robot_state import RobotState
from moveit.planning import MoveItPy, PlanningComponent
from geometry_msgs.msg import PoseStamped
from rclpy.executors import ExternalShutdownException
from tf_transformations import quaternion_from_euler
class MotionPlaning(Node):
def __init__(self):
super().__init__("motion_planning_node")
self.moveit = MoveItPy(node_name="motion_planning_node")
self.robot_arm: PlanningComponent = self.moveit.get_planning_component("iiwa_arm")
self.planning_scene_monitor = self.moveit.get_planning_scene_monitor()
self.get_logger().warn("***********************************************************")
self.get_logger().info("MotionPlanning node is ready...")
self.get_logger().warn("***********************************************************")
# self.robot_arm.set_start_state(configuration_name="home")
# self.robot_arm.set_goal_state(configuration_name="work")
# self.plan_end_execute()
self.timer = self.create_timer(5.0, self._run_once)
def _run_once(self):
self.timer.cancel()
self.handle_plan_execute_pose(0.677, 0.25, 0.21, 3.14, 0, 3.14)
def handle_plan_execute_pose(self, x, y, z, a, b, c):
pose_goal = PoseStamped()
pose_goal.header.frame_id="base_link"
pose_goal.header.stamp = self.get_clock().now().to_msg()
pose_goal.pose.position.x=x
pose_goal.pose.position.y=y
pose_goal.pose.position.z=z
xx, yy, zz, w = quaternion_from_euler(a, b, c)
pose_goal.pose.orientation.x=xx
pose_goal.pose.orientation.y=yy
pose_goal.pose.orientation.z=zz
pose_goal.pose.orientation.w=w
# Check collisions
with self.planning_scene_monitor.read_only() as scene:
robot_state = scene.current_state
original_joint_positions = robot_state.get_joint_group_positions("iiwa_arm")
ok_ik = robot_state.set_from_ik("iiwa_arm", pose_goal.pose, "link_ee")
robot_state.update()
if not ok_ik:
self.get_logger().warn("***********************************************************")
self.get_logger().error("IK failed -> abort")
self.get_logger().warn("***********************************************************")
return
robot_collision_status = scene.is_state_colliding(
robot_state=robot_state,
joint_model_group_name="iiwa_arm",
verbose=True
)
if robot_collision_status:
self.get_logger().warn("***********************************************************")
self.get_logger().error("Goal in collision -> abort")
self.get_logger().warn("***********************************************************")
return
self.get_logger().warn("***********************************************************")
self.get_logger().info(f"\nRobot is in collision: {robot_collision_status}\n")
self.get_logger().warn("***********************************************************")
robot_state.set_joint_group_positions(
"iiwa_arm",
original_joint_positions,
)
robot_state.update()
# получить текущее состояние как изначальное для перемещения
self.robot_arm.set_start_state_to_current_state()
# self.robot_arm.set_start_state(configuration_name="home")
# self.robot_arm.set_goal_state(configuration_name="work")
self.robot_arm.set_goal_state(
pose_stamped_msg=pose_goal,
pose_link="link_ee"
)
self.plan_end_execute()
def plan_end_execute(self):
self.get_logger().warn("***********************************************************")
self.get_logger().info("Planning trajectory")
self.get_logger().warn("***********************************************************")
plan_result = self.robot_arm.plan()
if plan_result:
self.get_logger().warn("***********************************************************")
self.get_logger().info("Executing plan")
self.get_logger().warn("***********************************************************")
robot_trajectory = plan_result.trajectory
self.moveit.execute(robot_trajectory, controllers=[])
else:
self.get_logger().warn("***********************************************************")
self.get_logger().info("Planning failed")
self.get_logger().warn("***********************************************************")
def main(args=None):
try:
rclpy.init(args=args)
node = MotionPlaning()
rclpy.spin(node=node)
node.destroy_node()
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
rclpy.shutdown()
if __name__ == "__main__":
main()
src/iiwa_planning/scripts/motion_planning_test.py
-89
View File
@@ -1,89 +0,0 @@
#!/usr/bin/env python3
import time
# generic ros libraries
import rclpy
from moveit.core.robot_state import RobotState
from moveit.planning import MoveItPy
from rclpy.impl.rcutils_logger import RcutilsLogger
from rclpy.logging import get_logger
def plan_and_execute(robot: MoveItPy,
planning_component,
logger: RcutilsLogger,
single_plan_parameters=None,
multi_plan_parameters=None,
sleep_time=0.0):
logger.info("Planning trajectory")
if multi_plan_parameters is not None:
plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters)
elif single_plan_parameters is not None:
plan_result = planning_component.plan(
single_plan_parameters=single_plan_parameters
)
else:
plan_result = planning_component.plan()
# execute the plan
if plan_result:
logger.info("Executing plan")
robot_trajectory = plan_result.trajectory
robot.execute(robot_trajectory, controllers=[])
else:
logger.error("Planning failed")
time.sleep(sleep_time)
def main():
rclpy.init()
logger = get_logger("moveit_py.pose_goal")
iiwa = MoveItPy(node_name="moveit_py")
iiwa_arm = iiwa.get_planning_component("iiwa_arm")
logger.info("MoveItPy instance created")
iiwa_arm.set_start_state(configuration_name="ready")
iiwa_arm.set_goal_state(configuration_name="extended")
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
robot_model = iiwa.get_robot_model()
robot_state = RobotState(robot_model)
# randomize the robot state
robot_state.set_to_random_positions()
# set plan start state to current state
iiwa_arm.set_start_state_to_current_state()
# set goal state to the initialized robot state
logger.info("Set goal state to the initialized robot state")
iiwa_arm.set_goal_state(robot_state=robot_state)
# plan to goal
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
# set plan start state to current state
iiwa_arm.set_start_state_to_current_state()
# set pose goal with PoseStamped message
from geometry_msgs.msg import PoseStamped
pose_goal = PoseStamped()
pose_goal.header.frame_id = "base_link"
pose_goal.pose.orientation.w = 1.0
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="patron")
# plan to goal
plan_and_execute(iiwa, iiwa_arm, logger, sleep_time=3.0)
if __name__ == "__main__":
main()
src/iiwa_planning/scripts/move_to_pose_server.py
+269 -46
View File
@@ -1,67 +1,290 @@
#!/usr/bin/env python3
import threading
import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, CancelResponse, GoalResponse
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor, ExternalShutdownException
from moveit.planning import MoveItPy, PlanningComponent
from iiwa_msgs.srv import MoveToPose
from geometry_msgs.msg import PoseStamped
from scipy.spatial.transform import Rotation
from std_srvs.srv import Trigger
from moveit.planning import MoveItPy, PlanningComponent, PlanRequestParameters
from moveit.core.robot_state import RobotState
from iiwa_msgs.action import MoveToPose, MoveToJoints
from iiwa_msgs.srv import MoveToNamedPose
POSE_LINK = "link_ee"
PLANNING_GROUP = "iiwa_arm"
# Таблица планировщиков: имя → (pipeline, planner_id, время планирования в секундах)
PLANNERS = {
"ompl": ("ompl", "RRTConnectkConfigDefault", 10.0),
"ptp": ("pilz_industrial_motion_planner", "PTP", 2.0),
"lin": ("pilz_industrial_motion_planner", "LIN", 2.0),
"circ": ("pilz_industrial_motion_planner", "CIRC", 2.0),
"chomp": ("chomp", "", 10.0),
}
def _abc_to_quaternion(a: float, b: float, c: float):
"""ZYX Euler (радианы, конвенция KUKA ABC) → (qx, qy, qz, qw)."""
return Rotation.from_euler("ZYX", [a, b, c], degrees=False).as_quat()
class IiwaMotionServer(Node):
"""Сервер управления движением манипулятора iiwa.
Предоставляет:
- action iiwa/move_to_pose — перемещение в декартову позу
- action iiwa/move_to_joints — перемещение по суставным координатам
- service iiwa/move_to_named — перемещение в именованную позу из SRDF
- service iiwa/stop — немедленная остановка движения
"""
def __init__(self):
super().__init__("iiwa_motion_server")
self._setup_parameters()
self._setup_moveit()
self._setup_servers()
self.get_logger().info("Сервер движения iiwa запущен")
self.get_logger().info(f" pose_link={self._pose_link} group={self._planning_group}")
def _setup_parameters(self):
self.declare_parameter("pose_link", "link_ee")
self.declare_parameter("planning_group", "iiwa_arm")
self.declare_parameter("default_frame", "base_link")
self.declare_parameter("default_planner", "ompl")
self.declare_parameter("planning_attempts", 3)
self._pose_link = self.get_parameter("pose_link").value
self._planning_group = self.get_parameter("planning_group").value
self._default_frame = self.get_parameter("default_frame").value
self._default_planner = self.get_parameter("default_planner").value
self._planning_attempts = self.get_parameter("planning_attempts").value
def _setup_moveit(self):
self._moveit = MoveItPy(node_name="iiwa_motion_server")
self._arm: PlanningComponent = self._moveit.get_planning_component(self._planning_group)
self._robot_model = self._moveit.get_robot_model()
def _setup_servers(self):
cb = ReentrantCallbackGroup()
ActionServer(
self, MoveToPose, "iiwa/move_to_pose", self._execute_pose,
callback_group=cb,
goal_callback=lambda _: GoalResponse.ACCEPT,
cancel_callback=lambda _: CancelResponse.ACCEPT,
)
ActionServer(
self, MoveToJoints, "iiwa/move_to_joints", self._execute_joints,
callback_group=cb,
goal_callback=lambda _: GoalResponse.ACCEPT,
cancel_callback=lambda _: CancelResponse.ACCEPT,
)
self.create_service(MoveToNamedPose, "iiwa/move_to_named", self._handle_named, callback_group=cb)
self.create_service(Trigger, "iiwa/stop", self._handle_stop, callback_group=cb)
def _make_plan_params(self, pipeline: str, planner_id: str, plan_time: float, velocity_scale: float) -> PlanRequestParameters:
params = PlanRequestParameters(self._moveit, self._planning_group)
params.planning_pipeline = pipeline
params.planner_id = planner_id
params.planning_time = plan_time
params.planning_attempts = self._planning_attempts
params.max_velocity_scaling_factor = velocity_scale
params.max_acceleration_scaling_factor = velocity_scale
return params
def _plan_and_execute(self, plan_params: PlanRequestParameters, goal_handle):
"""Планирует траекторию и выполняет её с поддержкой отмены.
Возвращает (True, msg) при успехе, (False, msg) при ошибке,
(None, 'canceled') если цель была отменена.
"""
plan_result = self._arm.plan(single_plan_parameters=plan_params)
if not plan_result:
return False, "Планирование не удалось: поза недостижима или в столкновении"
if goal_handle.is_cancel_requested:
return None, "canceled"
done = threading.Event()
failed = threading.Event()
def do_execute():
try:
self._moveit.execute(plan_result.trajectory, controllers=[])
except Exception as exc:
self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
failed.set()
finally:
done.set()
threading.Thread(target=do_execute, daemon=True).start()
while not done.wait(timeout=0.05):
if goal_handle.is_cancel_requested:
try:
self._moveit.get_trajectory_execution_manager().stop_execution()
except Exception:
pass
done.wait()
return None, "canceled"
if failed.is_set():
return False, "Выполнение траектории завершилось ошибкой"
return True, "Движение выполнено успешно"
def _finish_action(self, goal_handle, result, ok, msg):
"""Устанавливает финальное состояние action goal и заполняет result."""
result.success = bool(ok)
result.message = msg
if ok is None:
goal_handle.canceled()
elif ok:
goal_handle.succeed()
else:
self.get_logger().error(msg)
goal_handle.abort()
return result
def _execute_pose(self, goal_handle):
req = goal_handle.request
feedback = MoveToPose.Feedback()
result = MoveToPose.Result()
velocity_scale = max(0.01, min(1.0, float(req.speed)))
planner_key = (req.planner or self._default_planner).lower()
if planner_key not in PLANNERS:
result.success = False
result.message = f"Неизвестный планировщик '{planner_key}'. Доступные: {', '.join(PLANNERS)}"
self.get_logger().error(result.message)
goal_handle.abort()
return result
pipeline, planner_id, plan_time = PLANNERS[planner_key]
pose = PoseStamped()
pose.header.frame_id = req.frame_id or self._default_frame
pose.pose.position.x = req.x
pose.pose.position.y = req.y
pose.pose.position.z = req.z
qx, qy, qz, qw = _abc_to_quaternion(req.a, req.b, req.c)
pose.pose.orientation.x = qx
pose.pose.orientation.y = qy
pose.pose.orientation.z = qz
pose.pose.orientation.w = qw
self.get_logger().info(
f"[pose] xyz=({req.x:.3f}, {req.y:.3f}, {req.z:.3f}) "
f"abc=({req.a:.3f}, {req.b:.3f}, {req.c:.3f}) рад "
f"speed={velocity_scale:.2f} planner={planner_key}"
)
feedback.state = "planning"
goal_handle.publish_feedback(feedback)
self._arm.set_start_state_to_current_state()
self._arm.set_goal_state(pose_stamped_msg=pose, pose_link=self._pose_link)
feedback.state = "executing"
goal_handle.publish_feedback(feedback)
plan_params = self._make_plan_params(pipeline, planner_id, plan_time, velocity_scale)
ok, msg = self._plan_and_execute(plan_params, goal_handle)
return self._finish_action(goal_handle, result, ok, msg)
def _execute_joints(self, goal_handle):
req = goal_handle.request
feedback = MoveToJoints.Feedback()
result = MoveToJoints.Result()
velocity_scale = max(0.01, min(1.0, float(req.speed)))
joints = list(req.joints)
self.get_logger().info(
f"[joints] {[f'{v:.3f}' for v in joints]} speed={velocity_scale:.2f}"
)
feedback.state = "planning"
goal_handle.publish_feedback(feedback)
# Формируем целевое состояние по суставным координатам
goal_state = RobotState(self._robot_model)
goal_state.set_joint_group_positions(self._planning_group, joints)
goal_state.update()
self._arm.set_start_state_to_current_state()
self._arm.set_goal_state(robot_state=goal_state)
feedback.state = "executing"
goal_handle.publish_feedback(feedback)
plan_params = self._make_plan_params(
"ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
)
ok, msg = self._plan_and_execute(plan_params, goal_handle)
return self._finish_action(goal_handle, result, ok, msg)
def _handle_named(self, request: MoveToNamedPose.Request, response: MoveToNamedPose.Response):
name = request.name.strip()
velocity_scale = max(0.01, min(1.0, float(request.speed)))
self.get_logger().info(f"[named] name='{name}' speed={velocity_scale:.2f}")
self._arm.set_start_state_to_current_state()
try:
self._arm.set_goal_state(configuration_name=name)
except Exception as exc:
response.success = False
response.message = f"Неизвестное состояние '{name}': {exc}"
self.get_logger().error(response.message)
return response
plan_params = self._make_plan_params(
"ompl", "RRTConnectkConfigDefault", 10.0, velocity_scale
)
plan_result = self._arm.plan(single_plan_parameters=plan_params)
if not plan_result:
response.success = False
response.message = f"Не удалось построить траекторию для '{name}'"
self.get_logger().error(response.message)
return response
self._moveit.execute(plan_result.trajectory, controllers=[])
response.success = True
response.message = f"Переместился в '{name}'"
self.get_logger().info(response.message)
return response
def _handle_stop(self, request: Trigger.Request, response: Trigger.Response):
try:
self._moveit.get_trajectory_execution_manager().stop_execution()
response.success = True
response.message = "Выполнение траектории остановлено"
except Exception as exc:
response.success = False
response.message = str(exc)
self.get_logger().info(f"[stop] {response.message}")
return response
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:
rclpy.init(args=args)
node = IiwaMotionServer()
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
except (KeyboardInterrupt, ExternalShutdownException):
pass
finally:
node.destroy_node()
rclpy.shutdown()
src/iiwa_utils/iiwa_utils/setting_loader.py
+21
View File
@@ -54,6 +54,15 @@ class ControllerCfg:
moveit: MoveitCfg
@dataclass(frozen=True)
class PlanningCfg:
pose_link: str # TCP-линк для декартовых целей
planning_group: str # Группа планирования из SRDF
default_frame: str # Система отсчёта по умолчанию
default_planner: str # Планировщик по умолчанию
planning_attempts: int # Число попыток планирования
@dataclass(frozen=True)
class FoxgloveCfg:
enabled: bool # Запускать ли foxglove_bridge
@@ -87,6 +96,7 @@ class Settings:
robot: RobotCfg
digital_twin: DigitalTwinCfg
controller: ControllerCfg
planning: PlanningCfg
foxglove: FoxgloveCfg
def to_dict(self) -> Dict[str, Any]:
@@ -276,6 +286,16 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
moveit=moveit,
)
# planning
planning_raw = raw.get("planning", {})
planning = PlanningCfg(
pose_link=str(planning_raw.get("pose_link", "link_ee")),
planning_group=str(planning_raw.get("planning_group", "iiwa_arm")),
default_frame=str(planning_raw.get("default_frame", "base_link")),
default_planner=str(planning_raw.get("default_planner", "ompl")),
planning_attempts=int(planning_raw.get("planning_attempts", 3)),
)
# foxglove
foxglove = _parse_foxglove(raw.get("foxglove"))
@@ -283,6 +303,7 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
robot=robot,
digital_twin=digital_twin,
controller=controller,
planning=planning,
foxglove=foxglove,
)