Add demo for pr2

.github/workflows/ubuntu-24.04.yml

@@ -31,3 +31,8 @@ jobs:
       run: |
           chmod +x build_multiverse.sh
           ./build_multiverse.sh
+
+    - name: Run build_multiverse_ws2.sh
+      run: |
+          chmod +x build_multiverse_ws2.sh
+          ./build_multiverse_ws2.sh

multiverse/resources/muv/pr2_ros2_control.muv

@@ -0,0 +1,121 @@
+resources: 
+  - ../robots
+  - ../worlds
+  - ../objects
+
+worlds:
+  world:
+    rtf_desired: 1
+
+simulations:
+  pr2_simulation:
+    simulator: mujoco
+    world:
+      name: world
+      path: floor.xml
+    robots:
+      pr2:
+        path: pr2/pr2_with_pos_control.xml
+        apply:
+          body:
+            pr2:
+              pos: [0, 1, 0]
+            gravcomp: 1
+        disable_self_collision: auto
+    config:
+      max_time_step: 0.002
+      min_time_step: 0.001
+
+multiverse_server:
+  host: "tcp://127.0.0.1"
+  port: 7000
+
+multiverse_clients:
+  pr2_simulation:
+    port: 7500
+    send:
+      body: ["position", "quaternion"]
+      joint: ["joint_rvalue", "joint_tvalue", "joint_angular_velocity", "joint_linear_velocity", "joint_force", "joint_torque"]
+    receive:
+      r_shoulder_pan_actuator: ["cmd_joint_rvalue"]
+      r_shoulder_lift_actuator: ["cmd_joint_rvalue"]
+      r_upper_arm_roll_actuator: ["cmd_joint_rvalue"]
+      r_elbow_flex_actuator: ["cmd_joint_rvalue"]
+      r_forearm_roll_actuator: ["cmd_joint_rvalue"]
+      r_wrist_flex_actuator: ["cmd_joint_rvalue"]
+      r_wrist_roll_actuator: ["cmd_joint_rvalue"]
+      l_shoulder_pan_actuator: ["cmd_joint_rvalue"]
+      l_shoulder_lift_actuator: ["cmd_joint_rvalue"]
+      l_upper_arm_roll_actuator: ["cmd_joint_rvalue"]
+      l_elbow_flex_actuator: ["cmd_joint_rvalue"]
+      l_forearm_roll_actuator: ["cmd_joint_rvalue"]
+      l_wrist_flex_actuator: ["cmd_joint_rvalue"]
+      l_wrist_roll_actuator: ["cmd_joint_rvalue"]
+      torso_lift_actuator: ["cmd_joint_tvalue"]
+      head_pan_actuator: ["cmd_joint_rvalue"]
+      head_tilt_actuator: ["cmd_joint_rvalue"]
+
+      # r_shoulder_pan_actuator: ["cmd_joint_angular_velocity"]
+      # r_shoulder_lift_actuator: ["cmd_joint_angular_velocity"]
+      # r_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"]
+      # r_elbow_flex_actuator: ["cmd_joint_angular_velocity"]
+      # r_forearm_roll_actuator: ["cmd_joint_angular_velocity"]
+      # r_wrist_flex_actuator: ["cmd_joint_angular_velocity"]
+      # r_wrist_roll_actuator: ["cmd_joint_angular_velocity"]
+      # l_shoulder_pan_actuator: ["cmd_joint_angular_velocity"]
+      # l_shoulder_lift_actuator: ["cmd_joint_angular_velocity"]
+      # l_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"]
+      # l_elbow_flex_actuator: ["cmd_joint_angular_velocity"]
+      # l_forearm_roll_actuator: ["cmd_joint_angular_velocity"]
+      # l_wrist_flex_actuator: ["cmd_joint_angular_velocity"]
+      # l_wrist_roll_actuator: ["cmd_joint_angular_velocity"]
+      # torso_lift_actuator: ["cmd_joint_linear_velocity"]
+      # head_pan_actuator: ["cmd_joint_angular_velocity"]
+      # head_tilt_actuator: ["cmd_joint_angular_velocity"]
+
+  ros2:
+    ros2_control:
+      - meta_data:
+          world_name: world
+          length_unit: m
+          angle_unit: rad
+          mass_unit: kg
+          time_unit: s
+          handedness: rhs
+        port: 7600
+        controller_manager:
+          robot: pr2
+          robot_description: /robot_description
+          urdf: pr2/pr2.urdf
+          config: pr2/config/ros2_control.yaml
+          actuators:
+            r_shoulder_pan_actuator: r_shoulder_pan_joint
+            r_shoulder_lift_actuator: r_shoulder_lift_joint
+            r_upper_arm_roll_actuator: r_upper_arm_roll_joint
+            r_elbow_flex_actuator: r_elbow_flex_joint
+            r_forearm_roll_actuator: r_forearm_roll_joint
+            r_wrist_flex_actuator: r_wrist_flex_joint
+            r_wrist_roll_actuator: r_wrist_roll_joint
+            l_shoulder_pan_actuator: l_shoulder_pan_joint
+            l_shoulder_lift_actuator: l_shoulder_lift_joint
+            l_upper_arm_roll_actuator: l_upper_arm_roll_joint
+            l_elbow_flex_actuator: l_elbow_flex_joint
+            l_forearm_roll_actuator: l_forearm_roll_joint
+            l_wrist_flex_actuator: l_wrist_flex_joint
+            l_wrist_roll_actuator: l_wrist_roll_joint
+            torso_lift_actuator: torso_lift_joint
+            head_pan_actuator: head_pan_joint
+            head_tilt_actuator: head_tilt_joint
+          controllers:
+            spawner:
+              - joint_state_broadcaster
+                right_arm_trajectory_controller
+                left_arm_trajectory_controller
+                torso_arm_trajectory_controller
+                head_arm_trajectory_controller
+
+    ros2_run:
+      rviz2:
+        config: pr2/config/rviz.rviz2
+        robot_descriptions:
+          pr2_description: pr2/pr2.urdf

multiverse/resources/robots/pr2/config/ros2_control.yaml

@@ -0,0 +1,79 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+right_arm_trajectory_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    joints:
+      - r_shoulder_pan_actuator
+      - r_shoulder_lift_actuator
+      - r_upper_arm_roll_actuator
+      - r_elbow_flex_actuator
+      - r_forearm_roll_actuator
+      - r_wrist_flex_actuator
+      - r_wrist_roll_actuator
+
+    command_interfaces:
+      - position
+      - velocity
+      # - effort
+
+    state_interfaces:
+      - position
+      - velocity
+
+left_arm_trajectory_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    joints:
+      - l_shoulder_pan_actuator
+      - l_shoulder_lift_actuator
+      - l_upper_arm_roll_actuator
+      - l_elbow_flex_actuator
+      - l_forearm_roll_actuator
+      - l_wrist_flex_actuator
+      - l_wrist_roll_actuator
+
+    command_interfaces:
+      - position
+      - velocity
+      # - effort
+
+    state_interfaces:
+      - position
+      - velocity
+
+torso_trajectory_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    joints:
+      - torso_lift_actuator
+
+    command_interfaces:
+      - position
+      - velocity
+      # - effort
+
+    state_interfaces:
+      - position
+      - velocity
+
+head_trajectory_controller:
+  ros__parameters:
+    type: joint_trajectory_controller/JointTrajectoryController
+    joints:
+      - head_pan_actuator
+      - head_tilt_actuator
+
+    command_interfaces:
+      - position
+      - velocity
+      # - effort
+
+    state_interfaces:
+      - position
+      - velocity