diff --git a/CMakeLists.txt b/CMakeLists.txt index 8fe61fe..cd06bf7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,7 +4,7 @@ project(thais_urdf) find_package(ament_cmake REQUIRED) install( - DIRECTORY launch config description hardware_config + DIRECTORY launch config description docs DESTINATION share/${PROJECT_NAME}/ ) @@ -15,6 +15,9 @@ if(BUILD_TESTING) ament_add_pytest_test(xacro_smoke test/test_xacro_smoke.py TIMEOUT 180 ) + ament_add_pytest_test(hardware_yaml test/test_hardware_yaml.py + TIMEOUT 120 + ) endif() ament_package() diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md index 23d84fd..3c63c41 100644 --- a/CONTRIBUTING.md +++ b/CONTRIBUTING.md @@ -4,5 +4,5 @@ be under the GNU General Public License, as dictated by that # More information -- [Developer guide](doc/DEVELOPER.md) — workflow, CI, layout, style +- [Developer guide](docs/DEVELOPER.md) — workflow, CI, layout, style - [Code of Conduct](CODE_OF_CONDUCT.md) diff --git a/README.md b/README.md index afa2cb3..b1387c6 100644 --- a/README.md +++ b/README.md @@ -9,23 +9,27 @@ The **`package.xml` name is `thais_urdf`** (historical name; content is the InMo ```text thais_urdf/ ├── package.xml # ROS package: thais_urdf -├── CMakeLists.txt # Installs launch/, config/, inmoov/ into share/thais_urdf +├── CMakeLists.txt # Installs launch/, config/, description/, docs/ +├── docs/ # Developer + hardware mapping (see Documentation map) ├── launch/ -│ ├── rviz.launch.py # Real stack + RViz + rosbridge + ros2_control node + spawners -│ ├── gazebo.launch.py # Gazebo + clock bridge + spawn + gz_ros2_control + RViz + rosbridge +│ ├── control.launch.py # ros2_control + spawners (default controllers from this package) +│ ├── rviz.launch.py # Real stack + RViz + rosbridge + ros2_control + spawners +│ ├── gazebo.launch.py # Gazebo + clock bridge + spawn + gz_ros2_control + RViz + rosbridge │ └── rviz_standalone.launch.py ├── config/ -│ └── inmoov_rviz.rviz -└── inmoov/ - ├── urdf/inmoov.urdf.xacro # Top-level xacro (base_path, use_gazebo_sim, controller_config) - ├── 3dmodel/robot_description.urdf.xacro +│ ├── controllers.yaml +│ ├── inmoov_rviz.rviz +│ └── hardware/ # active.yaml — hardware single source of truth +└── description/ + ├── urdf/inmoov.urdf.xacro + ├── robot_description/ ├── ros2_control/ - │ ├── inmoov_ros2_control.xacro # Real vs sim ros2_control systems + │ ├── inmoov_ros2_control.xacro │ └── inmoov_gazebo.xacro - └── meshes/ # Collision/visual assets (e.g. dae) + └── meshes/ ``` -**Install:** `launch/`, `config/`, and **`inmoov/`** (URDF, meshes) install to **`share/thais_urdf`**. Default `urdf_path` / `base_path` in the combo launches use **`ros2 pkg prefix thais_urdf`**; override only for custom trees. +**Install:** `launch/`, `config/`, **`description/`**, and **`docs/`** install to **`share/thais_urdf`**. Default `urdf_path` / `base_path` in the combo launches use **`ros2 pkg prefix thais_urdf`**; override only for custom trees. ## Relationship to lucy_ros_packages @@ -73,8 +77,8 @@ ros2 launch thais_urdf gazebo.launch.py Optional arguments for **`rviz.launch.py`** and **`gazebo.launch.py`** only: -- `urdf_path:=` — default: `$(ros2 pkg prefix thais_urdf)/share/thais_urdf/inmoov/urdf/inmoov.urdf.xacro` -- `base_path:=` — default: `.../share/thais_urdf/inmoov` (mesh and xacro include root) +- `urdf_path:=` — default: `$(ros2 pkg prefix thais_urdf)/share/thais_urdf/description/urdf/inmoov.urdf.xacro` +- `base_path:=` — default: `.../share/thais_urdf/description` (mesh and xacro include root) ### RViz in a second terminal (bringup already running) @@ -136,9 +140,11 @@ Tests call **`xacro`** on the URDF; coverage mainly reflects **test + launch** P | Doc | Content | |-----|---------| | This file | Repository scope and integration | -| [**doc/DEVELOPER.md**](doc/DEVELOPER.md) | **Contributors** — URDF/xacro, launches, install layout, extension checklist | +| [**docs/DEVELOPER.md**](docs/DEVELOPER.md) | **Contributors** — URDF/xacro, hardware YAML, launches, install layout, extension checklist | +| [**docs/hardware_mapping.md**](docs/hardware_mapping.md) | Schema and calibration for `config/hardware/active.yaml` | +| [**docs/inmoov_i2.md**](docs/inmoov_i2.md) | i1 scope vs i2 head actuators (YAML appendix) | | **lucy_ros_packages** repo README | Bringup, hardware control, cameras | -| Workspace **`lucy_ws/docs/developer_lucy_packages.md`** | Index pointing to each repo’s `doc/DEVELOPER.md` | +| Workspace **`lucy_ws/docs/developer_lucy_packages.md`** | Index pointing to each repo’s developer doc (paths vary by repo) | | Workspace **`lucy_ws/docs/simulation_and_visualization.md`** | Control panel ↔ ROS pipeline, sim time, gaps | ## License and assets diff --git a/config/control.launch.yaml b/config/control.launch.yaml new file mode 100644 index 0000000..d905f74 --- /dev/null +++ b/config/control.launch.yaml @@ -0,0 +1,3 @@ +urdf_path: description/urdf/inmoov.urdf.xacro +base_path: description +controllers_yaml: config/controllers.yaml diff --git a/config/controllers.yaml b/config/controllers.yaml new file mode 100644 index 0000000..4613288 --- /dev/null +++ b/config/controllers.yaml @@ -0,0 +1,102 @@ +# Generated by lucy_config_generator — do not edit. +controller_manager: + ros__parameters: + update_rate: 100 + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + left_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + right_arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + torso_head_controller: + type: joint_trajectory_controller/JointTrajectoryController + +joint_state_broadcaster: + ros__parameters: + extra_joints: + - Empty.001_link_joint + - Empty.002_link_joint + - i01.head.eyeLeft.001_link_joint + - i01.head.eyeLeft_link_joint + - i01.head.eyeRight.001_link_joint + - i01.head.eyeRight_link_joint + - i01.head.jaw_link_joint + - i01.head.neck.001_link_joint + - i01.head.rollNeck_link_joint + - i01.head.rothead_link_joint + - i01.leftHand.index2_link_joint + - i01.leftHand.index3_link_joint + - i01.leftHand.majeure2_link_joint + - i01.leftHand.majeure3_link_joint + - i01.leftHand.pinky0_link_joint + - i01.leftHand.pinky2_link_joint + - i01.leftHand.pinky3_link_joint + - i01.leftHand.ringfinger0_link_joint + - i01.leftHand.ringfinger2_link_joint + - i01.leftHand.ringfinger3_link_joint + - i01.leftHand.thumb1_link_joint + - i01.leftHand.thumb3_link_joint + - i01.leftHand.wrist.001_link_joint + - i01.rightHand.index2_link_joint + - i01.rightHand.index3_link_joint + - i01.rightHand.majeure2_link_joint + - i01.rightHand.majeure3_link_joint + - i01.rightHand.pinky0_link_joint + - i01.rightHand.pinky2_link_joint + - i01.rightHand.pinky3_link_joint + - i01.rightHand.ringfinger0_link_joint + - i01.rightHand.ringfinger2_link_joint + - i01.rightHand.ringfinger3_link_joint + - i01.rightHand.thumb1_link_joint + - i01.rightHand.thumb3_link_joint + - i01.rightHand.wrist.001_link_joint + - i01.torso.midStom_link_joint + - i01.torso.topStom_link_joint + +left_arm_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - left_shoulder_z_link_joint + - left_shoulder_x_link_joint + - left_elbow_x_link_joint + - left_wrist_z_link_joint + - i01.leftHand.thumb_link_joint + - i01.leftHand.index_link_joint + - i01.leftHand.majeure_link_joint + - i01.leftHand.ringFinger_link_joint + - i01.leftHand.pinky_link_joint + command_interfaces: + - position + state_interfaces: + - position + +right_arm_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - right_shoulder_z_link_joint + - right_shoulder_x_link_joint + - right_elbow + - right_wrist_z_link_joint + - i01.rightHand.thumb_link_joint + - i01.rightHand.index_link_joint + - i01.rightHand.majeure_link_joint + - i01.rightHand.ringFinger_link_joint + - i01.rightHand.pinky_link_joint + command_interfaces: + - position + state_interfaces: + - position + +torso_head_controller: + ros__parameters: + allow_nonzero_velocity_at_trajectory_end: false + joints: + - left_shoulder_y_link_joint + - right_shoulder_y_link_joint + command_interfaces: + - position + state_interfaces: + - position + diff --git a/config/hardware/README.md b/config/hardware/README.md new file mode 100644 index 0000000..9f6fbc6 --- /dev/null +++ b/config/hardware/README.md @@ -0,0 +1,7 @@ +# Hardware YAML store + +Authoritative documentation for the schema, calibration fields, and workflow lives in the installed package docs: + +**[docs/hardware_mapping.md](../../docs/hardware_mapping.md)** (also under `share/thais_urdf/docs/` after `colcon` install). + +Related: [docs/inmoov_i2.md](../../docs/inmoov_i2.md) — InMoov i1 scope vs i2 head actuators (appendix). diff --git a/config/hardware/active.yaml b/config/hardware/active.yaml new file mode 100644 index 0000000..6df6dff --- /dev/null +++ b/config/hardware/active.yaml @@ -0,0 +1,407 @@ +# Hardware mapping — single source of truth for lucy_config_generator. +# URDF joint names must match description/robot_description (processed URDF). +# left_shoulder_y_link_joint / right_shoulder_y_link_joint: torso board. + +version: 1 +robot_name: thais + +firmware: + source_dir: micro_ros_raspberrypi_pico_sdk + build_dir: build + +controller_manager: + update_rate: 100 + +boards: + rp2040_left_arm: + serial_id: E6617C93E3858429 + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_left_arm + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: + name: left_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_right_arm: + serial_id: E6617C93E37A6629 + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_right_arm + compile_definition: USE_RIGHT_ARM + topic_actuators: actuators/right_arm + topic_sensors: sensors/right_arm + controller: + name: right_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_torso_head: + serial_id: "" + board_class: internal_servo_i2c_pwm + internal_servo_slots: 16 + firmware_target: pico_micro_ros_torso + compile_definition: USE_TORSO + topic_actuators: actuators/torso + topic_sensors: sensors/torso + controller: + name: torso_head_controller + type: joint_trajectory_controller/JointTrajectoryController + +actuators: + # Torso board — shoulder Y joints are wired here (legacy URDF names). + - id: left_shoulder_y_torso + urdf_joint: left_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 0 + physical_pin: 5 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: false + - id: right_shoulder_y_torso + urdf_joint: right_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 1 + physical_pin: 6 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: false + + # Left arm (no shoulder Y — see torso entries above). + - id: left_shoulder_z + urdf_joint: left_shoulder_z_link_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + - id: left_shoulder_x + urdf_joint: left_shoulder_x_link_joint + board: rp2040_left_arm + virtual_pin: 1 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + - id: left_elbow_x + urdf_joint: left_elbow_x_link_joint + board: rp2040_left_arm + virtual_pin: 2 + physical_pin: 12 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 90.0 + enabled: false + - id: left_wrist_z + urdf_joint: left_wrist_z_link_joint + board: rp2040_left_arm + virtual_pin: 3 + physical_pin: 13 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_thumb + urdf_joint: i01.leftHand.thumb_link_joint + board: rp2040_left_arm + virtual_pin: 4 + physical_pin: 14 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_index + urdf_joint: i01.leftHand.index_link_joint + board: rp2040_left_arm + virtual_pin: 5 + physical_pin: 15 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_majeure + urdf_joint: i01.leftHand.majeure_link_joint + board: rp2040_left_arm + virtual_pin: 6 + physical_pin: 16 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_ring + urdf_joint: i01.leftHand.ringFinger_link_joint + board: rp2040_left_arm + virtual_pin: 7 + physical_pin: 17 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_pinky + urdf_joint: i01.leftHand.pinky_link_joint + board: rp2040_left_arm + virtual_pin: 8 + physical_pin: 18 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + + # Right arm + - id: right_shoulder_z + urdf_joint: right_shoulder_z_link_joint + board: rp2040_right_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 155.0 + servo_max_deg: 165.0 + servo_default_deg: 160.0 + enabled: true + - id: right_shoulder_x + urdf_joint: right_shoulder_x_link_joint + board: rp2040_right_arm + virtual_pin: 1 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 140.0 + servo_max_deg: 180.0 + servo_default_deg: 180.0 + enabled: true + - id: right_elbow + urdf_joint: right_elbow + board: rp2040_right_arm + virtual_pin: 2 + physical_pin: 12 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 100.0 + servo_max_deg: 110.0 + servo_default_deg: 100.0 + enabled: true + - id: right_wrist_z + urdf_joint: right_wrist_z_link_joint + board: rp2040_right_arm + virtual_pin: 3 + physical_pin: 13 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 230.0 + servo_default_deg: 150.0 + enabled: true + - id: right_thumb + urdf_joint: i01.rightHand.thumb_link_joint + board: rp2040_right_arm + virtual_pin: 4 + physical_pin: 14 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 36.0 + servo_max_deg: 150.0 + servo_default_deg: 36.0 + enabled: true + - id: right_index + urdf_joint: i01.rightHand.index_link_joint + board: rp2040_right_arm + virtual_pin: 5 + physical_pin: 15 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 16.0 + servo_max_deg: 150.0 + servo_default_deg: 16.0 + enabled: true + - id: right_majeure + urdf_joint: i01.rightHand.majeure_link_joint + board: rp2040_right_arm + virtual_pin: 6 + physical_pin: 16 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 21.0 + servo_max_deg: 150.0 + servo_default_deg: 21.0 + enabled: true + - id: right_ring + urdf_joint: i01.rightHand.ringFinger_link_joint + board: rp2040_right_arm + virtual_pin: 7 + physical_pin: 17 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 33.0 + servo_max_deg: 150.0 + servo_default_deg: 33.0 + enabled: true + - id: right_pinky + urdf_joint: i01.rightHand.pinky_link_joint + board: rp2040_right_arm + virtual_pin: 8 + physical_pin: 18 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 30.0 + servo_max_deg: 150.0 + servo_default_deg: 30.0 + enabled: true + +sensors: + - id: left_thumb_pressure + type: pressure + associated_actuator: left_thumb + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 16 + min_value: null + max_value: null + enabled: false + - id: left_index_pressure + type: pressure + associated_actuator: left_index + board: rp2040_left_arm + virtual_pin: 1 + physical_pin: 17 + min_value: null + max_value: null + enabled: false + - id: left_middle_pressure + type: pressure + associated_actuator: left_majeure + board: rp2040_left_arm + virtual_pin: 2 + physical_pin: 18 + min_value: null + max_value: null + enabled: false + - id: left_ring_pressure + type: pressure + associated_actuator: left_ring + board: rp2040_left_arm + virtual_pin: 3 + physical_pin: 19 + min_value: null + max_value: null + enabled: false + - id: left_pinky_pressure + type: pressure + associated_actuator: left_pinky + board: rp2040_left_arm + virtual_pin: 4 + physical_pin: 20 + min_value: null + max_value: null + enabled: false + - id: right_thumb_pressure + type: pressure + associated_actuator: right_thumb + board: rp2040_right_arm + virtual_pin: 0 + physical_pin: 16 + min_value: null + max_value: null + enabled: false + - id: right_index_pressure + type: pressure + associated_actuator: right_index + board: rp2040_right_arm + virtual_pin: 1 + physical_pin: 17 + min_value: null + max_value: null + enabled: false + - id: right_middle_pressure + type: pressure + associated_actuator: right_majeure + board: rp2040_right_arm + virtual_pin: 2 + physical_pin: 18 + min_value: null + max_value: null + enabled: false + - id: right_ring_pressure + type: pressure + associated_actuator: right_ring + board: rp2040_right_arm + virtual_pin: 3 + physical_pin: 19 + min_value: null + max_value: null + enabled: false + - id: right_pinky_pressure + type: pressure + associated_actuator: right_pinky + board: rp2040_right_arm + virtual_pin: 4 + physical_pin: 20 + min_value: null + max_value: null + enabled: false diff --git a/config/hardware/active_meta.yaml b/config/hardware/active_meta.yaml new file mode 100644 index 0000000..b7a35a5 --- /dev/null +++ b/config/hardware/active_meta.yaml @@ -0,0 +1,2 @@ +name: default +activated_at: "2026-04-18T00:00:00Z" diff --git a/config/hardware/backups/.gitkeep b/config/hardware/backups/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/config/hardware/configs/default.yaml b/config/hardware/configs/default.yaml new file mode 100644 index 0000000..c735400 --- /dev/null +++ b/config/hardware/configs/default.yaml @@ -0,0 +1,407 @@ +# Hardware mapping — single source of truth for lucy_config_generator. +# URDF joint names must match description/robot_description (processed URDF). +# left_shoulder_y_link_joint / right_shoulder_y_link_joint: torso board. + +version: 1 +robot_name: thais + +firmware: + source_dir: micro_ros_raspberrypi_pico_sdk + build_dir: build + +controller_manager: + update_rate: 100 + +boards: + rp2040_left_arm: + serial_id: E6617C93E3858429 + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_left_arm + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: + name: left_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_right_arm: + serial_id: E6617C93E37A6629 + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: pico_micro_ros_right_arm + compile_definition: USE_RIGHT_ARM + topic_actuators: actuators/right_arm + topic_sensors: sensors/right_arm + controller: + name: right_arm_controller + type: joint_trajectory_controller/JointTrajectoryController + rp2040_torso_head: + serial_id: "" + board_class: internal_servo_i2c_pwm + internal_servo_slots: 16 + firmware_target: pico_micro_ros_torso + compile_definition: USE_TORSO + topic_actuators: actuators/torso + topic_sensors: sensors/torso + controller: + name: torso_head_controller + type: joint_trajectory_controller/JointTrajectoryController + +actuators: + # Torso board — shoulder Y joints are wired here (legacy URDF names). + - id: left_shoulder_y_torso + urdf_joint: left_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 0 + physical_pin: 5 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: false + - id: right_shoulder_y_torso + urdf_joint: right_shoulder_y_link_joint + board: rp2040_torso_head + virtual_pin: 1 + physical_pin: 6 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 0.0 + enabled: false + + # Left arm (no shoulder Y — see torso entries above). + - id: left_shoulder_z + urdf_joint: left_shoulder_z_link_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + - id: left_shoulder_x + urdf_joint: left_shoulder_x_link_joint + board: rp2040_left_arm + virtual_pin: 1 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + - id: left_elbow_x + urdf_joint: left_elbow_x_link_joint + board: rp2040_left_arm + virtual_pin: 2 + physical_pin: 12 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 90.0 + enabled: false + - id: left_wrist_z + urdf_joint: left_wrist_z_link_joint + board: rp2040_left_arm + virtual_pin: 3 + physical_pin: 13 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_thumb + urdf_joint: i01.leftHand.thumb_link_joint + board: rp2040_left_arm + virtual_pin: 4 + physical_pin: 14 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_index + urdf_joint: i01.leftHand.index_link_joint + board: rp2040_left_arm + virtual_pin: 5 + physical_pin: 15 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_majeure + urdf_joint: i01.leftHand.majeure_link_joint + board: rp2040_left_arm + virtual_pin: 6 + physical_pin: 16 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_ring + urdf_joint: i01.leftHand.ringFinger_link_joint + board: rp2040_left_arm + virtual_pin: 7 + physical_pin: 17 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + - id: left_pinky + urdf_joint: i01.leftHand.pinky_link_joint + board: rp2040_left_arm + virtual_pin: 8 + physical_pin: 18 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 50.0 + enabled: false + + # Right arm + - id: right_shoulder_z + urdf_joint: right_shoulder_z_link_joint + board: rp2040_right_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 140.0 + servo_max_deg: 180.0 + servo_default_deg: 180.0 + enabled: true + - id: right_shoulder_x + urdf_joint: right_shoulder_x_link_joint + board: rp2040_right_arm + virtual_pin: 1 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 155.0 + servo_max_deg: 165.0 + servo_default_deg: 160.0 + enabled: true + - id: right_elbow + urdf_joint: right_elbow + board: rp2040_right_arm + virtual_pin: 2 + physical_pin: 12 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 100.0 + servo_max_deg: 110.0 + servo_default_deg: 100.0 + enabled: true + - id: right_wrist_z + urdf_joint: right_wrist_z_link_joint + board: rp2040_right_arm + virtual_pin: 3 + physical_pin: 13 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 230.0 + servo_default_deg: 150.0 + enabled: true + - id: right_thumb + urdf_joint: i01.rightHand.thumb_link_joint + board: rp2040_right_arm + virtual_pin: 4 + physical_pin: 14 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 36.0 + servo_max_deg: 150.0 + servo_default_deg: 36.0 + enabled: true + - id: right_index + urdf_joint: i01.rightHand.index_link_joint + board: rp2040_right_arm + virtual_pin: 5 + physical_pin: 15 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 16.0 + servo_max_deg: 150.0 + servo_default_deg: 16.0 + enabled: true + - id: right_majeure + urdf_joint: i01.rightHand.majeure_link_joint + board: rp2040_right_arm + virtual_pin: 6 + physical_pin: 16 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 21.0 + servo_max_deg: 150.0 + servo_default_deg: 21.0 + enabled: true + - id: right_ring + urdf_joint: i01.rightHand.ringFinger_link_joint + board: rp2040_right_arm + virtual_pin: 7 + physical_pin: 17 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 33.0 + servo_max_deg: 150.0 + servo_default_deg: 33.0 + enabled: true + - id: right_pinky + urdf_joint: i01.rightHand.pinky_link_joint + board: rp2040_right_arm + virtual_pin: 8 + physical_pin: 18 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 30.0 + servo_max_deg: 150.0 + servo_default_deg: 30.0 + enabled: true + +sensors: + - id: left_thumb_pressure + type: pressure + associated_actuator: left_thumb + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 16 + min_value: null + max_value: null + enabled: false + - id: left_index_pressure + type: pressure + associated_actuator: left_index + board: rp2040_left_arm + virtual_pin: 1 + physical_pin: 17 + min_value: null + max_value: null + enabled: false + - id: left_middle_pressure + type: pressure + associated_actuator: left_majeure + board: rp2040_left_arm + virtual_pin: 2 + physical_pin: 18 + min_value: null + max_value: null + enabled: false + - id: left_ring_pressure + type: pressure + associated_actuator: left_ring + board: rp2040_left_arm + virtual_pin: 3 + physical_pin: 19 + min_value: null + max_value: null + enabled: false + - id: left_pinky_pressure + type: pressure + associated_actuator: left_pinky + board: rp2040_left_arm + virtual_pin: 4 + physical_pin: 20 + min_value: null + max_value: null + enabled: false + - id: right_thumb_pressure + type: pressure + associated_actuator: right_thumb + board: rp2040_right_arm + virtual_pin: 0 + physical_pin: 16 + min_value: null + max_value: null + enabled: false + - id: right_index_pressure + type: pressure + associated_actuator: right_index + board: rp2040_right_arm + virtual_pin: 1 + physical_pin: 17 + min_value: null + max_value: null + enabled: false + - id: right_middle_pressure + type: pressure + associated_actuator: right_majeure + board: rp2040_right_arm + virtual_pin: 2 + physical_pin: 18 + min_value: null + max_value: null + enabled: false + - id: right_ring_pressure + type: pressure + associated_actuator: right_ring + board: rp2040_right_arm + virtual_pin: 3 + physical_pin: 19 + min_value: null + max_value: null + enabled: false + - id: right_pinky_pressure + type: pressure + associated_actuator: right_pinky + board: rp2040_right_arm + virtual_pin: 4 + physical_pin: 20 + min_value: null + max_value: null + enabled: false diff --git a/description/robot_description/urdf/robot_description.urdf.xacro b/description/robot_description/urdf/robot_description.urdf.xacro index 92bf939..3981c0f 100644 --- a/description/robot_description/urdf/robot_description.urdf.xacro +++ b/description/robot_description/urdf/robot_description.urdf.xacro @@ -1,6 +1,12 @@ + + + + + + @@ -408,7 +414,7 @@ - + @@ -430,6 +436,12 @@ + + + + + + diff --git a/description/ros2_control/inmoov_ros2_control.xacro b/description/ros2_control/inmoov_ros2_control.xacro index b1b4340..1cd1378 100644 --- a/description/ros2_control/inmoov_ros2_control.xacro +++ b/description/ros2_control/inmoov_ros2_control.xacro @@ -1,8 +1,9 @@ + - + gz_ros2_control/GazeboSimSystem @@ -13,49 +14,111 @@ lucy_hardware_interface_left_arm - - - - - - - - + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 135.0 + 2 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 90.0 + 3 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + 4 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 - - 0 - 1 - + 5 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + + 6 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + 7 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 + 8 + 300 + 0.0 + 1 + 1.0 + 0.0 + 150.0 + 50.0 @@ -71,52 +134,150 @@ lucy_hardware_interface_right_arm - - - - - - - - + 0 + 270 + 0.0 + 1 + 1.0 + 155.0 + 165.0 + 160.0 + + 1 + 270 + 0.0 + 1 + 1.0 + 140.0 + 180.0 + 180.0 + 2 + 270 + 0.0 + 1 + 1.0 + 100.0 + 110.0 + 100.0 + 3 + 300 + 0.0 + 1 + 1.0 + 0.0 + 230.0 + 150.0 + 4 + 300 + 0.0 + 1 + 1.0 + 36.0 + 150.0 + 36.0 - - 0 - 1 - + 5 + 300 + 0.0 + 1 + 1.0 + 16.0 + 150.0 + 16.0 + + 6 + 300 + 0.0 + 1 + 1.0 + 21.0 + 150.0 + 21.0 + 7 + 300 + 0.0 + 1 + 1.0 + 33.0 + 150.0 + 33.0 + 8 + 300 + 0.0 + 1 + 1.0 + 30.0 + 150.0 + 30.0 + + + + + + + + gz_ros2_control/GazeboSimSystem + + + lucy_ros2_control/LucySystemHardware + actuators/torso + lucy_hardware_interface_torso_head + + + + 0 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 + + + + + 1 + 270 + 0.0 + 1 + 1.0 + 0.0 + 270.0 + 0.0 - + \ No newline at end of file diff --git a/doc/DEVELOPER.md b/doc/DEVELOPER.md deleted file mode 100644 index 4d774a2..0000000 --- a/doc/DEVELOPER.md +++ /dev/null @@ -1,126 +0,0 @@ -# Developer guide — `thais_urdf` - -ROS 2 **Humble**. For **contributors** who change URDF/xacro, meshes, RViz config, or **launch files** in the **`thais_urdf`** package (repository root = oneament package named `thais_urdf`). - -**Runtime bringup / hardware plugin / cameras**: `../lucy_ros_packages/doc/DEVELOPER.md` when both repos live under the same workspace `src/`. On GitHub: [Sentience-Robotics/lucy_ros_packages](https://github.com/Sentience-Robotics/lucy_ros_packages) → `doc/DEVELOPER.md`. - ---- - -## 1. Role of this repository - -| Concern | Owned here | -|---------|------------| -| Robot **description** (xacro, meshes, limits) | `inmoov/` | -| **ros2_control** blocks (real vs Gazebo plugins) | `inmoov/ros2_control/` | -| **RViz** saved layout | `config/inmoov_rviz.rviz` | -| **Combo launches** (RViz and/or GZ Sim + rosbridge + control) | `launch/` | - -Controller **parameters** (`joint_trajectory_controller`, etc.) live in **`lucy_ros_packages`** → `lucy_ros2_control/config/lucy_controllers.yaml`. Launches pass `controller_config` into xacro; keep YAML and xacro **joint lists identical**. - ---- - -## 2. Layout - -```text -thais_urdf/ -├── doc/DEVELOPER.md -├── package.xml -├── CMakeLists.txt -├── launch/ -│ ├── rviz.launch.py -│ ├── gazebo.launch.py -│ └── rviz_standalone.launch.py -├── config/ -│ └── inmoov_rviz.rviz -└── inmoov/ - ├── urdf/inmoov.urdf.xacro - ├── 3dmodel/robot_description.urdf.xacro - ├── ros2_control/ - │ ├── inmoov_ros2_control.xacro - │ └── inmoov_gazebo.xacro - └── meshes/ -``` - ---- - -## 3. Build and install (`CMakeLists.txt`) - -**`launch/`**, **`config/`**, and **`inmoov/`** are installed to `share/thais_urdf/`. Defaults for `urdf_path` and `base_path` in **`rviz.launch.py`** / **`gazebo.launch.py`** come from **`get_package_share_directory("thais_urdf")`** → `.../inmoov` and `.../inmoov/urdf/inmoov.urdf.xacro`. - -**`lucy_ros2_control`** **`control.launch.py`** uses the same share paths and **`exec_depend`s `thais_urdf`**. This package does **not** declare **`exec_depend` `lucy_ros2_control`** (that would create **lucy_ros2_control ↔ thais_urdf** cycle for `colcon`). Combo launches still **require** `lucy_ros2_control` at runtime for the default `lucy_controllers.yaml` path. - ---- - -## 4. Xacro and ros2_control - -| File | Role | -|------|------| -| `inmoov/urdf/inmoov.urdf.xacro` | Top-level arguments: `base_path`, `use_gazebo_sim`, `controller_config`; includes description + ros2_control (+ Gazebo when sim). | -| `inmoov/3dmodel/robot_description.urdf.xacro` | Links, joints, mesh paths. | -| `inmoov/ros2_control/inmoov_ros2_control.xacro` | Declares hardware/sim systems (e.g. `LucySystemHardware` vs `gz_ros2_control/GazeboSimSystem`). | -| `inmoov/ros2_control/inmoov_gazebo.xacro` | Gazebo spawn / plugin glue for sim. | - -Editing joint names or interfaces here requires matching **`lucy_controllers.yaml`** and any teleop/UI joint order (see lucy_ros_packages developer doc). - ---- - -## 5. Launch files (developer notes) - -| Launch | Stack | -|--------|--------| -| `rviz.launch.py` | Real-oriented: `robot_state_publisher`, delayed `ros2_control_node`, spawners, rosbridge, RViz (`use_sim_time: false`). | -| `gazebo.launch.py` | GZ Sim, `/clock` bridge, spawn, `gz_ros2_control` plugin path, rosbridge, RViz (`use_sim_time: true` where applicable). | - -**Arguments** (both): `urdf_path`, `base_path` — defaults come from **`get_package_share_directory("thais_urdf")`** (installed `inmoov/`). - -**Sim time**: every node in a sim session must agree with `/clock`; if you add nodes to `gazebo.launch.py`, set `use_sim_time` consistently (see ROS 2 sim time tutorial). - ---- - -## 6. RViz - -- Display config: `config/inmoov_rviz.rviz` — pin RViz version expectations in release notes if visuals break across upgrades. -- Typical chain: `joint_state_broadcaster` → `/joint_states` → `robot_state_publisher` → TF → RobotModel. - ---- - -## 7. Extension checklist (`thais_urdf`) - -1. **New link/joint/mesh** → update xacro and collision/visual paths; run `xacro`/`check_urdf` locally. -2. **New ros2_control joint** → edit `inmoov_ros2_control.xacro` **and** `lucy_controllers.yaml` (sibling repo) together. -3. **New launch argument** → document in root `README.md` and here if it affects integrators. -4. **Licensing** → InMoov-derived assets: keep `LICENSE` / attribution accurate when adding meshes. - ---- - -## 8. Quick commands - -```bash -ros2 launch thais_urdf rviz.launch.py -ros2 launch thais_urdf gazebo.launch.py -# Optional: urdf_path:=... base_path:=... -``` - ---- - -## 9. CI - -Triggers: **pull_request**, and **push** to **`main` / `master` / `dev`** only. - -`.github/workflows/ci.yml` uses a **colcon workspace** next to the checkout (not under it), runs **`rosdep install`**, **`colcon build --packages-select thais_urdf`**, **`colcon test`**, then **`pytest-cov`** on `thais_urdf` tests (XML/HTML under `colcon_ws/build/coverage_reports/`, **Codecov** flag `thais_urdf`, optional **`CODECOV_TOKEN`**). - -Local commands: see **README.md** → *Tests and coverage (local)*. - ---- - -## 10. Contributing - -The repository root **`CONTRIBUTING.md`** must include the **exact GPLv3 “contributing” wording** expected by **`ament_copyright`** (see that file’s first paragraph), plus **`LICENSE`** at the root. - -Copyright **2025 Sentience Robotics Team**; same **GPL-3.0** license as the package (see `LICENSE`). - -- Open issues and merge requests on the host for this repository. -- Match ROS 2 / ament style; run `colcon test --packages-select thais_urdf` before submitting. -- Update **README.md** or this guide when behavior or layout changes. - -See **`CODE_OF_CONDUCT.md`** at the repository root. diff --git a/docs/DEVELOPER.md b/docs/DEVELOPER.md new file mode 100644 index 0000000..b64526e --- /dev/null +++ b/docs/DEVELOPER.md @@ -0,0 +1,140 @@ +# Developer guide — `thais_urdf` + +ROS 2 **Humble**. For **contributors** who change URDF/xacro, meshes, RViz config, **hardware YAML**, or **launch files** in the **`thais_urdf`** package (repository root = one ament package named `thais_urdf`). + +**Runtime bringup / hardware plugin / cameras**: `../lucy_ros_packages/docs/DEVELOPER.md` when both repos live under the same workspace `src/`. On GitHub: [Sentience-Robotics/lucy_ros_packages](https://github.com/Sentience-Robotics/lucy_ros_packages) → `docs/DEVELOPER.md`. + +--- + +## 1. Role of this repository + +| Concern | Owned here | +|---------|------------| +| Robot **description** (xacro, meshes, limits) | `description/` | +| **ros2_control** blocks (real vs Gazebo plugins) | `description/ros2_control/` | +| **Hardware mapping** (boards, actuators, sensors) | `config/hardware/active.yaml` — see [hardware_mapping.md](hardware_mapping.md) | +| **Controller parameters** for this package’s default bringup | `config/controllers.yaml` (joint lists must stay aligned with ros2_control / URDF) | +| **RViz** saved layout | `config/inmoov_rviz.rviz` | +| **Launches** (RViz and/or GZ Sim + rosbridge + control) | `launch/` | + +For **InMoov i2–style** head and expression actuators not present in the current i1 URDF, see [inmoov_i2.md](inmoov_i2.md). + +If you use **`lucy_ros_packages`** for Jetson bringup, keep **`lucy_controllers.yaml`** (or equivalent) in sync with **`config/controllers.yaml`** when joint sets overlap. + +--- + +## 2. Layout + +```text +thais_urdf/ +├── docs/ +│ ├── DEVELOPER.md +│ ├── hardware_mapping.md +│ └── inmoov_i2.md +├── package.xml +├── CMakeLists.txt +├── launch/ +│ ├── control.launch.py +│ ├── rviz.launch.py +│ ├── gazebo.launch.py +│ └── rviz_standalone.launch.py +├── config/ +│ ├── controllers.yaml +│ ├── inmoov_rviz.rviz +│ └── hardware/ +│ ├── active.yaml +│ └── configs/ +├── description/ +│ ├── urdf/inmoov.urdf.xacro +│ ├── robot_description/ +│ ├── ros2_control/ +│ │ ├── inmoov_ros2_control.xacro +│ │ └── inmoov_gazebo.xacro +│ └── meshes/ +``` + +--- + +## 3. Build and install (`CMakeLists.txt`) + +**`launch/`**, **`config/`**, **`description/`**, and **`docs/`** install to `share/thais_urdf/`. Defaults for `urdf_path` and `base_path` in **`rviz.launch.py`** / **`gazebo.launch.py`** / **`control.launch.py`** come from **`get_package_share_directory("thais_urdf")`** → `.../description` and `.../description/urdf/inmoov.urdf.xacro`. + +**`lucy_ros2_control`** may consume this URDF when both packages are in one workspace. This package does **not** declare **`exec_depend` `lucy_ros2_control`** (avoids a **lucy_ros2_control ↔ thais_urdf** cycle for `colcon`). Combo launches still **require** `lucy_ros2_control` at runtime if you use its default controller YAML path. + +--- + +## 4. Xacro and ros2_control + +| File | Role | +|------|------| +| `description/urdf/inmoov.urdf.xacro` | Top-level arguments: `base_path`, `use_gazebo_sim`, `controller_config`; includes description + ros2_control (+ Gazebo when sim). | +| `description/robot_description/...` | Links, joints, mesh paths (included from top-level xacro). | +| `description/ros2_control/inmoov_ros2_control.xacro` | Declares hardware/sim systems (e.g. `LucySystemHardware` vs `gz_ros2_control/GazeboSimSystem`). | +| `description/ros2_control/inmoov_gazebo.xacro` | Gazebo spawn / plugin glue for sim. | + +Editing joint names or interfaces here requires matching **`config/controllers.yaml`**, **`config/hardware/active.yaml`**, and any teleop/UI joint order (see lucy_ros_packages developer doc). + +--- + +## 5. Launch files (developer notes) + +| Launch | Stack | +|--------|-------| +| `control.launch.py` | ros2_control node + spawners; default `controller_config` from this package’s share. | +| `rviz.launch.py` | Real-oriented: `robot_state_publisher`, delayed `ros2_control_node`, spawners, rosbridge, RViz (`use_sim_time: false`). | +| `gazebo.launch.py` | GZ Sim, `/clock` bridge, spawn, `gz_ros2_control` plugin path, rosbridge, RViz (`use_sim_time: true` where applicable). | + +**Arguments** (URDF launches): `urdf_path`, `base_path` — defaults come from **`get_package_share_directory("thais_urdf")`** (installed `description/`). + +**Sim time**: every node in a sim session must agree with `/clock`; if you add nodes to `gazebo.launch.py`, set `use_sim_time` consistently (see ROS 2 sim time tutorial). + +--- + +## 6. RViz + +- Display config: `config/inmoov_rviz.rviz` — pin RViz version expectations in release notes if visuals break across upgrades. +- Typical chain: `joint_state_broadcaster` → `/joint_states` → `robot_state_publisher` → TF → RobotModel. + +--- + +## 7. Extension checklist (`thais_urdf`) + +1. **New link/joint/mesh** → update xacro and collision/visual paths; run `xacro`/`check_urdf` locally. +2. **New ros2_control joint** → edit `inmoov_ros2_control.xacro`, **`config/hardware/active.yaml`**, and **`config/controllers.yaml`** together (and firmware when the generator is wired). +3. **New launch argument** → document in root `README.md` and here if it affects integrators. +4. **Licensing** → InMoov-derived assets: keep `LICENSE` / attribution accurate when adding meshes. + +--- + +## 8. Quick commands + +```bash +ros2 launch thais_urdf rviz.launch.py +ros2 launch thais_urdf gazebo.launch.py +ros2 launch thais_urdf control.launch.py +# Optional: urdf_path:=... base_path:=... +``` + +--- + +## 9. CI + +Triggers: **pull_request**, and **push** to **`main` / `master` / `dev`** only. + +`.github/workflows/ci.yml` uses a **colcon workspace** next to the checkout (not under it), runs **`rosdep install`**, **`colcon build --packages-select thais_urdf`**, **`colcon test`**, then **`pytest-cov`** on `thais_urdf` tests (XML/HTML under `colcon_ws/build/coverage_reports/`, **Codecov** flag `thais_urdf`, optional **`CODECOV_TOKEN`**). + +Local commands: see **README.md** → *Tests and coverage (local)*. + +--- + +## 10. Contributing + +The repository root **`CONTRIBUTING.md`** must include the **exact GPLv3 “contributing” wording** expected by **`ament_copyright`** (see that file’s first paragraph), plus **`LICENSE`** at the root. + +Copyright **2025 Sentience Robotics Team**; same **GPL-3.0** license as the package (see `LICENSE`). + +- Open issues and merge requests on the host for this repository. +- Match ROS 2 / ament style; run `colcon test --packages-select thais_urdf` before submitting. +- Update **README.md** or this guide when behavior or layout changes. + +See **`CODE_OF_CONDUCT.md`** at the repository root. diff --git a/docs/hardware_mapping.md b/docs/hardware_mapping.md new file mode 100644 index 0000000..a696100 --- /dev/null +++ b/docs/hardware_mapping.md @@ -0,0 +1,54 @@ +# Hardware mapping (YAML) + +`config/hardware/active.yaml` is the **single source of truth** for Lucy hardware: RP2040 boards, actuators, and finger pressure sensors. The **`lucy_config_generator`** package (in `lucy_ros_packages`) reads this file and emits firmware C, `description/ros2_control/inmoov_ros2_control.xacro`, and `config/controllers.yaml`. + +For **InMoov i1 vs i2** scope and how to extend the YAML with **head / expression** actuators when the URDF evolves, see [inmoov_i2.md](inmoov_i2.md). + +## Layout + +| Path | Role | +|------|------| +| `config/hardware/active.yaml` | Active mapping used for generation and validation | +| `config/hardware/active_meta.yaml` | `{ name, activated_at }` for the active slot | +| `config/hardware/configs/*.yaml` | Named presets (e.g. `default.yaml` mirrors the last activated snapshot) | +| `config/hardware/backups/` | Automatic backups before activation | + +## Top-level keys + +- **`version`**: schema version (integer, currently `1`). +- **`robot_name`**: logical name (e.g. `thais`). +- **`firmware`**: `source_dir`, `build_dir` — paths relative to the micro-ROS firmware workspace for the pipeline. +- **`controller_manager`**: e.g. `update_rate`. +- **`boards`**: ordered map of board id → `serial_id` (optional USB serial for `picotool --ser`, alphanumeric or empty), **`board_class`** (`internal_servo_only` \| `internal_servo_i2c_pwm`), **`internal_servo_slots`** (max valid `physical_pin` for actuators on that board), firmware target, compile definition, micro-ROS actuator/sensor topics, and `controller` (`name`, `type`). **Order** of keys is the order of generated ros2_control blocks and controller sections. **Board id** is also the firmware C basename: `config_.c`. **No `/dev/ttyACM*`** here — serial devices are launch-time (`lucy_bringup` args), not committed hardware truth. +- **`actuators`**: list of actuators with `urdf_joint` (must match URDF), `board`, `virtual_pin` (contiguous per board, used for firmware ordering and for `JointState.position` indices on that board’s actuator topic), `physical_pin` (**1..`boards..internal_servo_slots`**: the `N` in `INTERNAL_SERVO_N` emitted in firmware C — not GPIO index and not “+1” from another convention), `servo_type`, calibration (`offset_deg`, `direction`, `scale`), limits, `enabled` (if `false`, the row is still listed in generated `ros2_control` and trajectory controllers, but omitted from firmware C so the servo is not driven on the Pico). +- **`sensors`**: finger pressure sensors; `associated_actuator` references an actuator `id`. + +## Hardware angle limits (`servo_*_deg`) + +Values sent to real servos are expressed in **degrees in the servo’s own range**. This is independent of how the URDF might visualize the joint (often symmetric angles); the mapping from **URDF / ros2_control command** to **hardware command** is what `offset_deg`, `direction`, and `scale` are for (see below). + +## `offset_deg`, `direction`, and `scale` (URDF command → hardware) + +These fields tune how a **trajectory command in joint space** (what `joint_trajectory_controller` and the URDF use for that `urdf_joint`) is converted to the **angle actually sent to the servo** (within `[servo_min_deg, servo_max_deg]`): + +- **`direction`**: `+1` or `-1` to flip motion if the horn is mounted opposite to the positive URDF axis. +- **`scale`**: ratio between a change in **commanded joint angle** and a change in **servo angle** when the mechanism is not 1:1 (gear reduction, non-linear linkage approximated as linear, etc.). +- **`offset_deg`**: constant shift after direction/scale so that “zero” in the controller matches the real neutral pose for that hardware assembly. + +So they are **calibration between ros2_control / URDF joint values and real actuator commands**, not a separate “RViz-only” layer. Gazebo (ideal model) may ignore them unless the sim stack is extended to mimic the same mapping; the **LucySystemHardware** plugin and firmware are the consumers of these parameters once generated into ros2_control. + +## Shoulder Y joints + +`left_shoulder_y_link_joint` and `right_shoulder_y_link_joint` are **torso** joints (servos on `rp2040_torso_head`). Legacy URDF names still say “shoulder”; the YAML assigns them to the torso board so generated ros2_control and firmware stay consistent. + +Older InMoov-style actuator tables often name torso PCA pins **5** and **6** as `left_shoulder_pitch_joint` / `right_shoulder_pitch_joint` (legacy naming). This repository’s YAML maps those **same pins** to URDF **`left_shoulder_y_link_joint` / `right_shoulder_y_link_joint`** (Lucy wiring / model). + +## Head and expression actuators + +Extra head DOFs (eyelids, cheeks, separate eyeball drivers, and so on) are **not** listed in `active.yaml` until the URDF exports matching joint names and each row is validated on hardware. See [inmoov_i2.md](inmoov_i2.md) for a YAML appendix for a future **InMoov i2**–style extension. + +## Editing + +1. Edit `config/hardware/active.yaml` (or a copy under `configs/`). +2. Run validation (`colcon test --packages-select thais_urdf`) and the generator when implemented. +3. Re-commit generated artifacts (`description/ros2_control/inmoov_ros2_control.xacro`, `config/controllers.yaml`) after regeneration. diff --git a/docs/inmoov_i2.md b/docs/inmoov_i2.md new file mode 100644 index 0000000..c3ff511 --- /dev/null +++ b/docs/inmoov_i2.md @@ -0,0 +1,275 @@ +# InMoov i1 vs i2 (this package) + +## What this repository implements today + +**`thais_urdf`** ships an **InMoov i1–line** robot description under `description/` (meshes, joints, ros2_control xacro). The hardware YAML in **`config/hardware/active.yaml`** therefore lists only actuators and sensors that match **that** URDF and the current Lucy wiring (arms, torso shoulder Y, fingers, pressure sensors, and so on). + +It does **not** list **InMoov i2–style** head and expression actuators (extra eyelids, cheeks, brows, separate eyeball drivers, and so on) because those joints are either absent from the processed URDF or not yet committed to a single wiring story in this repo. + +## Why keep talking about i2? + +A future evolution might split or extend into an **`inmoov_i2`** description package with additional head DOFs. When that happens, the same **YAML schema** as `active.yaml` still applies: each new servo needs a row under **`actuators:`** with a **`urdf_joint`** that exactly matches the joint name in **`robot_description`**, plus board, pins, calibration, and limits. + +This document is the **bridge**: it explains that gap and gives a **copy-paste-oriented appendix** in YAML form. Treat every **`urdf_joint`** in the appendix as **something you must reconcile** with the actual i2 (or extended i1) URDF—some entries use **current i1** head joint names where they exist; others use **placeholder** names for expression axes that the legacy xacro named differently from any joint in today’s URDF. + +## How to add i2 head actuators to the YAML + +1. **Freeze URDF joint names** in the description package (each DOF exactly one `joint name="..."` in the generated URDF). +2. For each servo on **`rp2040_torso_head`**, append one list element to **`actuators:`** in `active.yaml` (or a preset under `configs/`). +3. Assign **`virtual_pin`** as **contiguous integers per board**, starting after the last existing actuator on that board (today, shoulder Y uses `0` and `1` on `rp2040_torso_head`). +4. Set **`physical_pin`** to the PCA9685 channel (or equivalent) from your **actual** schematic. The appendix below uses **example** channels (jaw on **7**, cheeks **8–9**, and so on through **22**) typical of full InMoov-style head wiring. Many draft tables also put “head roll” on **physical_pin 6**, which **collides** with Lucy’s use of pins **5–6** for shoulder Y on `rp2040_torso_head`—treat the appendix as a **template**, then renumber every `physical_pin` to match the board you build. +5. Tune **`servo_*_deg`**, **`offset_deg`**, **`direction`**, and **`scale`** on the bench once the joint exists in ros2_control. + +## Appendix — example YAML: i2-oriented head / expression actuators + +Paste the list items at the end of the top-level **`actuators:`** sequence in `active.yaml` (or merge a preset) once the URDF exposes matching joints and each **`physical_pin`** is verified on the PCA. + +**Pin conflict note:** If you follow common InMoov-style numbering, “head roll” often lands on **`physical_pin: 6`**, which collides with Lucy’s **`right_shoulder_y_link_joint`** on the same board (also pin **6**). The first row below uses **`physical_pin: 23`** as a stand-in for “move head roll to a free channel”; change it to match your PCB. + +**`virtual_pin`:** Continue after the last actuator on `rp2040_torso_head` (today **`0`** and **`1`** for shoulder Y). The appendix uses **`2` … `18`**. + +**`urdf_joint`:** Rows with **`i02.head.*`** are placeholders until the i2 (or extended) URDF defines those joints. Rows with **`i01.head.*`** match joint names present in the current `description/robot_description` tree. + +```yaml +# Append under the top-level key `actuators:` (not as a nested document). + - id: i2_head_roll_neck + urdf_joint: i01.head.rollNeck_link_joint + board: rp2040_torso_head + virtual_pin: 2 + physical_pin: 23 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 270.0 + servo_default_deg: 135.0 + enabled: false + + - id: i2_jaw + urdf_joint: i01.head.jaw_link_joint + board: rp2040_torso_head + virtual_pin: 3 + physical_pin: 7 + servo_type: "300" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 150.0 + servo_default_deg: 75.0 + enabled: false + + - id: i2_left_cheek + urdf_joint: i02.head.left_cheek_link_joint + board: rp2040_torso_head + virtual_pin: 4 + physical_pin: 8 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_right_cheek + urdf_joint: i02.head.right_cheek_link_joint + board: rp2040_torso_head + virtual_pin: 5 + physical_pin: 9 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_upper_lip + urdf_joint: i02.head.upper_lip_link_joint + board: rp2040_torso_head + virtual_pin: 6 + physical_pin: 10 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_upper_left_eyelid + urdf_joint: i02.head.upper_left_eyelid_link_joint + board: rp2040_torso_head + virtual_pin: 7 + physical_pin: 11 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_upper_right_eyelid + urdf_joint: i02.head.upper_right_eyelid_link_joint + board: rp2040_torso_head + virtual_pin: 8 + physical_pin: 12 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_lower_left_eyelid + urdf_joint: i02.head.lower_left_eyelid_link_joint + board: rp2040_torso_head + virtual_pin: 9 + physical_pin: 13 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_lower_right_eyelid + urdf_joint: i02.head.lower_right_eyelid_link_joint + board: rp2040_torso_head + virtual_pin: 10 + physical_pin: 14 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_left_eye_horizontal + urdf_joint: i02.head.left_eye_horizontal_link_joint + board: rp2040_torso_head + virtual_pin: 11 + physical_pin: 15 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_left_eye_vertical + urdf_joint: i02.head.left_eye_vertical_link_joint + board: rp2040_torso_head + virtual_pin: 12 + physical_pin: 16 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_right_eye_horizontal + urdf_joint: i02.head.right_eye_horizontal_link_joint + board: rp2040_torso_head + virtual_pin: 13 + physical_pin: 17 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_right_eye_vertical + urdf_joint: i02.head.right_eye_vertical_link_joint + board: rp2040_torso_head + virtual_pin: 14 + physical_pin: 18 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_left_eyebrow + urdf_joint: i02.head.left_eyebrow_link_joint + board: rp2040_torso_head + virtual_pin: 15 + physical_pin: 19 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_right_eyebrow + urdf_joint: i02.head.right_eyebrow_link_joint + board: rp2040_torso_head + virtual_pin: 16 + physical_pin: 20 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_left_forehead + urdf_joint: i02.head.left_forehead_link_joint + board: rp2040_torso_head + virtual_pin: 17 + physical_pin: 21 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false + + - id: i2_right_forehead + urdf_joint: i02.head.right_forehead_link_joint + board: rp2040_torso_head + virtual_pin: 18 + physical_pin: 22 + servo_type: "180" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 180.0 + servo_default_deg: 90.0 + enabled: false +``` + +**Optional i1 head joints** not covered by the legacy expression block (add only if your URDF keeps them and you have servos): `i01.head.rothead_link_joint`, `i01.head.neck.001_link_joint`, `i01.head.eyeLeft_link_joint`, `i01.head.eyeRight_link_joint`, and the `*.001_link_joint` eye variants—each needs its own free **`physical_pin`** and next **`virtual_pin`**. + diff --git a/hardware_config/inmoov_actuators_config.xacro b/hardware_config/inmoov_actuators_config.xacro deleted file mode 100644 index c56e695..0000000 --- a/hardware_config/inmoov_actuators_config.xacro +++ /dev/null @@ -1,556 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/hardware_config/inmoov_hardware_config.xacro b/hardware_config/inmoov_hardware_config.xacro deleted file mode 100644 index 2a35fac..0000000 --- a/hardware_config/inmoov_hardware_config.xacro +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - \ No newline at end of file diff --git a/hardware_config/inmoov_sensors_config.xacro b/hardware_config/inmoov_sensors_config.xacro deleted file mode 100644 index 74f5848..0000000 --- a/hardware_config/inmoov_sensors_config.xacro +++ /dev/null @@ -1,131 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/control.launch.py b/launch/control.launch.py new file mode 100644 index 0000000..8c93276 --- /dev/null +++ b/launch/control.launch.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python3 +# Copyright 2025 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +# Real robot: robot_state_publisher + ros2_control_node + spawners. +# Controllers live in this package (config/controllers.yaml). + +from pathlib import Path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, TimerAction +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import Node +import yaml + + +def _controllers_to_spawn(controllers_yaml_path: Path) -> list[str]: + """Return controller names declared under controller_manager.ros__parameters.""" + data = yaml.safe_load(controllers_yaml_path.read_text(encoding="utf-8")) or {} + cm_params = data.get("controller_manager", {}).get("ros__parameters", {}) + if not isinstance(cm_params, dict): + return [] + return [name for name in cm_params.keys() if name != "update_rate"] + + +def _load_launch_defaults(package_root: Path) -> dict[str, str]: + """Load launch path defaults from config/control.launch.yaml.""" + config_path = package_root / "config" / "control.launch.yaml" + data = yaml.safe_load(config_path.read_text(encoding="utf-8")) or {} + if not isinstance(data, dict): + raise RuntimeError(f"Invalid launch defaults file: {config_path}") + required = ("urdf_path", "base_path", "controllers_yaml") + missing = [key for key in required if key not in data or not str(data[key]).strip()] + if missing: + raise RuntimeError(f"Missing keys in {config_path}: {missing}") + return {key: str(data[key]).strip() for key in required} + + +def generate_launch_description(): + package_root = Path(__file__).resolve().parents[1] + defaults = _load_launch_defaults(package_root) + default_urdf = str((package_root / defaults["urdf_path"]).resolve()) + default_base = str((package_root / defaults["base_path"]).resolve()) + default_controllers_yaml = str((package_root / defaults["controllers_yaml"]).resolve()) + controllers_yaml_path = Path(default_controllers_yaml) + controller_names = _controllers_to_spawn(controllers_yaml_path) + + urdf_path_arg = DeclareLaunchArgument( + "urdf_path", + default_value=default_urdf, + description="Absolute path to robot URDF xacro", + ) + base_path_arg = DeclareLaunchArgument( + "base_path", + default_value=default_base, + description="Base path for xacro (mesh_dir)", + ) + controllers_yaml_arg = DeclareLaunchArgument( + "controllers_yaml", + default_value=default_controllers_yaml, + description="Absolute path to controller_manager YAML config", + ) + urdf_path = LaunchConfiguration("urdf_path") + base_path = LaunchConfiguration("base_path") + controllers_yaml = LaunchConfiguration("controllers_yaml") + + robot_description = Command(["xacro ", urdf_path, " base_path:=", base_path]) + robot_description_dict = {"robot_description": robot_description} + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="screen", + parameters=[robot_description_dict], + ) + ros2_control_node = TimerAction( + period=2.0, + actions=[ + Node( + package="controller_manager", + executable="ros2_control_node", + output="screen", + parameters=[controllers_yaml], + remappings=[("~/robot_description", "/robot_description")], + ) + ], + ) + spawners = [ + Node( + package="controller_manager", + executable="spawner", + arguments=[controller], + output="screen", + ) + for controller in controller_names + ] + + return LaunchDescription([ + urdf_path_arg, base_path_arg, controllers_yaml_arg, + robot_state_publisher, ros2_control_node, + *spawners, + ]) diff --git a/launch/gazebo.launch.py b/launch/gazebo.launch.py index 44c961f..f534980 100644 --- a/launch/gazebo.launch.py +++ b/launch/gazebo.launch.py @@ -51,8 +51,7 @@ def generate_launch_description(): share = get_package_share_directory("thais_urdf") default_base = os.path.join(share, "description") default_urdf = os.path.join(default_base, "urdf", "inmoov.urdf.xacro") - lucy_share = get_package_share_directory("lucy_ros2_control") - controller_config_path = os.path.join(lucy_share, "config", "lucy_controllers.yaml") + controller_config_path = os.path.join(share, "config", "controllers.yaml") urdf_path_arg = DeclareLaunchArgument( "urdf_path", diff --git a/launch/rviz.launch.py b/launch/rviz.launch.py index e7af326..4fa362f 100644 --- a/launch/rviz.launch.py +++ b/launch/rviz.launch.py @@ -14,39 +14,22 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . -# Real robot + RViz + rosbridge. Control panel at ws://localhost:9090. +# Real robot + RViz + rosbridge. Control stack comes from control.launch.py. +from pathlib import Path -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction -from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution +from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): - share = get_package_share_directory("thais_urdf") - default_base = os.path.join(share, "description") - default_urdf = os.path.join(default_base, "urdf", "inmoov.urdf.xacro") - urdf_path_arg = DeclareLaunchArgument( - "urdf_path", - default_value=default_urdf, - description="Path to inmoov.urdf.xacro", - ) - base_path_arg = DeclareLaunchArgument( - "base_path", - default_value=default_base, - description="Base path for xacro (mesh_dir)", + package_root = Path(__file__).resolve().parents[1] + control_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(str(package_root / "launch" / "control.launch.py")) ) - urdf_path = LaunchConfiguration("urdf_path") - base_path = LaunchConfiguration("base_path") - - lucy_share = get_package_share_directory("lucy_ros2_control") - controllers_yaml = os.path.join(lucy_share, "config", "lucy_controllers.yaml") - robot_description = Command(["xacro ", urdf_path, " base_path:=", base_path]) - robot_description_dict = {"robot_description": robot_description} rosbridge = ExecuteProcess( cmd=["ros2", "launch", "rosbridge_server", "rosbridge_websocket_launch.xml"], @@ -54,42 +37,6 @@ def generate_launch_description(): shell=True, ) - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - output="screen", - parameters=[robot_description_dict], - ) - ros2_control_node = TimerAction( - period=2.0, - actions=[ - Node( - package="controller_manager", - executable="ros2_control_node", - output="screen", - parameters=[controllers_yaml, robot_description_dict], - ) - ], - ) - spawn_joint_state = Node( - package="controller_manager", - executable="spawner", - arguments=["joint_state_broadcaster"], - output="screen", - ) - spawn_left = Node( - package="controller_manager", - executable="spawner", - arguments=["left_arm_controller"], - output="screen", - ) - spawn_right = Node( - package="controller_manager", - executable="spawner", - arguments=["right_arm_controller"], - output="screen", - ) - rviz_config = PathJoinSubstitution([ FindPackageShare("thais_urdf"), "config", "inmoov_rviz.rviz", ]) @@ -103,9 +50,7 @@ def generate_launch_description(): ) return LaunchDescription([ - urdf_path_arg, base_path_arg, + control_launch, rosbridge, - robot_state_publisher, ros2_control_node, - spawn_joint_state, spawn_left, spawn_right, rviz, ]) diff --git a/package.xml b/package.xml index 299cccd..20a58eb 100644 --- a/package.xml +++ b/package.xml @@ -16,7 +16,7 @@ ros_gz_bridge robot_state_publisher gz_ros2_control - + rosbridge_server controller_manager @@ -25,6 +25,7 @@ ament_cmake_pytest python3-pytest xacro + python3-yaml ament_cmake diff --git a/test/fixtures/invalid_duplicate_vpin.yaml b/test/fixtures/invalid_duplicate_vpin.yaml new file mode 100644 index 0000000..2bf1519 --- /dev/null +++ b/test/fixtures/invalid_duplicate_vpin.yaml @@ -0,0 +1,42 @@ +version: 1 +robot_name: test +firmware: {source_dir: ".", build_dir: "build"} +controller_manager: {update_rate: 100} +boards: + rp2040_left_arm: + serial_id: "" + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: t + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: {name: left_arm_controller, type: joint_trajectory_controller/JointTrajectoryController} +actuators: + - id: a + urdf_joint: left_shoulder_z_link_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: -1.0 + servo_max_deg: 1.0 + servo_default_deg: 0.0 + enabled: false + - id: b + urdf_joint: left_shoulder_x_link_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 11 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: -1.0 + servo_max_deg: 1.0 + servo_default_deg: 0.0 + enabled: false +sensors: [] diff --git a/test/fixtures/invalid_missing_board.yaml b/test/fixtures/invalid_missing_board.yaml new file mode 100644 index 0000000..73a4309 --- /dev/null +++ b/test/fixtures/invalid_missing_board.yaml @@ -0,0 +1,29 @@ +version: 1 +robot_name: test +firmware: {source_dir: ".", build_dir: "build"} +controller_manager: {update_rate: 100} +boards: + rp2040_left_arm: + serial_id: "" + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: t + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: {name: left_arm_controller, type: joint_trajectory_controller/JointTrajectoryController} +actuators: + - id: a + urdf_joint: left_shoulder_z_link_joint + board: rp2040_unknown_board + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: -1.0 + servo_max_deg: 1.0 + servo_default_deg: 0.0 + enabled: false +sensors: [] diff --git a/test/fixtures/invalid_servo_default_out_of_range.yaml b/test/fixtures/invalid_servo_default_out_of_range.yaml new file mode 100644 index 0000000..7dad633 --- /dev/null +++ b/test/fixtures/invalid_servo_default_out_of_range.yaml @@ -0,0 +1,29 @@ +version: 1 +robot_name: test +firmware: {source_dir: ".", build_dir: "build"} +controller_manager: {update_rate: 100} +boards: + rp2040_left_arm: + serial_id: "" + board_class: internal_servo_only + internal_servo_slots: 18 + firmware_target: t + compile_definition: USE_LEFT_ARM + topic_actuators: actuators/left_arm + topic_sensors: sensors/left_arm + controller: {name: left_arm_controller, type: joint_trajectory_controller/JointTrajectoryController} +actuators: + - id: a + urdf_joint: left_shoulder_z_link_joint + board: rp2040_left_arm + virtual_pin: 0 + physical_pin: 10 + servo_type: "270" + offset_deg: 0.0 + direction: 1 + scale: 1.0 + servo_min_deg: 0.0 + servo_max_deg: 10.0 + servo_default_deg: 99.0 + enabled: false +sensors: [] diff --git a/test/test_hardware_yaml.py b/test/test_hardware_yaml.py new file mode 100644 index 0000000..e4ab0d5 --- /dev/null +++ b/test/test_hardware_yaml.py @@ -0,0 +1,271 @@ +# Copyright 2025 Sentience Robotics Team +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see . + +"""Structural validation for config/hardware/active.yaml (issue #95).""" + +from __future__ import annotations + +import re +import shutil +import subprocess +import xml.etree.ElementTree as ET +from pathlib import Path +from typing import Any + +import pytest +import yaml + +REQUIRED_ROOT = ( + "version", + "robot_name", + "firmware", + "controller_manager", + "boards", + "actuators", + "sensors", +) +REQUIRED_ACTUATOR = ( + "id", + "urdf_joint", + "board", + "virtual_pin", + "physical_pin", + "servo_type", + "offset_deg", + "direction", + "scale", + "servo_min_deg", + "servo_max_deg", + "servo_default_deg", + "enabled", +) +REQUIRED_SENSOR = ( + "id", + "type", + "associated_actuator", + "board", + "virtual_pin", + "physical_pin", + "min_value", + "max_value", + "enabled", +) +REQUIRED_BOARD = ( + "serial_id", + "board_class", + "internal_servo_slots", + "firmware_target", + "compile_definition", + "topic_actuators", + "topic_sensors", + "controller", +) + +BOARD_CLASSES = frozenset({"internal_servo_only", "internal_servo_i2c_pwm"}) +_BOARD_ID_RE = re.compile(r"^rp2040_[a-z][a-z0-9_]*$") +_TOPIC_RE = re.compile(r"^[a-z][a-z0-9_/]*$") +_SERIAL_ID_RE = re.compile(r"^[A-Za-z0-9]*$") + + +def _active_yaml_path() -> Path: + """Prefer the checkout under ``test/..`` so pytest validates edited YAML without reinstall.""" + src = Path(__file__).resolve().parents[1] / "config" / "hardware" / "active.yaml" + if src.is_file(): + return src + try: + from ament_index_python.packages import get_package_share_directory + + p = Path(get_package_share_directory("thais_urdf")) / "config" / "hardware" / "active.yaml" + if p.is_file(): + return p + except Exception: + pass + return src + + +def _urdf_xacro_path() -> Path: + try: + from ament_index_python.packages import get_package_share_directory + + share = Path(get_package_share_directory("thais_urdf")) + urdf = share / "description" / "urdf" / "inmoov.urdf.xacro" + if urdf.is_file(): + return urdf + except Exception: + pass + root = Path(__file__).resolve().parents[1] + return root / "description" / "urdf" / "inmoov.urdf.xacro" + + +def _controllers_yaml_path() -> Path: + try: + from ament_index_python.packages import get_package_share_directory + + p = Path(get_package_share_directory("thais_urdf")) / "config" / "controllers.yaml" + if p.is_file(): + return p + except Exception: + pass + return Path(__file__).resolve().parents[1] / "config" / "controllers.yaml" + + +def load_hardware_yaml(path: Path) -> dict[str, Any]: + with path.open(encoding="utf-8") as f: + return yaml.safe_load(f) + + +def validate_hardware_yaml(data: dict[str, Any], urdf_joints: set[str] | None = None) -> None: + for key in REQUIRED_ROOT: + if key not in data: + raise ValueError(f"missing root key: {key}") + if data["version"] != 1: + raise ValueError("version must be 1") + boards: dict[str, Any] = data["boards"] + if not isinstance(boards, dict) or not boards: + raise ValueError("boards must be a non-empty map") + + for bid, bdef in boards.items(): + for k in REQUIRED_BOARD: + if k not in bdef: + raise ValueError(f"board {bid}: missing {k}") + ctrl = bdef["controller"] + if "name" not in ctrl or "type" not in ctrl: + raise ValueError(f"board {bid}: controller needs name and type") + + if not _BOARD_ID_RE.fullmatch(bid): + raise ValueError( + f"board id {bid!r} must match {_BOARD_ID_RE.pattern} " + "(must stay aligned with lucy_config_generator derivation rules)" + ) + bc = bdef["board_class"] + if bc not in BOARD_CLASSES: + raise ValueError( + f"board {bid}: board_class must be one of {sorted(BOARD_CLASSES)}, got {bc!r}" + ) + sid = bdef["serial_id"] + if not isinstance(sid, str): + raise ValueError(f"board {bid}: serial_id must be a string") + if sid and not _SERIAL_ID_RE.fullmatch(sid): + raise ValueError( + f"board {bid}: serial_id must be empty or alphanumeric " + "(USB serial / picotool --ser)" + ) + for topic_key in ("topic_actuators", "topic_sensors"): + t = bdef[topic_key] + if not isinstance(t, str) or not t: + raise ValueError(f"board {bid}: {topic_key} must be a non-empty string") + if not _TOPIC_RE.fullmatch(t): + raise ValueError( + f"board {bid}: {topic_key} must match {_TOPIC_RE.pattern} (no leading slash)" + ) + + actuators: list[dict[str, Any]] = data["actuators"] + if not isinstance(actuators, list): + raise ValueError("actuators must be a list") + by_board: dict[str, list[dict[str, Any]]] = {} + for a in actuators: + for k in REQUIRED_ACTUATOR: + if k not in a: + raise ValueError(f"actuator {a.get('id')}: missing {k}") + b = a["board"] + if b not in boards: + raise ValueError(f"actuator {a['id']}: unknown board {b}") + by_board.setdefault(b, []).append(a) + + for board_id, lst in by_board.items(): + vpins = sorted(int(x["virtual_pin"]) for x in lst) + if len(vpins) != len(set(vpins)): + raise ValueError(f"board {board_id}: duplicate virtual_pin") + if vpins != list(range(len(vpins))): + raise ValueError( + f"board {board_id}: virtual_pin must be contiguous from 0..N-1, got {vpins}" + ) + for a in lst: + lo = a["servo_min_deg"] + hi = a["servo_max_deg"] + d = a["servo_default_deg"] + if lo is not None and hi is not None and d is not None: + if float(d) < float(lo) or float(d) > float(hi): + raise ValueError( + f"actuator {a['id']}: servo_default_deg out of [{lo}, {hi}]" + ) + + sensors: list[dict[str, Any]] = data["sensors"] + if not isinstance(sensors, list): + raise ValueError("sensors must be a list") + actuator_ids = {a["id"] for a in actuators} + for s in sensors: + for k in REQUIRED_SENSOR: + if k not in s: + raise ValueError(f"sensor {s.get('id')}: missing {k}") + if s["board"] not in boards: + raise ValueError(f"sensor {s['id']}: unknown board {s['board']}") + if s["associated_actuator"] not in actuator_ids: + raise ValueError( + f"sensor {s['id']}: associated_actuator {s['associated_actuator']} not found" + ) + + if urdf_joints is not None: + for a in actuators: + j = a["urdf_joint"] + if j not in urdf_joints: + raise ValueError(f"actuator {a['id']}: urdf_joint {j!r} not in URDF") + + +def urdf_joint_names_from_xacro() -> set[str]: + xacro = shutil.which("xacro") + if not xacro: + pytest.skip("xacro not on PATH") + urdf = _urdf_xacro_path() + ctrl = _controllers_yaml_path() + if not urdf.is_file(): + pytest.skip(f"missing {urdf}") + if not ctrl.is_file(): + pytest.skip(f"missing {ctrl}") + base = urdf.parent.parent + cmd = [ + xacro, + str(urdf), + f"base_path:={base}", + f"controller_config:={ctrl}", + "use_gazebo_sim:=false", + ] + r = subprocess.run(cmd, capture_output=True, text=True, timeout=120, check=False) + if r.returncode != 0: + pytest.skip(f"xacro failed: {r.stderr}") + root = ET.fromstring(r.stdout) + return {j.attrib["name"] for j in root.findall("joint") if "name" in j.attrib} + + +def test_active_yaml_validates(): + path = _active_yaml_path() + assert path.is_file(), f"missing {path}" + data = load_hardware_yaml(path) + validate_hardware_yaml(data, urdf_joint_names_from_xacro()) + + +@pytest.mark.parametrize( + "name,msg", + [ + ("invalid_duplicate_vpin.yaml", "duplicate virtual_pin"), + ("invalid_missing_board.yaml", "unknown board"), + ("invalid_servo_default_out_of_range.yaml", "servo_default_deg"), + ], +) +def test_invalid_fixture_rejected(name: str, msg: str): + path = Path(__file__).resolve().parent / "fixtures" / name + data = load_hardware_yaml(path) + with pytest.raises(ValueError, match=msg): + validate_hardware_yaml(data) diff --git a/test/test_xacro_smoke.py b/test/test_xacro_smoke.py index fae931c..b13bfd0 100644 --- a/test/test_xacro_smoke.py +++ b/test/test_xacro_smoke.py @@ -40,15 +40,16 @@ def _inmoov_paths(): def _controller_yaml() -> Path: try: from ament_index_python.packages import get_package_share_directory - p = ( - Path(get_package_share_directory("lucy_ros2_control")) - / "config" - / "lucy_controllers.yaml" - ) - if p.is_file(): - return p + + thais = Path(get_package_share_directory("thais_urdf")) / "config" / "controllers.yaml" + if thais.is_file(): + return thais except Exception: pass + root = Path(__file__).resolve().parents[1] + p = root / "config" / "controllers.yaml" + if p.is_file(): + return p ws_src = Path(__file__).resolve().parents[1].parent return ws_src / "lucy_ros2_control" / "config" / "lucy_controllers.yaml" @@ -58,8 +59,8 @@ def controller_config() -> Path: path = _controller_yaml() if not path.is_file(): pytest.skip( - "Missing lucy_ros2_control/config/lucy_controllers.yaml — " - "build lucy_ros2_control or place it next to thais_urdf under colcon src/" + "Missing thais_urdf/config/controllers.yaml (or legacy lucy_ros2_control path) — " + "build thais_urdf from this workspace." ) return path