ROS2 MoveIt2 에서 UR3e + Intel RealSense Pick and Place 예제 수행

 

 

점점 심연 속으로 들어왔다는 느낌이 들고 있습니다.

가끔 어떤 자료를 찾는 것 조차 버겁게 느껴집니다.

 

학문적으로 수준이 높아서라기보단,

특정한 주제에 대해 꽤 깊게 들어왔다는 말을 하고 싶습니다.

더 솔직히 말하면, ROS2로 UR 로봇에 vision을 다는 task를 수행하는 것 자체가 금전적으로 장벽이 높고,

수행하더라도 그 과정을 전부 open하지 않는 것 같습니다.

대부분이 짧은 유튜브 영상에 결과물만 담을 뿐인 것 같습니다.

물론 ROS2 에 대한 자료가 아직은 많이 부족하다는 느낌도 받습니다.

이 글이 이 분야에 있는 누군가 또는 미래의 저에게 많은 도움이 되길 바랍니다.

 

이번 글은 ROS2 MoveIt2 에서 UR3e + Intel RealSense Pick and Place 동작을 수행한 과정에 대한 글입니다.

지난 번에 작성한 글에 추가적으로 Gripper를 달고 Grasping action을 넣었고,

3D 프린팅을 통해 그리퍼에 Intel RealSense D435를 장착시켰습니다.

ROS2 MoveIt2 이용하여 UR3e Pick and Place 수행하기

 

ROS2 UR3e + Intel RealSense 임시 결합 하기

 

이번에는 가장 먼저 결과부터 보여드리도록 하겠습니다.

 

아직 vision을 통한 Picking 위치를 잡는 것은 하지 않는 상태고,

고정 위치에 대해서 동작합니다.

능력이 닿는 대로 다음 주 내에 ROS2 패키지를 보완해서

Picking 위치를 잡는 것을 추가해보겠습니다.

 

그럼 지난 글에 이어 어떻게 발전시켰는지 부터 설명드리겠습니다.

먼저 Gripper에 대해서 설명하겠습니다.

아래 Aliexpress 의 4 Finger Electric gripper를 이용하였습니다.

NEMA 17 Step Motor 로 작동합니다.

135022.0₩ 5% OFF|로봇 그리퍼 기계 발톱 | 로봇 발톱 로봇 그리퍼 | 실리콘 정렬 그리퍼-프로그래밍

 

여기에 MKS42C Motor Driver를 붙여서 그리퍼가 작동하도록 만들었습니다.

12307.0₩ 15% OFF|2022 신형 스테퍼 모터 MKS SERVO42C 폐 루프 SMT32 컨트롤러 Nema 17 서보 모터| | - AliExpre

 

해당 과정을 수행한 이전 글입니다.

다만 아래 글은 Python3 로 구현되었기 때문에, cpp serial 통신으로의 수정이 필요했습니다.

이 부분은 아래에서 추가적으로 설명하겠습니다.

MKS 42C 드라이버를 이용한 Step Motor 제어와 Gripper 작동 시키기

 

 

최종 형태입니다. 연결을 하기 위해 M3 60mm 의 긴 나사를 별도 주문해서 결합했고,

Intel RealSense가 달릴 수 있는 연결부를 3D 모델링하고 프린팅 했습니다.

이 때 카메라가 얼마나 로봇과 떨어져있는지가

뒤에서 설명할 URDF 파일에 반영됩니다.

 

지난 글에서와 마찬가지로 URDF를 먼저 수정해주겠습니다.

해당 파일은 다음 경로에 위치합니다.

~/workspace/ros_ur_driver/src/Universal_Robots_ROS2_Description/urdf/ur.urdf.xacro

 

지난 번엔 단순히 box로 카메라를 표현했는데,

이번에는 Intel realsense 패키지 내부에 있는 mesh 파일을 입혀주겠습니다.

 

mesh 파일 경로와 camera_joint 의 origin xyz, rpy 는 적절하게 수정하셔야 합니다.

저의 경우 처음에 이렇게 작동시키면 카메라가 이상한 방향에 화면을 출력시켰습니다.

카메라 출력 방법은 위에 첨부했던 임시 결합과 관련된 이전 글을 확인하시기 바랍니다.

