Finalizing Demo IROS

clearpath/README.md

@@ -1,4 +1,6 @@
 # Gen3 + Husky Integration
+
+The Gen3 robotic arm from Kinova has been mounted on the Husky mobile platform from clearpath in Gazebo simulation environment. Both the arm and the mobile platform can be commanded simultaneously and the following explains in details what is needed to build the needed setup.
 ## Simulation Setup
 
 1. Install Gazebo Fortress
@@ -35,16 +37,29 @@ echo 'source ~/clearpath/setup.bash' >> ~/.bashrc
 ```
 sudo cp ~/workspace/ros2_kortex_ws/src/ros2_kortex/clearpath/gz_sim.launch.py /opt/ros/humble/share/clearpath_gz/launch/gz_sim.launch.py
 ```
+This will lead to the following modifications:
+
+    a. Starting the simulation in a running state which will prevent controllers timeout
+
+    b. Adding the keyboard press topic to the ROS2-Gazebo communication bridge
 
 7. Modify the simulation world using the following command:
 ```
 sudo cp ~/workspace/ros2_kortex_ws/src/ros2_kortex/clearpath/warehouse.sdf /opt/ros/humble/share/clearpath_gz/worlds/warehouse.sdf
 ```
+This will lead to the following modifications:
+
+    a. Decreasing the simulation step size down to `1 millisec` to allow for a high frequency publication of the joint states of the spawned robot
+
+    b. Adding the table and grasping box to the simulation scene
+
+    c. Simplifying the warehouse environment to avoid system's crashing (please note that more powerful computers can run more sophisticated simulation scenes)
 
 8. Modify the simulation configuration using the following command:
 ```
 sudo cp ~/workspace/ros2_kortex_ws/src/ros2_kortex/clearpath/gui.config /opt/ros/humble/share/clearpath_gz/config/gui.config
 ```
+This will change the default topic name in the teleop plugin panel in the simulation window
 
 9. Run the following command in **NEW** terminal window:
 ```
@@ -54,6 +69,7 @@ ros2 launch clearpath_gz simulation.launch.py
 ## Real-life Setup to establish a sim-to-real robotic arm & gripper synchronization
 
 1. Follow the [instructions](https://github.com/Kinovarobotics/Kinova-kortex2_Gen3_G3L/tree/master/api_python/examples) to prepare the setup for the python kortex-api
+
 2. If you are using Python >=3.10 (it will probably be the case), replace `import collections` by `import collections.abc as collections` in the following files: `~/.local/lib/python3.10/site-packages/google/protobuf/internal/containers.py` and `~/.local/lib/python3.10/site-packages/google/protobuf/internal/well_known_types.py` so that you will be able to use the python kortex-api included in the `commanding_script.py`
 
 3. Import the `~/workspace/ros2_kortex_ws/src/ros2_kortex/clearpath/Gen3_Husky_Initial_Position.xml` file to the webapp actions.
@@ -81,3 +97,80 @@ python3 commanding_script.py
 ```
 
 5. In the simulation window switch to the keyboard tab in the teleop panel on the right side, then command the robot using the keyboard. Please refer to the instructions included in `~/workspace/ros2_kortex_ws/src/ros2_kortex/clearpath/instructions.txt` for more details about the available keyboard commands.
+
+**P.S.** Before restarting the simulation, make sure to use the following command to eliminate any leftovers from a previous iteration:
+```
+killall ruby
+```
+
+The following file "~/workspace/ros2_kortex_ws/install/clearpath_config/lib/python3.10/site-packages/clearpath_config/common/types/rmw_implementation.py" was modified to the following:
+```
+# Software License Agreement (BSD)
+#
+# @author    Luis Camero <[email protected]>
+# @copyright (c) 2023, Clearpath Robotics, Inc., All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Clearpath Robotics nor the names of its contributors
+#   may be used to endorse or promote products derived from this software
+#   without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+class RMWImplementation:
+    CONNEXT = "rmw_connext_cpp"
+    CYCLONE_DDS = "rmw_cyclonedds_cpp"
+    FAST_RTPS = "rmw_fastrtps_cpp"
+    GURUM_DDS = "rmw_gurumdds_cpp"
+
+    ALL_SUPPORTED = [CYCLONE_DDS]
+
+    DEFAULT = CYCLONE_DDS
+
+    def __init__(
+            self,
+            rmw: str = DEFAULT
+            ) -> None:
+        self.assert_valid(rmw)
+        self.rmw = rmw
+
+    def __eq__(self, other: object) -> bool:
+        if isinstance(other, str):
+            return self.rmw == other
+        elif isinstance(other, RMWImplementation):
+            return self.rmw == other.rmw
+        else:
+            return False
+
+    def __str__(self) -> str:
+        return self.rmw
+
+    @classmethod
+    def is_valid(cls, rmw: str) -> bool:
+        return rmw in cls.ALL_SUPPORTED
+
+    @classmethod
+    def assert_valid(cls, rmw: str) -> None:
+        assert cls.is_valid(rmw), ("\n".join[
+            "RMW '%s' not supported." % rmw,
+            "RMW must be one of: '%s'" % cls.ALL_SUPPORTED
+        ])
+```
+
+Also check and push the modifications on the clearpath_common and ros2_kortex

clearpath/commanding_script.py

@@ -248,8 +248,8 @@ def move_arm_to(self, positions, duration):
         traj.points.append(point)
         self.arm_pub.publish(traj)
 
-        # Wait for the arm to complete the movement
-        self.wait_for_trajectory_completion(positions)
+        # Wait for the arm to complete the movement (optional)
+        #self.wait_for_trajectory_completion(positions)
 
     def wait_for_trajectory_completion(self, positions):
         """Block execution until the arm reaches the target trajectory."""

clearpath/gen3_husky.sh

