diff --git a/.github/workflows/build_page.yml b/.github/workflows/build_page.yml new file mode 100644 index 0000000..231b982 --- /dev/null +++ b/.github/workflows/build_page.yml @@ -0,0 +1,57 @@ +name: Documentation + +# Controls when the workflow will run +on: + # Triggers the workflow on push or pull request events but only for the master branch + push: + branches: [ main ] + pull_request: + branches: [ main ] + +# A workflow run is made up of one or more jobs that can run sequentially or in parallel +jobs: + # This workflow contains a single job called "build" + build_documentation: + # The type of runner that the job will run on + runs-on: ubuntu-22.04 + + # Steps represent a sequence of tasks that will be executed as part of the job + steps: + # Checks-out your repository under $GITHUB_WORKSPACE, so your job can access it + - uses: actions/checkout@v4 + + - name: Install dependencies + run: | + sudo apt-get update -qq + sudo apt-get install -y -qq doxygen graphviz plantuml + pip install sphinx-rtd-theme + pip install sphinxcontrib-plantuml + pip install sphinx-mdinclude + pip install breathe + pip install exhale + pip install myst-parser + pip install sphinx_copybutton + + - name: Build documentation + run: | + cd ./workshop + make html + cd ../ + + - name: Create commit + run: | + git clone https://github.com/ipa-vsp/industrial_standards.git --branch gh-pages --single-branch gh-pages + mkdir -p gh-pages/ + cp -r ./workshop/build/html/* gh-pages/ + cd gh-pages + git config --local user.email "action@github.com" + git config --local user.name "GitHub Action" + git add . + git commit -m "Update documentation" -a || true + + - name: Push changes + uses: ad-m/github-push-action@master + with: + branch: gh-pages + directory: gh-pages + github_token: ${{ secrets.GITHUB_TOKEN }} \ No newline at end of file diff --git a/workshop/source/_source/control/01_Example.md b/workshop/source/_source/control/01_Example.md new file mode 100644 index 0000000..b0f7ce1 --- /dev/null +++ b/workshop/source/_source/control/01_Example.md @@ -0,0 +1,233 @@ +# RRBot + +**![Reference from ros2_control_demos](https://control.ros.org/rolling/doc/ros2_control_demos/doc/index.html#)** + +**RRBot**, or *Revolute-Revolute Manipulator Robot*, is a simple 3-linkage, 2-joint arm used to demonstrate various features. + +It is essentially a double inverted pendulum and demonstrates some control concepts within a simulator. It was originally introduced for Gazebo tutorials. + +For *example\_1*, the hardware interface plugin is implemented with only one interface: + +* Communication is done using proprietary API to communicate with the robot control box. +* Data for all joints is exchanged at once. +* Example: KUKA RSI + +The **RRBot** URDF files can be found in the `description/urdf` folder. + +## Tutorial Steps + +### 0. Prerequisites + +Ensure you have the following packages installed: + +```sh +cd +mkdir -p colcon_control_ws/src +cd colcon_control_ws/src +git clone -b demo/tricycle https://github.com/ipa-vsp/ros2_control_demos.git +cd .. +rosdep install --from-paths src -iry +colcon build --symlink-install +source install/setup.bash +``` + +### 1. Check RRBot Description Launch + +(Optional) To verify that RRBot descriptions are working properly, run: + +```sh +ros2 launch ros2_control_demo_example_1 view_robot.launch.py +``` + +In another terminal, launch the GUI: + +```sh +source /opt/ros/${ROS_DISTRO}/setup.bash +ros2 run joint_state_publisher_gui joint_state_publisher_gui +``` + +Start RViz: + +```sh +source /opt/ros/${ROS_DISTRO}/setup.bash +rviz2 -d src/ros2_control_demos/ros2_control_demo_description/rrbot/rviz/rrbot.rviz +``` + +> **Note**: Warning `Invalid frame ID "odom" passed to canTransform` is expected. It happens while `joint_state_publisher_gui` is starting. + +![RRBot](rrbot.png) + +Once working, stop RViz using `CTRL+C`. + +### 2. Start RRBot Launch + +```sh +ros2 launch ros2_control_demo_example_1 rrbot.launch.py +``` + +This starts the robot hardware, controllers, and opens RViz. If you see two orange and one yellow rectangle in RViz, it is working. + +### 3. Check Hardware Interface + +```sh +ros2 control list_hardware_interfaces +``` + +Expected output: + +```sh +command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] +state interfaces + joint1/position + joint2/position +``` + +### 4. Check Running Controllers + +```sh +ros2 control list_controllers +``` + +Expected output: + +```sh +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active +forward_position_controller[forward_command_controller/ForwardCommandController] active +``` + +### 5. Send Commands + +**a. Using CLI:** + +```sh +ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: +- 0.5 +- 0.5" +``` + +**b. Using Demo Node:** + +```sh +ros2 launch ros2_control_demo_example_1 test_forward_position_controller.launch.py +``` + +You should see motion in RViz and terminal logs like: + +```sh +[INFO] Writing commands: + 0.50 for joint 'joint2/position' + 0.50 for joint 'joint1/position' +``` + +To verify joint state outputs: + +```sh +ros2 topic echo /joint_states +ros2 topic echo /dynamic_joint_states +``` + +### 6. Switch to Joint Trajectory Controller + +Load controller: + +```sh +ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml +``` + +List controllers: + +```sh +ros2 control list_controllers +``` + +Expected: + +```sh +joint_state_broadcaster[...] active +forward_position_controller[...] active +joint_trajectory_position_controller[...] unconfigured +``` + +Configure controller: + +```sh +ros2 control set_controller_state joint_trajectory_position_controller inactive +``` + +Alternatively: + +```sh +ros2 control load_controller --set-state inactive joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml +``` + +Activate controller: + +```sh +ros2 control switch_controllers --activate joint_trajectory_position_controller --deactivate forward_position_controller +``` + +Verify: + +```sh +ros2 control list_controllers +``` + +Should return: + +```sh +joint_state_broadcaster[...] active +forward_position_controller[...] inactive +joint_trajectory_position_controller[...] active +``` + +Launch demo node: + +```sh +ros2 launch ros2_control_demo_example_1 test_joint_trajectory_controller.launch.py +``` + +### 7. Use rqt\_joint\_trajectory\_controller GUI + +```sh +ros2 control load_controller joint_trajectory_position_controller $(ros2 pkg prefix ros2_control_demo_example_1 --share)/config/rrbot_jtc.yaml --set-state inactive && ros2 control switch_controllers --activate joint_trajectory_position_controller --deactivate forward_position_controller +``` + +Launch GUI: + +```sh +ros2 run rqt_joint_trajectory_controller rqt_joint_trajectory_controller +``` + +Features: + +* Dropdown to select controller +* Sliders to set joint positions +* Control execution time +* Send trajectory commands + +## Files Used for This Demo + +* **Launch file**: [rrbot.launch.py](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/bringup/launch/rrbot.launch.py) +* **Controllers YAML**: + + * [rrbot\_controllers.yaml](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/bringup/config/rrbot_controllers.yaml) + * [rrbot\_jtc.yaml](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/bringup/config/rrbot_jtc.yaml) +* **URDF files**: + + * [rrbot.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/description/urdf/rrbot.urdf.xacro) + * [rrbot\_description.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot/urdf/rrbot_description.urdf.xacro) + * [rrbot.ros2\_control.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/description/ros2_control/rrbot.ros2_control.xacro) +* **RViz config**: [rrbot.rviz](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot/rviz/rrbot.rviz) +* **Test configs**: + + * [rrbot\_forward\_position\_publisher](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/bringup/config/rrbot_forward_position_publisher.yaml) + * [rrbot\_joint\_trajectory\_publisher](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/bringup/config/rrbot_joint_trajectory_publisher.yaml) +* **Hardware interface**: [rrbot.cpp](https://github.com/ros-controls/ros2_control_demos/tree/master/example_1/hardware/rrbot.cpp) + +## Controllers in This Demo + +* **Joint State Broadcaster** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)) +* **Forward Command Controller** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/forward_command_controller)) +* **Joint Trajectory Controller** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/joint_trajectory_controller)) diff --git a/workshop/source/_source/control/02_Example.md b/workshop/source/_source/control/02_Example.md new file mode 100644 index 0000000..d53dde7 --- /dev/null +++ b/workshop/source/_source/control/02_Example.md @@ -0,0 +1,164 @@ +# DiffBot + +**DiffBot**, or *Differential Mobile Robot*, is a simple mobile base with differential drive. It is essentially a box that moves according to differential drive kinematics. + +For *example\_2*, the hardware interface plugin is implemented with only one interface: + +* Communication is done using a proprietary API to communicate with the robot control box. +* Data for all joints is exchanged at once. + +The **DiffBot** URDF files can be found in the `description/urdf` folder. + +## Tutorial Steps + +### 1. Check DiffBot Description Launch + +To verify that DiffBot descriptions are working properly, run: + +```sh +ros2 launch ros2_control_demo_example_2 view_robot.launch.py +``` + +> **Warning**: Output like `Warning: Invalid frame ID "odom" passed to canTransform` is expected. It happens while `joint_state_publisher_gui` is starting. + +![DiffBot](diffbot.png) + +### 2. Start DiffBot Launch + +```sh +ros2 launch ros2_control_demo_example_2 diffbot.launch.py +``` + +This starts the robot hardware, controllers, and opens RViz. You will see internal state logs in the terminal—these are for demonstration only and should be avoided in production code. + +If you see an orange box in RViz, everything has started properly. + +### 3. Check Hardware Interface + +```sh +ros2 control list_hardware_interfaces +``` + +Expected output: + +```sh +command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] +state interfaces + left_wheel_joint/position + left_wheel_joint/velocity + right_wheel_joint/position + right_wheel_joint/velocity +``` + +The `[claimed]` marker means a controller has access to command **DiffBot**. The interface type is `velocity`, typical for differential drive. + +### 4. Check Controllers + +```sh +ros2 control list_controllers +``` + +Expected output: + +```sh +diffbot_base_controller[diff_drive_controller/DiffDriveController] active +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active +``` + +### 5. Send Commands to Diff Drive Controller + +```sh +ros2 topic pub --rate 10 /cmd_vel geometry_msgs/msg/TwistStamped " + header: auto + twist: + linear: + x: 0.7 + y: 0.0 + z: 0.0 + angular: + x: 0.0 + y: 0.0 + z: 1.0" +``` + +You should now see an orange box circling in RViz and terminal logs like: + +```sh +[INFO] Writing commands: + command 43.33 for 'left_wheel_joint'! + command 50.00 for 'right_wheel_joint'! +``` + +### 6. Introspect Hardware Component + +```sh +ros2 control list_hardware_components +``` + +Expected output: + +```sh +Hardware Component 1 + name: DiffBot + type: system + plugin name: ros2_control_demo_example_2/DiffBotSystemHardware + state: id=3 label=active + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] +``` + +To test with mock hardware (useful if simulator is unavailable), restart with: + +```sh +ros2 launch ros2_control_demo_example_2 diffbot.launch.py use_mock_hardware:=True +``` + +Then run: + +```sh +ros2 control list_hardware_components +``` + +Expected output: + +```sh +Hardware Component 1 + name: DiffBot + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + command interfaces + left_wheel_joint/velocity [available] [claimed] + right_wheel_joint/velocity [available] [claimed] +``` + +This shows a different plugin was loaded. Check `diffbot.ros2_control.xacro` to see: + +```xml + + mock_components/GenericSystem + true + +``` + +This enables integration of velocity commands into position state, visible in `/joint_states`. Position should increase over time while moving. + +## Files Used for This Demo + +* **Launch file**: [diffbot.launch.py](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2/bringup/launch/diffbot.launch.py) +* **Controllers YAML**: [diffbot\_controllers.yaml](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2/bringup/config/diffbot_controllers.yaml) +* **URDF files**: + + * [diffbot.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2/description/urdf/diffbot.urdf.xacro) + * [diffbot\_description.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/diffbot/urdf/diffbot_description.urdf.xacro) + * [diffbot.ros2\_control.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2/description/ros2_control/diffbot.ros2_control.xacro) +* **RViz config**: [diffbot.rviz](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/diffbot/rviz/diffbot.rviz) +* **Hardware interface plugin**: [diffbot\_system.cpp](https://github.com/ros-controls/ros2_control_demos/tree/master/example_2/hardware/diffbot_system.cpp) + +## Controllers in This Demo + +* **Joint State Broadcaster** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)) +* **Diff Drive Controller** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/diff_drive_controller)) diff --git a/workshop/source/_source/control/03_Example.md b/workshop/source/_source/control/03_Example.md new file mode 100644 index 0000000..ab26f97 --- /dev/null +++ b/workshop/source/_source/control/03_Example.md @@ -0,0 +1,210 @@ +# Robots with Multiple Interfaces + +This example demonstrates how to implement a multi-interface robot hardware, taking care of the interfaces used. + +For *example\_3*, the hardware interface plugin supports multiple command and state interfaces: + +* Communication is done using a proprietary API to interact with the robot control box. +* Data for all joints is exchanged simultaneously. +* Examples: KUKA FRI, ABB Yumi, Schunk LWA4p, etc. + +Two intentionally misconfigured controllers demonstrate how the hardware interface can reject invalid joint interface claims. + +## Tutorial Steps + +### 1. Launch RRBot Visualization + +```sh +ros2 launch ros2_control_demo_example_3 view_robot.launch.py +``` + +> **Note**: Warnings like `Invalid frame ID "odom"` are expected while `joint_state_publisher_gui` initializes. It provides a GUI to generate random configurations displayed in RViz. + +### 2. Start RRBot with Multi-Interface Support + +```sh +ros2 launch ros2_control_demo_example_3 rrbot_system_multi_interface.launch.py +``` + +Useful options: + +* `robot_controller:=forward_position_controller`: spawns a position controller. +* `robot_controller:=forward_acceleration_controller`: spawns an acceleration controller. + +This launch file loads the hardware, controllers, and RViz. Console output will show the internal hardware state (for demonstration only). + +If RViz shows two orange and one yellow rectangles, the system has started correctly. + +### 3. Verify Hardware Interface + +```sh +ros2 control list_hardware_interfaces +``` + +Expected output: + +```sh +command interfaces + joint1/acceleration [available] [unclaimed] + joint1/position [available] [unclaimed] + joint1/velocity [available] [claimed] + joint2/acceleration [available] [unclaimed] + joint2/position [available] [unclaimed] + joint2/velocity [available] [claimed] +state interfaces + joint1/acceleration + joint1/position + joint1/velocity + joint2/acceleration + joint2/position + joint2/velocity +``` + +### 4. Check Active Controllers + +```sh +ros2 control list_controllers +``` + +Example output: + +```sh +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active +forward_velocity_controller[velocity_controllers/JointGroupVelocityController] active +``` + +This output varies based on the `robot_controller` argument used. + +### 5. Send Commands + +#### a. Using CLI + +* For `forward_position_controller`: + +```sh +ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: +- 0.5 +- 0.5" +``` + +* For `forward_velocity_controller` (default): + +```sh +ros2 topic pub /forward_velocity_controller/commands std_msgs/msg/Float64MultiArray "data: +- 5 +- 5" +``` + +* For `forward_acceleration_controller`: + +```sh +ros2 topic pub /forward_acceleration_controller/commands std_msgs/msg/Float64MultiArray "data: +- 10 +- 10" +``` + +#### b. Using Demo Node (for position controller): + +```sh +ros2 launch ros2_control_demo_example_3 test_forward_position_controller.launch.py +``` + +Terminal will log updates: + +```sh +[INFO] Writing commands: + command pos: 0.00, vel: 5.00, acc: 0.00 for joint 0 + command pos: 0.00, vel: 5.00, acc: 0.00 for joint 1 +[INFO] Reading states: + pos: 0.67, vel: 5.00, acc: 0.00 for joint 0 + pos: 0.67, vel: 5.00, acc: 0.00 for joint 1 +``` + +### 6. Switch Controllers at Runtime + +Load a new controller (if not already active): + +```sh +ros2 control load_controller forward_position_controller $(ros2 pkg prefix ros2_control_demo_example_3 --share)/config/rrbot_multi_interface_forward_controllers.yaml +ros2 control set_controller_state forward_position_controller inactive +``` + +Then switch controllers: + +```sh +ros2 control switch_controllers --deactivate forward_velocity_controller --activate forward_position_controller +``` + +Check updated interface claims: + +```sh +ros2 control list_controllers +ros2 control list_hardware_interfaces +``` + +Now send commands using the new controller. + +### 7. Demonstrate Illegal Controllers + +Launch with: + +* `robot_controller:=forward_illegal1_controller` or +* `robot_controller:=forward_illegal2_controller` + +Expected error output: + +```sh +[ERROR] [resource_manager]: Component 'RRBotSystemMultiInterface' did not accept new command resource combination: + Start interfaces: + joint1/position + Stop interfaces: + (none) +[ERROR] [controller_manager]: Could not switch controllers since prepare command mode switch was rejected. +[ERROR] [spawner_forward_illegal1_controller]: Failed to activate controller +``` + +Run: + +```sh +ros2 control list_hardware_interfaces +``` + +Output: + +```sh +command interfaces + joint1/acceleration [available] [unclaimed] + joint1/position [available] [unclaimed] + joint1/velocity [available] [unclaimed] + joint2/acceleration [available] [unclaimed] + joint2/position [available] [unclaimed] + joint2/velocity [available] [unclaimed] +``` + +```sh +ros2 control list_controllers +``` + +Output: + +```sh +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active +forward_illegal1_controller[forward_command_controller/ForwardCommandController] inactive +``` + +## Files Used in This Demo + +* **Launch file**: [rrbot\_system\_multi\_interface.launch.py](https://github.com/ros-controls/ros2_control_demos/tree/master/example_3/bringup/launch/rrbot_system_multi_interface.launch.py) +* **Controllers YAML**: [rrbot\_multi\_interface\_forward\_controllers.yaml](https://github.com/ros-controls/ros2_control_demos/tree/master/example_3/bringup/config/rrbot_multi_interface_forward_controllers.yaml) +* **URDF**: + + * [rrbot\_system\_multi\_interface.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_3/description/urdf/rrbot_system_multi_interface.urdf.xacro) + * [rrbot\_description.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot/urdf/rrbot_description.urdf.xacro) + * [rrbot\_system\_multi\_interface.ros2\_control.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_3/description/ros2_control/rrbot_system_multi_interface.ros2_control.xacro) +* **RViz Config**: [rrbot.rviz](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot/rviz/rrbot.rviz) +* **Hardware Interface Plugin**: [rrbot\_system\_multi\_interface.cpp](https://github.com/ros-controls/ros2_control_demos/blob/master/example_3/hardware/rrbot_system_multi_interface.cpp) + +## Controllers in This Demo + +* **Joint State Broadcaster** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)) +* **Forward Command Controller** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/forward_command_controller)) diff --git a/workshop/source/_source/control/04_Example.md b/workshop/source/_source/control/04_Example.md new file mode 100644 index 0000000..c6883eb --- /dev/null +++ b/workshop/source/_source/control/04_Example.md @@ -0,0 +1,166 @@ +# Industrial Robot with Externally Connected Sensor + +This example shows how an externally connected sensor can be accessed: + +* Communication is done using a proprietary API to interface with the robot control box. +* Joint data are exchanged simultaneously. +* Sensor data are exchanged independently of joint data. +* Examples: KUKA RSI and FTS connected to independent PC with ROS 2. + +A 3D Force-Torque Sensor (FTS) is simulated using a `SensorInterface` that generates random sensor readings. + +## Tutorial Steps + +### 1. Launch RRBot Visualization + +```sh +ros2 launch ros2_control_demo_example_5 view_robot.launch.py +``` + +> **Note**: Warning `Invalid frame ID "odom"` is expected while `joint_state_publisher_gui` initializes. It provides a GUI to set a random configuration for RRBot, shown in RViz. + +### 2. Launch RRBot with External Sensor + +```sh +ros2 launch ros2_control_demo_example_5 rrbot_system_with_external_sensor.launch.py +``` + +This starts the robot hardware, controllers, and opens RViz. Console output will include internal state logs (for demonstration only). + +If RViz shows two orange and one yellow rectangles, the system has started correctly. + +### 3. Verify Hardware Interfaces + +```sh +ros2 control list_hardware_interfaces +``` + +Expected output: + +```sh +command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] +state interfaces + joint1/position + joint2/position + tcp_fts_sensor/force.x + tcp_fts_sensor/force.y + tcp_fts_sensor/force.z + tcp_fts_sensor/torque.x + tcp_fts_sensor/torque.y +``` + +Check hardware components: + +```sh +ros2 control list_hardware_components +``` + +Expected output: + +```sh +Hardware Component 1 + name: ExternalRRBotFTSensor + type: sensor + plugin name: ros2_control_demo_example_5/ExternalRRBotForceTorqueSensorHardware + state: id=3 label=active + command interfaces +Hardware Component 2 + name: RRBotSystemPositionOnly + type: system + plugin name: ros2_control_demo_example_5/RRBotSystemPositionOnlyHardware + state: id=3 label=active + command interfaces + joint1/position [available] [claimed] + joint2/position [available] [claimed] +``` + +### 4. Check Running Controllers + +```sh +ros2 control list_controllers +``` + +Expected output: + +```sh +forward_position_controller[forward_command_controller/ForwardCommandController] active +fts_broadcaster[force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster] active +joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active +``` + +### 5. Send Commands to RRBot + +**a. Manual Control:** + +```sh +ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: +- 0.5 +- 0.5" +``` + +**b. Using Demo Node:** + +```sh +ros2 launch ros2_control_demo_example_5 test_forward_position_controller.launch.py +``` + +Example log output: + +```sh +[INFO] Writing commands: + 0.50 for joint 'joint1' + 0.50 for joint 'joint2' +``` + +### 6. Access Force-Torque Sensor Data + +```sh +ros2 topic echo /fts_broadcaster/wrench +``` + +Expected output: + +```yaml +header: + stamp: + sec: 1676444704 + nanosec: 332221422 + frame_id: tool_link +wrench: + force: + x: 1.21 + y: 2.32 + z: 3.43 + torque: + x: 4.54 + y: 0.65 + z: 1.76 +``` + +Sensor data is visualized in RViz: + +![RRBot Wrench](rrbot_wrench.png) + +## Files Used for This Demo + +* **Launch file**: [rrbot\_system\_with\_external\_sensor.launch.py](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/bringup/launch/rrbot_system_with_external_sensor.launch.py) +* **Controllers YAML**: [rrbot\_with\_external\_sensor\_controllers.yaml](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/bringup/config/rrbot_with_external_sensor_controllers.yaml) +* **URDF**: + + * [rrbot\_with\_external\_sensor\_controllers.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/blob/master/example_5/description/urdf/rrbot_system_with_external_sensor.urdf.xacro) + * [rrbot\_description.urdf.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/ros2_control_demo_description/rrbot/urdf/rrbot_description.urdf.xacro) + * [rrbot\_system\_position\_only.ros2\_control.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/description/ros2_control/rrbot_system_position_only.ros2_control.xacro) + * [external\_rrbot\_force\_torque\_sensor.ros2\_control.xacro](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/description/ros2_control/external_rrbot_force_torque_sensor.ros2_control.xacro) +* **RViz Config**: [rrbot.rviz](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/description/rviz/rrbot.rviz) +* **Hardware Interface Plugins**: + + * [rrbot.cpp (robot)](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/hardware/rrbot.cpp) + * [external\_rrbot\_force\_torque\_sensor.cpp (sensor)](https://github.com/ros-controls/ros2_control_demos/tree/master/example_5/hardware/external_rrbot_force_torque_sensor.cpp) + +## Controllers in This Demo + +* **Joint State Broadcaster** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/joint_state_broadcaster)) +* **Forward Command Controller** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/forward_command_controller)) +* **Force Torque Sensor Broadcaster** ([docs](https://github.com/ros-controls/ros2_controllers/tree/master/force_torque_sensor_broadcaster)) diff --git a/workshop/source/_source/control/05_Example.md b/workshop/source/_source/control/05_Example.md new file mode 100644 index 0000000..7acf09d --- /dev/null +++ b/workshop/source/_source/control/05_Example.md @@ -0,0 +1,308 @@ +# WBot Mobile Manipulator Example + +Bring up and control the WBot differential-drive base and 6-DoF arm with `ros2_control`. This example covers setup, launch, teleop, controller introspection, and mixing real base hardware with a mock arm. + +![WBot Teleop](mobile_manipulator/wbot_teleop.gif) + +## 0. Setup (one time) + +```sh +sudo apt install git-lfs +git lfs install + +mkdir -p ~/colcon_control_ws/src +cd ~/colcon_control_ws/src +git clone https://github.com/ROSI-IPA/wbot.git +git clone https://github.com/MarqRazz/piper_ros2.git +cd .. +rosdep install --from-paths src -iry +colcon build --symlink-install +source install/setup.bash +``` + +## 1. Launch the robot + +```sh +ros2 launch wbot_bringup wbot.launch.xml +``` + +This starts the base, arm, controllers, and RViz. In another terminal, open `rqt_graph` if you want to inspect nodes. + +## 2. Teleop the base + +```sh +ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true +``` + +Drive around and watch RViz to see odometry and the footprint moving. + +## 3. Inspect ros2_control state + +While the launch is running, query the control stack: + +```sh +ros2 control list_controllers +ros2 control list_controller_types +ros2 control list_hardware_components -v +ros2 control list_hardware_interfaces +``` + +For full controller manager introspection: + +```sh +ros2 topic echo /controller_manager/introspection_data/full +``` + +Focus on `wbot_base_control.nonlimited` and `wbot_base_control.limited` as you drive to see command limiting in action. + +## 4. Visualize introspection values in PlotJuggler + +```sh +ros2 run plotjuggler plotjuggler +``` + +1. Start the ROS 2 subscriber. +2. Add `/controller_manager/introspection_data/values` as a topic. +3. Plot `wbot_base_control.nonlimited` and `wbot_base_control.limited`; they publish at 100 Hz. + +## 5. ros2_control configuration reference + +Hardware macro snippet: + +```xml + + + + + + mock_components/GenericSystem + false + 0.0 + true + + + joint_state_topic_hardware_interface/JointStateTopicSystem + ${joint_command_topic} + ${joint_states_topic} + true + -1 + ${enable_command_limiting} + + + + + -16.00 + 16.00 + + + + + + + -16.00 + 13.0 + + + + + + +``` + +Controller YAML (base, arm, gripper): + +```yaml +controller_manager: + ros__parameters: + update_rate: 100 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + diff_drive_base_controller: + type: diff_drive_controller/DiffDriveController + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + gripper_controller: + type: parallel_gripper_action_controller/GripperActionController + +diff_drive_base_controller: + ros__parameters: + left_wheel_names: ["wbot_wheel_left_joint"] + right_wheel_names: ["wbot_wheel_right_joint"] + wheel_separation: 0.287 + wheel_radius: 0.033 + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + publish_rate: 50.0 + odom_frame_id: odom + base_frame_id: wbot_base_footprint + pose_covariance_diagonal: [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] + twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] + open_loop: false + position_feedback: true + enable_odom_tf: true + cmd_vel_timeout: 0.5 + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: true + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 2.5 + linear.x.max_jerk: 0.0 + linear.x.min_jerk: 0.0 + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: true + angular.z.has_jerk_limits: false + angular.z.max_velocity: 2.0 + angular.z.min_velocity: -2.0 + angular.z.max_acceleration: 6.2 + angular.z.min_acceleration: -6.2 + angular.z.max_jerk: 0.0 + angular.z.min_jerk: 0.0 + +joint_trajectory_controller: + ros__parameters: + joints: + - wbot_arm_joint_1 + - wbot_arm_joint_2 + - wbot_arm_joint_3 + - wbot_arm_joint_4 + - wbot_arm_joint_5 + - wbot_arm_joint_6 + command_interfaces: + - position + state_interfaces: + - position + - velocity + state_publish_rate: 0.0 + action_monitor_rate: 20.0 + allow_partial_joints_goal: false + open_loop_control: false + allow_nonzero_velocity_at_trajectory_end: true + interpolation_method: splines + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.0 + joint_1: + goal: 0.001 + joint_2: + goal: 0.001 + joint_3: + goal: 0.001 + joint_4: + goal: 0.001 + joint_5: + goal: 0.001 + joint_6: + goal: 0.001 + +gripper_controller: + ros__parameters: + action_monitor_rate: 20.0 + allow_stalling: false + goal_tolerance: 0.01 + joint: wbot_arm_gripper_joint + stall_timeout: 1.0 + stall_velocity_threshold: 0.002 +``` + +## 6. Mobile manipulator with mixed hardware + +If you have the base hardware but no arm, run the arm with mock hardware: + +```sh +ros2 launch wbot_bringup wbot_manipulator.launch.xml mock_hardware:=true +``` + +List hardware components: + +```sh +ros2 control list_hardware_components +``` + +Example output: + +```text +Hardware Component 1 + name: wbot_arm_piper_control + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + read/write rate: 100 Hz + is_async: False + command interfaces + wbot_arm_joint_5/position [available] [claimed] + wbot_arm_joint_6/position [available] [claimed] + wbot_arm_joint_4/position [available] [claimed] + wbot_arm_gripper_joint/position [available] [claimed] + wbot_arm_joint_3/position [available] [claimed] + wbot_arm_joint_2/position [available] [claimed] + wbot_arm_joint_1/position [available] [claimed] +Hardware Component 2 + name: wbot_base_control + type: system + plugin name: mock_components/GenericSystem + state: id=3 label=active + read/write rate: 100 Hz + is_async: False + command interfaces + wbot_wheel_right_joint/velocity [available] [claimed] + wbot_wheel_left_joint/velocity [available] [claimed] +``` + +## 7. Move the robot + +### Base (DiffDriveController) + +```sh +ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -p stamped:=true +``` + +The diff-drive controller computes wheel velocities and sends them to the embedded board while streaming state back. + +### Arm (JointTrajectoryController) + +The joint trajectory controller supports both an action and a topic: + +```sh +ros2 action info /joint_trajectory_controller/follow_joint_trajectory +ros2 topic info -v /joint_trajectory_controller/joint_trajectory +``` + +Send a pose: + +```sh +ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/JointTrajectory "{ + joint_names: [wbot_arm_joint_1, wbot_arm_joint_2, wbot_arm_joint_3, wbot_arm_joint_4, wbot_arm_joint_5, wbot_arm_joint_6], + points: [ + { positions: [0.0, 0.85, -0.75, 0.0, 0.5, 0.0], time_from_start: { sec: 2 } } + ] +}" -1 +``` + +Return home: + +```sh +ros2 topic pub /joint_trajectory_controller/joint_trajectory trajectory_msgs/JointTrajectory "{ + joint_names: [wbot_arm_joint_1, wbot_arm_joint_2, wbot_arm_joint_3, wbot_arm_joint_4, wbot_arm_joint_5, wbot_arm_joint_6], + points: [ + { positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 1 } } + ] +}" -1 +``` + +![WBot Manipulator Move](mobile_manipulator/wbot_manipulator_move.gif) + +### Gripper (GripperActionController) + +```sh +ros2 action send_goal /gripper_controller/gripper_cmd control_msgs/action/ParallelGripperCommand "{command: {name: [wbot_arm_gripper_joint], position: [0.03]}}" +ros2 action send_goal /gripper_controller/gripper_cmd control_msgs/action/ParallelGripperCommand "{command: {name: [wbot_arm_gripper_joint], position: [0.0]}}" +``` + +![WBot Gripper](mobile_manipulator/wbot_gripper.gif) diff --git a/workshop/source/_source/control/diffbot.png b/workshop/source/_source/control/diffbot.png new file mode 100644 index 0000000..1c4fc24 Binary files /dev/null and b/workshop/source/_source/control/diffbot.png differ diff --git a/workshop/source/_source/control/rrbot.png b/workshop/source/_source/control/rrbot.png new file mode 100644 index 0000000..9f5980b Binary files /dev/null and b/workshop/source/_source/control/rrbot.png differ diff --git a/workshop/source/_source/control/rrbot_wrench.png b/workshop/source/_source/control/rrbot_wrench.png new file mode 100644 index 0000000..012ca52 Binary files /dev/null and b/workshop/source/_source/control/rrbot_wrench.png differ diff --git a/workshop/source/_source/control/wbot_gripper.gif b/workshop/source/_source/control/wbot_gripper.gif new file mode 100644 index 0000000..067d473 Binary files /dev/null and b/workshop/source/_source/control/wbot_gripper.gif differ diff --git a/workshop/source/_source/control/wbot_manipulator_move.gif b/workshop/source/_source/control/wbot_manipulator_move.gif new file mode 100644 index 0000000..da45887 Binary files /dev/null and b/workshop/source/_source/control/wbot_manipulator_move.gif differ diff --git a/workshop/source/_source/control/wbot_teleop.gif b/workshop/source/_source/control/wbot_teleop.gif new file mode 100644 index 0000000..ec6e7ac Binary files /dev/null and b/workshop/source/_source/control/wbot_teleop.gif differ diff --git a/workshop/source/_source/manipulation/01_Example.md b/workshop/source/_source/manipulation/01_Example.md new file mode 100644 index 0000000..6690147 --- /dev/null +++ b/workshop/source/_source/manipulation/01_Example.md @@ -0,0 +1,165 @@ +# MoveIt 2 Jazzy: Python Move Group Tutorial + +This tutorial walks you through creating a Python-based MoveIt 2 interface package using ROS 2 Jazzy. You'll learn how to: + +1. Create a Python ROS 2 package. +2. Write a script that moves a robot to a specific pose. +3. Configure the package for MoveIt. +4. Run the motion planning script. + +--- + +## Step 1: Create a Python Package for Move Group + +Open a terminal in your ROS 2 workspace `src` directory: + +```sh +cd ~/ws_moveit2/src +ros2 pkg create --build-type ament_python moveit_python_demo --dependencies rclpy moveit_commander geometry_msgs +``` + +This will generate the basic Python package structure. + +### Update `setup.py` + +Edit the generated `setup.py` file and update the `entry_points` block: + +```python +entry_points={ + 'console_scripts': [ + 'move_to_pose = moveit_python_demo.move_to_pose:main', + ], +}, +``` + +--- + +## Step 2: Create Script to Move Robot to a Pose + +Create a file called `move_to_pose.py` inside the `moveit_python_demo/moveit_python_demo/` directory: + +```python +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander +from geometry_msgs.msg import PoseStamped + +class MoveToPoseDemo(Node): + def __init__(self): + super().__init__('move_to_pose_demo') + rclpy.spin_once(self, timeout_sec=1.0) # ensure time is updated + + self.robot = RobotCommander() + self.scene = PlanningSceneInterface() + self.group = MoveGroupCommander('arm') + + self.group.set_pose_reference_frame('world') + self.group.set_max_velocity_scaling_factor(0.5) + self.group.set_max_acceleration_scaling_factor(0.5) + + self.move_to_ready_pose() + + def move_to_ready_pose(self): + target_pose = PoseStamped() + target_pose.header.frame_id = 'world' + target_pose.pose.position.x = 0.4 + target_pose.pose.position.y = 0.0 + target_pose.pose.position.z = 0.4 + target_pose.pose.orientation.w = 1.0 + + self.group.set_pose_target(target_pose) + success = self.group.go(wait=True) + self.group.stop() + self.group.clear_pose_targets() + + if success: + self.get_logger().info('Motion executed successfully!') + else: + self.get_logger().error('Motion planning failed!') + + +def main(): + rclpy.init() + node = MoveToPoseDemo() + rclpy.shutdown() + +if __name__ == '__main__': + main() +``` + +Make it executable: + +```sh +chmod +x moveit_python_demo/moveit_python_demo/move_to_pose.py +``` + +--- + +## Step 3: Configure the Package + +### Add `install_requires` to `setup.py` + +Make sure your `setup.py` includes: + +```python +install_requires=['setuptools'], +``` + +### Add an `__init__.py` + +Create an empty `__init__.py` file: + +```sh +touch moveit_python_demo/moveit_python_demo/__init__.py +``` + +### Update `package.xml` + +Make sure your `package.xml` includes the correct dependencies: + +```xml +moveit_commander +rclpy +geometry_msgs +``` + +--- + +## Step 4: Build and Run the Package + +Build your workspace: + +```sh +cd ~/ws_moveit2 +colcon build --packages-select moveit_python_demo +source install/setup.bash +``` + +Launch the robot simulation (if not already running): + +```sh +ros2 launch panda_moveit_config demo.launch.py +``` + +Run your Python move group script: + +```sh +ros2 run moveit_python_demo move_to_pose +``` + +You should see your robot arm move to the specified pose in RViz. + +--- + +## Summary + +You have now: + +* Created a Python ROS 2 package. +* Implemented a motion planning script using MoveIt Commander. +* Configured and launched a robot demo. +* Executed a pose target command. + +Feel free to expand the script to include waypoints, cartesian motions, or object collision handling! diff --git a/workshop/source/_source/manipulation/setup_assistance.md b/workshop/source/_source/manipulation/setup_assistance.md new file mode 100644 index 0000000..8296d06 --- /dev/null +++ b/workshop/source/_source/manipulation/setup_assistance.md @@ -0,0 +1,237 @@ +# MoveIt Setup Assistant + +## Prepare + +```bash +cd +mkdir -p ws_moveit2/src +cd ws_moveit2/src +git clone -b jazzy https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git +cd .. +rosdep install -y --from-paths src --iry +colcon build --symlink-install +source install/setup.bash +``` + +Now look for `src/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro` + +**Warning**: This setup is using UR robots instead of the Panda robot. Therefore, please replace **panda** with **ur** in the following instructions. + +![Setup Assistant Launch](setup_assistant_launch.png) + +## Overview + +The MoveIt Setup Assistant is a GUI tool for configuring a robot for MoveIt. It generates a Semantic Robot Description Format (SRDF) file and other necessary configuration files. A URDF is required to use the Setup Assistant. + +Once you have a URDF, import it into the Setup Assistant to configure kinematics, planning groups, end effectors, and collision checking. + +## Getting Started + +### MoveIt and ROS 2 + +* [Install MoveIt](/doc/tutorials/getting_started/getting_started) if you haven't already. +* Use the `moveit_resources_panda_description` package, included in your workspace after installation. + +## Step 1: Start + +```sh +ros2 launch moveit_setup_assistant setup_assistant.launch.py +``` + +Click **Create New MoveIt Configuration Package**. + +![Create Package](setup_assistant_create_package.png) + +Browse to and load: + +```sh +~/ws_moveit2/src/moveit_resources/panda_description/urdf/panda.urdf +``` + +![Load URDF](setup_assistant_load_panda_urdf.png) + +## Step 2: Generate Self-Collision Matrix + +Set the sampling density and click **Generate Collision Matrix**. + +![Collision Matrix](setup_assistant_panda_collision_matrix.png) + +View and modify results: + +![Collision Matrix Done](setup_assistant_panda_collision_matrix_done.png) + +## Step 3: Add Virtual Joints + +* Joint Name: `virtual_joint` +* Child Link: `panda_link0` +* Parent Frame: `world` +* Type: `fixed` + +![Virtual Joints](setup_assistant_panda_virtual_joints.png) + +> For mobile robots, use planar or floating joints. + +## Step 4: Add Planning Groups + +### Arm Group + +* Name: `panda_arm` +* Solver: `kdl_kinematics_plugin/KDLKinematicsPlugin` + +![Planning Groups](setup_assistant_panda_planning_groups.png) + +Select joints from `virtual_joint` to `panda_joint8`: + +![Arm Joints](setup_assistant_panda_arm_group_joints.png) + +Save group: + +![Arm Group Saved](setup_assistant_panda_arm_group_saved.png) + +### End Effector Group + +* Name: `hand` +* Solver: `None` +* Links: `panda_hand`, `panda_leftfinger`, `panda_rightfinger` + +![Hand Group](setup_assistant_panda_hand_group_links.png) + +Final group list: + +![Planning Groups Done](setup_assistant_panda_planning_groups_done.png) + +> Use **Add Subgroup** for compound groups (e.g., dual-arm systems). + +## Step 5: Add Robot Poses + +### Ready Pose for Arm + +* Group: `panda_arm` +* Pose: `ready` +* Values: `{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}` + +![Ready Pose](setup_assistant_panda_predefined_arm_pose.png) + +### Open/Close Poses for Hand + +* Group: `hand` +* `open`: `0.035` +* `close`: `0.0` + +![Open Pose](setup_assistant_panda_predefined_hand_open_pose.png) + +![Close Pose](setup_assistant_panda_predefined_hand_close_pose.png) + +> Only `panda_finger_joint1` is shown as `panda_finger_joint2` mimics it. + +Final poses: + +![Predefined Poses Done](setup_assistant_panda_predefined_poses_done.png) + +## Step 6: Label End Effectors + +* Name: `hand` +* Group: `hand` +* Parent Link: `panda_link8` + +![Add End Effector](setup_assistant_panda_add_end_effector.png) + +## Step 7: Add Passive Joints + +> Skip this step for Panda. Use if robot has unactuated joints. + +## Step 8: ros2\_control URDF Modification + +Define command/state interfaces (default: position command; position + velocity states). + +![ros2\_control Tags](setup_assistant_ros2_control_tags.png) + +## Step 9: ROS 2 Controllers + +### Panda Arm Controller + +* Name: `panda_arm_controller` +* Type: `joint_trajectory_controller/JointTrajectoryController` +* Group: `panda_arm` + +![Arm Controller Type](setup_assistant_panda_arm_ros2_controller_type.png) + +![Arm Controller Group](setup_assistant_panda_arm_ros2_controller_group.png) + +### Hand Controller + +* Name: `hand_controller` +* Type: `position_controllers/GripperActionController` +* Group: `hand` + +![Hand Controller Type](setup_assistant_hand_ros2_controller_type.png) + +![Hand Controller Group](setup_assistant_hand_ros2_controller_group.png) + +Final list: + +![Controllers Done](setup_assistant_ros2_controllers_done.png) + +## Step 10: MoveIt Controllers + +Configure MoveIt interfaces to match ROS 2 controllers. + +### Arm + +* Name: `panda_arm_controller` +* Type: `FollowJointTrajectory` +* Group: `panda_arm` + +![MoveIt Arm Controller](setup_assistant_panda_arm_moveit_controller_type.png) + +### Hand + +* Name: `hand_controller` +* Type: `Gripper Command` +* Group: `hand` + +![MoveIt Hand Controller](setup_assistant_hand_moveit_controller_type_gripper.png) + +Final MoveIt controller list: + +![MoveIt Controllers Done](setup_assistant_moveit_controllers_done_gripper.png) + +## Step 11: Perception + +Use this pane to define 3D sensors. Skip or choose **None** if not needed. + +![Perception None](setup_assistant_panda_3d_perception.png) + +Example (for robots with cameras): + +![Point Cloud](setup_assistant_panda_3d_perception_point_cloud.png) + +See [Perception Pipeline tutorial](/doc/examples/perception_pipeline/perception_pipeline_tutorial). + +## Step 12: Launch Files + +View generated launch files and their purposes. + +![Launch Files](setup_assistant_launch_files.png) + +## Step 13: Add Author Information + +Enter your name and email in the **Author Information** pane. + +## Step 14: Generate Configuration Files + +Click **Configuration Files**, choose a location (e.g., `~/ws_moveit2/src/panda_moveit_config`), and click **Generate Package**. + +![Done](setup_assistant_done.png) + +## Build and Run the Demo + +```sh +cd ~/ws_moveit2 +colcon build --packages-select panda_moveit_config +source install/setup.bash + +ros2 launch panda_moveit_config demo.launch.py +``` + +Watch the [YouTube demo](https://youtu.be/f__udZlnTdM). diff --git a/workshop/source/_source/manipulation/setup_assistant_create_package.png b/workshop/source/_source/manipulation/setup_assistant_create_package.png new file mode 100644 index 0000000..d3b655a Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_create_package.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_done.png b/workshop/source/_source/manipulation/setup_assistant_done.png new file mode 100644 index 0000000..2dd3cf5 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_done.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_hand_moveit_controller_type_gripper.png b/workshop/source/_source/manipulation/setup_assistant_hand_moveit_controller_type_gripper.png new file mode 100644 index 0000000..0e640d5 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_hand_moveit_controller_type_gripper.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_group.png b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_group.png new file mode 100644 index 0000000..bbc77d7 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_group.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type (1).png b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type (1).png new file mode 100644 index 0000000..a1ca436 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type (1).png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type.png b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type.png new file mode 100644 index 0000000..a1ca436 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_hand_ros2_controller_type.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_launch.png b/workshop/source/_source/manipulation/setup_assistant_launch.png new file mode 100644 index 0000000..d938467 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_launch.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_launch_files.png b/workshop/source/_source/manipulation/setup_assistant_launch_files.png new file mode 100644 index 0000000..6e5e863 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_launch_files.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_load_panda_urdf.png b/workshop/source/_source/manipulation/setup_assistant_load_panda_urdf.png new file mode 100644 index 0000000..fd2c234 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_load_panda_urdf.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_moveit_controllers.png b/workshop/source/_source/manipulation/setup_assistant_moveit_controllers.png new file mode 100644 index 0000000..3feaa44 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_moveit_controllers.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_moveit_controllers_done_gripper.png b/workshop/source/_source/manipulation/setup_assistant_moveit_controllers_done_gripper.png new file mode 100644 index 0000000..871aa97 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_moveit_controllers_done_gripper.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception.png b/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception.png new file mode 100644 index 0000000..20f9181 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception_point_cloud.png b/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception_point_cloud.png new file mode 100644 index 0000000..2cb0041 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_3d_perception_point_cloud.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_add_end_effector.png b/workshop/source/_source/manipulation/setup_assistant_panda_add_end_effector.png new file mode 100644 index 0000000..15104b3 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_add_end_effector.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_group.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group.png new file mode 100644 index 0000000..f8729a8 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_joints.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_joints.png new file mode 100644 index 0000000..f066896 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_joints.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_saved.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_saved.png new file mode 100644 index 0000000..2bac526 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_group_saved.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_moveit_controller_type.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_moveit_controller_type.png new file mode 100644 index 0000000..cc86507 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_moveit_controller_type.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group (1).png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group (1).png new file mode 100644 index 0000000..570747d Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group (1).png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group.png new file mode 100644 index 0000000..570747d Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_group.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (1).png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (1).png new file mode 100644 index 0000000..108007c Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (1).png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (2).png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (2).png new file mode 100644 index 0000000..108007c Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type (2).png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type.png b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type.png new file mode 100644 index 0000000..108007c Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_arm_ros2_controller_type.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix.png b/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix.png new file mode 100644 index 0000000..8ba9306 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix_done.png b/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix_done.png new file mode 100644 index 0000000..4cb038d Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_collision_matrix_done.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_hand_group.png b/workshop/source/_source/manipulation/setup_assistant_panda_hand_group.png new file mode 100644 index 0000000..540a13b Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_hand_group.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_hand_group_links.png b/workshop/source/_source/manipulation/setup_assistant_panda_hand_group_links.png new file mode 100644 index 0000000..1c22a8e Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_hand_group_links.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups.png b/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups.png new file mode 100644 index 0000000..ad53755 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups_done.png b/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups_done.png new file mode 100644 index 0000000..35b04aa Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_planning_groups_done.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_predefined_arm_pose.png b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_arm_pose.png new file mode 100644 index 0000000..796c174 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_arm_pose.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_close_pose.png b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_close_pose.png new file mode 100644 index 0000000..2702faf Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_close_pose.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_open_pose.png b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_open_pose.png new file mode 100644 index 0000000..077c41d Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_hand_open_pose.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_predefined_poses_done.png b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_poses_done.png new file mode 100644 index 0000000..c5822b5 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_predefined_poses_done.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_panda_virtual_joints.png b/workshop/source/_source/manipulation/setup_assistant_panda_virtual_joints.png new file mode 100644 index 0000000..f439093 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_panda_virtual_joints.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags (1).png b/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags (1).png new file mode 100644 index 0000000..38f5d1f Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags (1).png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags.png b/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags.png new file mode 100644 index 0000000..38f5d1f Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_ros2_control_tags.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_ros2_controllers.png b/workshop/source/_source/manipulation/setup_assistant_ros2_controllers.png new file mode 100644 index 0000000..99624e8 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_ros2_controllers.png differ diff --git a/workshop/source/_source/manipulation/setup_assistant_ros2_controllers_done.png b/workshop/source/_source/manipulation/setup_assistant_ros2_controllers_done.png new file mode 100644 index 0000000..7c489c3 Binary files /dev/null and b/workshop/source/_source/manipulation/setup_assistant_ros2_controllers_done.png differ diff --git a/workshop/source/index.rst b/workshop/source/index.rst index 71f74ad..feb55cd 100644 --- a/workshop/source/index.rst +++ b/workshop/source/index.rst @@ -34,7 +34,7 @@ Session 4 - ROS2 Manipulation .. toctree:: :maxdepth: 1 - Exercise 4 - ROS2-Moveit2 <_source/manipulation/readme.md> + Exercise 4 - ROS2-Moveit2-Setup-Assisitance <_source/manipulation/setup_assistance.md> Session 5 - ROS2 URDF ~~~~~~~~~~~~~~~~~~~~~~ @@ -43,4 +43,14 @@ Session 5 - ROS2 URDF ROS2 URDF_Introduction <_source/urdf/introduction.md> Exercise 5 - Ros2-Cartesian Robot <_source/urdf/cartesian_tutorial.md> + +Session 6 - ROS2 Control +~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +.. toctree:: + :maxdepth: 1 + + Exercise 6.1 - ROS2-Control-Example-1 <_source/control/01_Example.md> + Exercise 6.2 - ROS2-Control-Example-2 <_source/control/02_Example.md> + Exercise 6.3 - ROS2-Control-Example-3 <_source/control/03_Example.md> + Exercise 6.4 - ROS2-Control-Example-4 <_source/control/04_Example.md> \ No newline at end of file