<!-- RealSense Camera Link -->
    <link name="camera_link">
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="file:///home/user/workspace/ros_ur_driver/src/realsense-ros/realsense2_description/meshes/d435.dae"/>
          <!--<box size="0.025 0.09 0.025"/>-->
        </geometry>
        <!--<material name="black"/>-->
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="file:///home/user/workspace/ros_ur_driver/src/realsense-ros/realsense2_description/meshes/d435.dae"/>
          <!--<box size="0.025 0.09 0.025"/>-->
        </geometry>
        <!--<material name="black"/>-->
      </collision>
    </link>

   <!-- Joint connecting wrist_3_link and camera_link  -->
   <joint name="camera_joint" type="fixed">
     <parent link="wrist_3_link"/>
     <child link="camera_link"/>
     <origin xyz="-0.0389 -0.0389 0.037" rpy="0 4.71 0.785"/>
   </joint>

   <!-- Gripper Link -->
   <link name="gripper_link">
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
         <geometry>
           <box size="0.185 0.095 0.060"/>
         </geometry>
         <material name="black"/>
     </visual>
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
         <box size="0.185 0.095 0.060"/>
       </geometry>
       <material name="black"/>
     </collision>
   </link>

   <!-- Joint connecting wrist_3_link and gripper_link  -->
   <joint name="gripper_joint" type="fixed">
     <parent link="wrist_3_link"/>
     <child link="gripper_link"/>
     <origin xyz="0 0 0.093" rpy="0 4.71 0.785"/>
   </joint>

 

저는 mesh 파일 자체에 수정이 필요했습니다.

blender를 열고 D435.dae 파일을 불러오고

다음과 같이 Transform의 XYZ Rotation 수정 후 저장해주었습니다.

 

Gripper는 간단하게 박스 형태로 충돌이 이루어지지 않게만 URDF를 등록했습니다.

최종적으로 다음과 같은 형태로 moveit이 실행됩니다.

 

URDF는 완료되었으니 cpp를 이용해 Gripper가 정상적으로 작동하는지 확인할 차례입니다.

MKS42c가 워낙 개복치같은 친구라..

처음에 애를 조금 많이 먹었지만

익숙해지니 버그 걸렸을 때 해결하는 능숙함이 생겼습니다.

주의해야할 점은 그리퍼의 작동 범위 이상으로 모터가 돌아가면 무조건 문제가 발생합니다.

가동 범위 내에서 잘 조절하시는게 좋을 것 같습니다.

Sample 코드만 첨부드리고, 적절히 수정하셔서

작동 전에 Gripper가 최대로 열려 있는 상태로 준비하여야 합니다.

 

다음 코드를 통해 Linux serial port를 통해 Gripper를 테스트 해볼 수 있습니다.

window 환경의 경우 라이브러리와 함수 등의 수정이 필요한 것으로 알고있습니다.

Makefile을 통해 컴파일해서 사용하시기 바랍니다.

#include <iostream>
#include <unistd.h>     // UNIX Standard Definitions
#include <fcntl.h>      // File Control Definitions
#include <termios.h>    // POSIX Terminal Control Definitions
#include <string>
#include <sstream>
#include <iomanip>
#include <cstring>
#include <vector>
#include <cctype>
#include <cstdlib>

using namespace std;

// 입력된 속도에 대해 16진수 명령을 유도하는 함수
string VelToHex(int velocity) {
    stringstream ss;
    if (-127 <= velocity && velocity <= 127) {
        int adjusted_val = velocity;
        
        if (velocity < 0) {
            adjusted_val = 128 + abs(velocity); // 음수 값을 변환
        }
        ss << setw(2) << setfill('0') << hex << adjusted_val;
    }
    else {
        throw invalid_argument("Wrong Velocity");
    }
    return ss.str();
}



// 입력된 각도에 대해 16진수 명령을 유도하는 함수
string AngToHex(int angle) {
    double increment_per_degree = (0x00000C80 - 0x00000000) / 360.0;
    int hex_value = static_cast<int>(increment_per_degree * angle);
    stringstream ss;
    ss << setw(8) << setfill('0') << uppercase << hex << hex_value;
    return ss.str();
}

// 16진수 문자열을 바이트 배열로 변환하는 함수
vector<unsigned char> HexStringToBytes(const string& hex) {
    vector<unsigned char> bytes;

    for (size_t i = 0; i < hex.length(); i += 2) {
        string byteString = hex.substr(i, 2);
        unsigned char byte = static_cast<unsigned char>(strtol(byteString.c_str(), nullptr, 16));
        bytes.push_back(byte);
    }

    return bytes;
}

// 16진수 데이터의 check Sum을 계산하는 함수
unsigned char CheckSum(const vector<unsigned char>& data) {
    unsigned int sum = 0;
    for (size_t i = 0; i < data.size(); i++) {
        sum += data[i];
    }
    return static_cast<unsigned char>(sum % 256);
}

// 시리얼 포트로 데이터를 전송하는 함수
void TxSerial(int serial_port, const string& data_hex) {
    cout << data_hex << "\n";
    vector<unsigned char> data = HexStringToBytes(data_hex);
    unsigned char checksum = CheckSum(data);
    data.push_back(checksum); // 체크섬 바이트 추가
    write(serial_port, data.data(), data.size());
}