@@ -9,9 +9,73 @@ fi
 sleep 5
 # After the Python script finishes, open a new terminal for each of the remaining commands
 gnome-terminal -- bash -c "ros2 launch clearpath_gz simulation.launch.py; exec bash"
+sleep 30
+
+# Function to retry a command until it succeeds
+retry_command() {
+    local cmd="$1"
+    local max_retries=10
+    local count=0
+    until eval "$cmd"
+    do
+        count=$((count + 1))
+        if [ $count -ge $max_retries ]; then
+            echo "Command failed after $max_retries attempts: $cmd"
+            return 1
+        fi
+        echo "Retry $count/$max_retries: $cmd"
+        sleep 2
+    done
+}
+
+# Reset the robot pose in Ignition Gazebo
+echo "Resetting robot pose..."
+retry_command "ign service -s /world/warehouse/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 5000 --req \"position: {x: 0.000925, y: 0.000067, z: 0.132280}, orientation: {x: 0, y: 0, z: 0.000007, w: 1}, name: 'a200_0000/robot'\""
+
+# Reset the grip box pose in Ignition Gazebo
+echo "Resetting grip box pose..."
+retry_command "ign service -s /world/warehouse/set_pose --reqtype ignition.msgs.Pose --reptype ignition.msgs.Boolean --timeout 5000 --req \"position: {x: 3.0, y: 0.0, z: 0.53}, orientation: {x: 0, y: 0, z: 0, w: 1}, name: 'grip_box'\""
 sleep 5
+
+# Publish the initial joint positions to the simulation
+echo "Setting arm initial joint positions..."
+ros2 topic pub /a200_0000/arm_0_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{
+  header: {
+    stamp: {sec: 0, nanosec: 0},
+    frame_id: ''
+  },
+  joint_names: [
+    'arm_0_joint_1',
+    'arm_0_joint_2',
+    'arm_0_joint_3',
+    'arm_0_joint_4',
+    'arm_0_joint_5',
+    'arm_0_joint_6',
+    'arm_0_joint_7'
+  ],
+  points: [
+    {
+      positions: [-0.23921483, 0.85088292, 0.1007404, 2.40223628, -0.07318166, -1.70646077, -1.63699667],
+      velocities: [],
+      accelerations: [],
+      effort: [],
+      time_from_start: {sec: 10, nanosec: 0}
+    }
+  ]
+}" -1
+echo "Setting gripper initial position..."
+ros2 action send_goal /a200_0000/robotiq_85_controller/gripper_cmd control_msgs/action/GripperCommand "{command:{position: 0.7, max_effort: 100.0}}"
+sleep 20
+# Prompt user to check simulation status before proceeding
+read -p "Check the simulation scene and the related terminal for any error or anomaly and check the roobt state using the Webapp then press Enter to continue if all is working well, otherwise relaunch the system again or eventually restart the computer"
+
 gnome-terminal -- bash -c "ros2 launch kortex_bringup gen3.launch.py robot_ip:=192.168.1.10 gripper:=robotiq_2f_85 launch_rviz:=false; exec bash"
 sleep 5
 gnome-terminal -- bash -c "ros2 run clearpath_manipulators gazebo_to_real_robot_node; exec bash"
 sleep 5
 gnome-terminal -- bash -c "ros2 run clearpath_manipulators gazebo_to_real_gripper_node; exec bash"
+
+read -p "Wait until the entire system is up and running then press Enter to activate the Keyboard control"
+
+gnome-terminal -- bash -c "python3 commanding_script.py; exec bash"
+

clearpath/rl_robot_config_init_script.py

@@ -27,36 +27,6 @@
 TIMEOUT_DURATION = 20
 
 
-def disable_joint_safety(device_config, device_manager):
-    safety_handle = Common_pb2.SafetyHandle()
-    """
-    safety_handle.identifier = 4 # Disabling joint high limit
-    safety_enable = device_config.GetSafetyEnable(safety_handle)
-    safety_enable.enable = False
-    # Extraction of the joints IDs
-    all_devices_info = device_manager.ReadAllDevices()
-    for dev in all_devices_info.device_handle:
-        if dev.device_type == 3 or dev.device_type == 4: #If the device is big or medium actuator (the only actuator types used in Gen3)
-            device_config.SetSafetyEnable(safety_enable,dev.device_identifier)
-            print("Joint high limit safety activation: ", device_config.GetSafetyEnable(safety_handle, dev.device_identifier).enable)
-    """
-    safety_handle.identifier = 8  # Disabling joint low limit
-    safety_enable = device_config.GetSafetyEnable(safety_handle)
-    safety_enable.enable = False
-    # Extraction of the joints IDs
-    all_devices_info = device_manager.ReadAllDevices()
-    for dev in all_devices_info.device_handle:
-        if (
-            dev.device_type == 3 or dev.device_type == 4
-        ):  # If the device is big or medium actuator (the only actuator types used in Gen3)
-            device_config.SetSafetyEnable(safety_enable, dev.device_identifier)
-            print(
-                "Joint low limit safety activation: ",
-                device_config.GetSafetyEnable(safety_handle, dev.device_identifier).enable,
-            )
-    return True
-
-
 # Create closure to set an event after an END or an ABORT
 def check_for_end_or_abort(e):
     """
@@ -136,7 +106,6 @@ def main():
         # Example core
         success = False
         success &= example_move_to_initial_position(base)
-        success &= disable_joint_safety(device_conf, device_manager)
 
         return 0 if success else 1
 

clearpath/robot.yaml

@@ -11,7 +11,7 @@ system:
     namespace: a200_0000
     domain_id: 0
     middleware:
-      implementation: rmw_fastrtps_cpp
+      implementation: rmw_cyclonedds_cpp
       discovery: simple
 
 platform: