Enhancing Tasks with LIMO & myCobot Integration (B)

Introduction

Prelude: In the previous article, we introduced a project that utilizes the LIMO cobot to implement a composite robotic system capable of executing complex tasks. This project was discussed from three aspects: conceptual design, system architecture, and key components.

This article delves into the core parts of the project, which are also divided into three main sections: the robotic arm's visual grasping, the functionality of LIMO Pro within ROS, and mapping, navigation, and obstacle avoidance, as well as the integration of both systems.

Equipment Preparation

1. myCobot 280 M5stack

2. myCobot Adaptive Gripper

3. myCobot Camera Flange 2.0

4. LIMO PRO

Robotic Arm Grab Vision

This section describes the appearance of the robotic arm after the installation of the Adaptive Gripper and Camera Flange 2.0.

The camera flange 2.0 we use is a 2D camera, which cannot rely on the camera itself to obtain the three dimensions (length, width, and height) of an object. However, we can use markers to obtain the target object's dimensions. Common markers include ArUco, STag, AR codes, and AprilTags. In this case, we utilize the STag algorithm for visual recognition.

STag Marker System

The STag is a marker system designed for high stability and precise three-dimensional positioning. It is particularly suited for environments with obstructions and varying lighting conditions.

A video demonstrates the recognition performance of STag markers, ARToolKit+, ArUco, and RUNE-Tag codes under the same conditions. The video showcases STag's strong adaptability to environmental changes and its high reliability in complex scenes, making it the preferred choice for applications requiring high-precision tracking and positioning.

Additionally, there is a paper that specifically discusses the stable benchmark marker system of STag for those interested in a more in-depth understanding.

https://arxiv.org/abs/1707.06292

The STag system is compatible with ROS and has a ROS package. It is written in C++ and also supports Python use.

● For C++/ROS, you can visit the GitHub repository at https://github.com/bbenligiray/stag.

● For Python, the repository is available at https://github.com/ManfredStoiber/stag-python.

Simple code:

import cv2
import stag
import numpy as np

# Load camera parameters
camera_params = np.load("camera_params.npz")
mtx, dist = camera_params["mtx"], camera_params["dist"]

# Initialize STag detector
stag_detector = stag.detectMarkers(mtx, dist)

# Initialize video capture
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
    
    # Apply camera calibration (optional)
    frame_undistorted = cv2.undistort(frame, mtx, dist, None, mtx)
    # Detect STag markers
    (corners, ids, rejected_corners) = stag.detectMarkers(frame_undistorted, 21)
    # Draw the detected markers and their IDs
    stag.drawDetectedMarkers(frame_undistorted, corners, ids)
    # Draw the rejected candidate areas in red
    stag.drawDetectedMarkers(frame_undistorted, rejected_corners, border_color=(255, 0, 0))

    # Display the results
    cv2.imshow("STag Detection", frame_undistorted)

    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

After marking the STag code, parameters of the four corners of the marker can be obtained.

(array([[[257., 368.],
        [185., 226.],
        [345., 162.],
        [420., 302.]]], dtype=float32),)

Once the marker coordinates are acquired, the cv2.solvePnP function from OpenCV can be used to calculate the rotation and offset of the marker relative to the camera. This function requires the 3D coordinates of the marker (its position in the physical world) and the corresponding 2D image coordinates (i.e., the detected corners), as well as the camera's intrinsic parameters and distortion coefficients. solvePnP returns a rotation vector (rvec) and a translation vector (tvec), which describe the transformation from the marker coordinate system to the camera coordinate system. Thus, the position and orientation of the marker can be calculated based on these parameters

Pseudo code:

def estimate_pose(corners):
    #Do some calculations with PnP, camera rotation and offset
    return rvec，tvec
    
def convert_pose_to_arm_coordinates(rvec, tvec):
    # Convert rotation and translation vectors to x, y, z, rx, ry, rz in the robot coordinate system
    return x, y, z, rx, ry, rz
    
def convert_grab(object_coord_list):
    #Do some calculations to convert the coordinates into grasping coordinates for the robotic arm
    return grab_position
    
    
cap = cv2.VideoCapture(0)
while cap.isOpened():
    ret, frame = cap.read()
    if not ret:
        break
        
    
    maker = (corners, ids, rejected_corners) = stag.detectMarkers(image, 21)
    
    rvec, tvec = stag.estimate_pose(marker)
    
    object_coord_list = convert_pose_to_arm_coordinates(rvec, tvec)
    grab_position = convert_grab(object_coord_list)
    mycobot.move_to_position(grab_position)

Robotic Arm Control

The above code is a rough grabbing process. The more complicated parts have been solved. Next, we deal with the motion control of the robotic arm, using the pymycobot library.

from pymycobot import MyCobot

#Create an instance and link the robotic arm
mc = MyCobot('com3',115200)

#Control the robotic arm with angle
mc.send_angles(angles_list,speed)

#Control the robotic arm using coordinates
mc.send_coords(coords_list,speed,mode)

#Control gripper,value-0~100
mc.set_gripper_value(value,speed)

Because the robotic arm has many open interfaces, we only need to use coordinate control and gripper control.

Limo Pro Mapping and Navigation

After completing the functionality of the robotic arm's grabbing, the next step is to implement the mapping and navigation module for LIMO.

First, we need to create a map. With a map, we can then navigate on it, perform fixed-point cruising, and other functions. Currently, there are various algorithms for mapping. Given that our constructed scenario is not very large and the environment is relatively static, we choose to use the gmapping algorithm for implementation.

Mapping

