Даниил Грабарь
3a5bc28bba
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.
56 lines
2.2 KiB
Python
56 lines
2.2 KiB
Python
from launch_ros.actions import Node
|
|
from moveit_configs_utils import MoveItConfigsBuilder
|
|
from .tcp_gizmo_nodes import make_tcp_gizmo_nodes
|
|
|
|
|
|
def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
|
|
"""Возвращает (moveit_configs, [move_group, move_to_pose_server])."""
|
|
moveit_configs = (
|
|
MoveItConfigsBuilder("iiwa7", package_name="iiwa_config")
|
|
.robot_description(
|
|
file_path=settings.robot.description,
|
|
mappings={
|
|
"initial_positions_file": settings.controller.moveit.initial_positions
|
|
},
|
|
)
|
|
.robot_description_semantic(file_path=settings.controller.moveit.srdf)
|
|
.robot_description_kinematics(file_path=settings.controller.moveit.kinematics)
|
|
.joint_limits(file_path=settings.controller.moveit.joint_limits)
|
|
.pilz_cartesian_limits(file_path=settings.controller.moveit.pilz_limits)
|
|
.trajectory_execution(file_path=settings.controller.moveit.moveit_controllers)
|
|
.moveit_cpp(file_path=settings.controller.moveit.moveit_cpp)
|
|
.to_moveit_configs()
|
|
)
|
|
|
|
common_params = [
|
|
moveit_configs.to_dict(),
|
|
{"robot_description": robot_description},
|
|
{"use_sim_time": use_sim_time},
|
|
]
|
|
|
|
move_group = Node(
|
|
package="moveit_ros_move_group",
|
|
executable="move_group",
|
|
output="screen",
|
|
parameters=common_params,
|
|
)
|
|
|
|
move_to_pose_server = Node(
|
|
package="iiwa_planning",
|
|
executable="move_to_pose_server",
|
|
output="screen",
|
|
parameters=[
|
|
*common_params,
|
|
{
|
|
"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,
|
|
"tcp_gizmo_enabled": use_sim_time and settings.digital_twin.tcp_gizmo.enabled,
|
|
},
|
|
],
|
|
)
|
|
|
|
return moveit_configs, [move_group, move_to_pose_server, *make_tcp_gizmo_nodes(settings, common_params, use_sim_time)]
|