int main() {
    const char* portname = "/dev/ttyUSB0";
    int serial_port = open(portname, O_RDWR | O_NOCTTY);

    if (serial_port < 0) {
        cerr << "Error " << errno << " opening " << portname << ": " << strerror(errno) << endl;
        return 1;
    }

    struct termios tty;
    memset(&tty, 0, sizeof tty);

    if (tcgetattr(serial_port, &tty) != 0) {
        cerr << "Error " << errno << " from tcgetattr: " << strerror(errno) << endl;
        return 1;
    }

    cfsetospeed(&tty, B38400);
    cfsetispeed(&tty, B38400);

    tty.c_cflag |= (CLOCAL | CREAD);   // Enable reading and ignore control lines
    tty.c_cflag &= ~CSIZE;             // Clear bit mask for data bits
    tty.c_cflag |= CS8;                // 8 data bits
    tty.c_cflag &= ~PARENB;            // No parity bit
    tty.c_cflag &= ~CSTOPB;            // 1 stop bit
    tty.c_cflag &= ~CRTSCTS;           // No hardware flow control

    tty.c_lflag &= ~ICANON;
    tty.c_lflag &= ~ECHO;
    tty.c_lflag &= ~ECHOE;
    tty.c_lflag &= ~ECHONL;
    tty.c_lflag &= ~ISIG;

    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);

    tty.c_oflag &= ~OPOST;
    tty.c_oflag &= ~ONLCR;

    tty.c_cc[VTIME] = 1;
    tty.c_cc[VMIN] = 0;

    if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
        cerr << "Error " << errno << " from tcsetattr" << endl;
        return 1;
    }


    while (true) {
        cout << "\nOpen = 1 | Close = 2 | 입력 종료 = q" << endl;
        char mode;
        cin >> mode;

        if (mode == '1') {
            string data_hex = "e0fd" + VelToHex(10) + AngToHex(90);
            TxSerial(serial_port, data_hex);
            sleep(2);
        }
        else if (mode == '2') {
            string data_hex = "e0fd" + VelToHex(-10) + AngToHex(90);
            TxSerial(serial_port, data_hex);
            sleep(2);
        }
        else if (mode == 'q') {
            break;
        }
        else {
            cout << "Wrong Input!" << endl;
        }
    }

    close(serial_port);
    return 0;
}

 

저는 처음에 ttyUSB0 경로가 인식되지 않아 확인해보았습니다.

sudo dmesg | grep tty

[ 2954.628721] usb 1-2: ch341-uart converter now attached to ttyUSB0

[ 2955.166628] usb 1-2: usbfs: interface 0 claimed by ch341 while 'brltty' sets config #1

[ 2955.167198] ch341-uart ttyUSB0: ch341-uart converter now disconnected from ttyUSB0

 

이런 충돌 문제가 발생해 아래 명령으로 삭제해주었습니다.

sudo apt-get remove brltty

이후에 sudo reboot를 해주어야 잘 작동했습니다.

 

모든 것이 다 준비되셨다면, cpp를 수정할 차례입니다.

ur_pick_and_place_moveit 의 src 내부의 ur_pick_and_place_moveit.cpp 를 다음과 같이 수정합니다.

 

 

Grasping 동작과 Dropping 동작을 추가하여 작성했습니다.

이제 이전에 했던 pick and place와 동일하게 실행해줍니다.

4개의 터미널이 필요합니다.

첫 번째 터미널에서 build 를 실행하고, 카메라 노드를 실행하겠습니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

colcon build

source install/setup.bash

ros2 launch realsense2_camera rs_launch.py depth_module.profile:=1280x720x30 pointcloud.enable:=true

 

두 번째 터미널에서는 rviz를 실행해줍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_calibration calibration_correction.launch.py robot_ip:=192.168.1.101 target_filename:="${HOME}/my_robot_calibration.yaml"

ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur3e robot_ip:=192.168.1.101 launch_rviz:=false

 

터치패드에서 external control 프로그램을 활성화하고

세 번째 터미널에서 moveit! 을 실행해줍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_moveit_config ur_moveit.launch.py ur_type:=ur3e launch_rviz:=true robot_ip:=192.168.1.101 reverse_ip:=192.168.1.102

 

네 번째 터미널에서 pick and place를 실행합니다.

로봇이 움직이니 주의하시길 바랍니다.

export COLCON_WS=~/workspace/ros_ur_driver

cd $COLCON_WS

source install/setup.bash

ros2 launch ur_pick_and_place_moveit ur_pick_and_place_moveit_launch.py

 

 

 

매일 조금씩 함께 성장하는 저와 로봇을 보면 기분이 좋습니다.

많은 도움 되셨길 바라며 글 마치겠습니다.