Gmapping is a commonly used open-source SLAM algorithm based on the filter SLAM framework. It effectively utilizes wheel odometer information and does not require a high frequency from the laser radar. When building maps of small scenes, it requires less computational power and achieves higher accuracy. Here, we implement Limo's mapping by using the Gmapping package wrapped in ROS.

Note: The following features are pre-packaged and can be directly utilized.

Launch a new terminal and enter the command:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

Then launch the gmapping mapping algorithm. Open another new terminal, and enter the command:

roslaunch limo_bringup limo_gmapping.launch

After launching successfully, the rviz visualization tool will start up. The interface is shown in the figure.

At this time, the handle can be set to remote control mode and control limo mapping.

After building the map, run the following command to save the map to the specified directory:

1. Switch to the directory where you need to save the map, save the map to ~/agilex_ws/src/limo_ros/limo_bringup/maps/, and enter the command in the terminal:

cd ~/agilex_ws/src/limo_ros/limo_bringup/maps/

2. After switching to /agilex_ws/limo_bringup/maps, continue to enter the command in the terminal:

rosrun map_server map_saver -f map1

Note: map1 is the name of the saved map, and duplicate names should be avoided when saving the map

Navigation

Having used gmapping for mapping, we now proceed with navigation. The key to navigation lies in two major parts: robot localization and path planning. For these core components, ROS provides the following two packages:

(1) move_base: Implements optimal path planning in robot navigation.

(2) amcl: Implements robot localization in a two-dimensional map.

Based on the above two packages, ROS offers a complete navigation framework. The robot only needs to publish the necessary sensor information and the target location for navigation, and ROS can complete the navigation function. In this framework, the move_base package provides the main execution and interaction interface for navigation. To ensure the accuracy of the navigation path, the robot must precisely locate its position, a functionality implemented by the amcl package.

During navigation, two algorithms, DWA and TEB, are employed for global and local path planning, respectively. These algorithms ensure that the LIMO can safely proceed to its destination without colliding with obstacles.

（1）First launch the LiDAR and enter the command in the terminal:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

（2）Launch the navigation and enter the command in the terminal:

roslaunch limo_bringup limo_navigation_diff.launch

After launching successfully, the rviz interface will be shown in the figure below:

Note: If you need to customize the opened map, please open the limo_navigation_diff.launch file to modify the parameters. The file directory is: ~/agilex_ws/src/limo_ros/limo_bringup/launch. Please modify map02 to the name of the map that needs to be replaced.

（3）After launching the navigation, it may be observed that the laser-scanned shape does not align with the map, requiring manual correction. To rectify this, adjust the actual position of the chassis in the scene displayed on the rviz map. Use the rviz tools to designate an approximate position for the vehicle, providing it with a preliminary estimation. Subsequently, use the handle tool to remotely rotate the vehicle until automatic alignment is achieved. Once the laser shape overlaps with the scene shape on the map, the correction process is concluded. The operational steps are outlined as follows:

The correction is completed:

（4）Set the navigation goal point through 2D Nav Goal.

A purple path will be generated on the map. Switch the handle to command mode, and Limo will automatically navigate to the goal point.

Limo path inspection

If we want to move back and forth on a road, we need to enable the path inspection function, which will be used later.

1）First launch the LiDAR and enter the command in the terminal:

roslaunch limo_bringup limo_start.launch pub_odom_tf:=false

（2）Launch the navigation and enter the command in the terminal:

roslaunch limo_bringup limo_navigation_diff.launch

（3）Launch the path recording function. Open a new terminal, and enter the command in the terminal:

roslaunch agilex_pure_pursuit record_path.launch

After the path recording is completed, terminate the path recording program, and enter the command in the terminal: Ctrl+c.

（4）Launch the path inspection function. Open a new terminal, and enter the command in the terminal:

Note: Switch the handle to command mode.

roslaunch agilex_pure_pursuit pure_pursuit.launch

Integration of Both Systems

Having separately completed the myCobot robotic arm's visual grasping, LIMO's mapping, navigation, and patrol functions, we now aim to integrate these components within the ROS (Robot Operating System) framework. Our predefined scenario involves LIMO performing fixed-point patrols, stopping upon encountering a marker, waiting for the robotic arm to execute object grasping, and then moving to the next location after completion.

Functional Node Distribution

Implementing a functionality within ROS involves multiple steps and components, including nodes, topics, services, the parameter server, and actions. Based on ROS's functional node architecture, we have identified the distribution of nodes and their interactions:

1. Image Recognition Node

• Responsibility: Continuously receives image streams from the camera and uses image recognition algorithms (such as OpenCV or deep learning models) to detect specific markers.
• Input: Image stream from the camera.
• Output: Publishes a message to a specific topic (e.g., /marker_detected) when a marker is detected.

2. Control Node

• Responsibility: Manages the robot's movement, including starting, stopping, and continuing patrols.
• Input: Subscribes to the /marker_detected topic to listen for outputs from the image recognition node. It may also subscribe to a topic specifically for receiving manual control commands (e.g., /control_commands).
• Output: Sends control commands to the robot's underlying control system (e.g., nodes driving the motors).

3. Task Execution Node

• Responsibility: Performs specific tasks upon encountering a marker, which could include data collection, status reporting, etc.
• Input: Listens for instructions from the control node, indicating when to start executing tasks.
• Output: Feedback on task completion status, which may be sent to the control node or a dedicated status topic (e.g., /task_status).

4. Navigation and Path Planning Node

Summary

This scenario has been preliminarily completed. However, many details can still be added to enrich it further. For example, adding moving obstacles during the journey or setting up traffic lights or similar objects could make the scenario more closely resemble real-life situations. If you have any suggestions for improvement or ideas on what you would like to achieve using the LIMO cobot, feel free to share your thoughts. Your feedback and likes are our greatest motivation for updates!