Dynamixel 액추에이터를 MoveIt2로 조종할 시 토크(혹은 속도) 제한 문제

khykim · August 29, 2025, 12:37am

안녕하세요, Dynamixel Actuator를 Dynamixel SDK로 조종했다가, 현재 MoveGroup을 통한 ROS2 Control로 조종하기 위해서 *.ros2_control.xacro 로 바꾸었는데 문제가 발생하였습니다.

OS Version : Ubuntu 22.04
ROS2 Version : ROS2 Humble
Dynamixel Type : XM540-W270 (6개 모두 동일)

다음과 같이 기존 Dynamixel SDK를 통해 Hardware를 조종할 때 쓰는 방식과 Configuration 정보를 동일하기 ros2_control.xacro 파일에 작성하였습니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="kumdori_robot_ros2_control" params="name initial_positions_file">
        
        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>

        <ros2_control name="${name}" type="system">
            
            <hardware>
                <plugin>dynamixel_hardware_interface/DynamixelHardware</plugin>
                <param name="port_name">/dev/ttyDynamixel485</param>
                <param name="baud_rate">3000000</param>
                <param name="error_timeout_sec">0.5</param>
                <param name="dynamixel_model_folder">/param/dxl_model</param>
                <param name="number_of_joints">6</param>
                <param name="number_of_transmissions">6</param>
                <param name="transmission_to_joint_matrix">
                    1, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0,
                    0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 1
                </param>

                <param name="joint_to_transmission_matrix">
                    1, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0,
                    0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 1
                </param>
                
                <param name="ros_update_freq">50</param>
                <param name="dynamixel_state_pub_msg_name">dynamixel_hardware_interface/dxl_state</param>
                <param name="get_dynamixel_data_srv_name">dynamixel_hardware_interface/get_dxl_data</param>
                <param name="set_dynamixel_data_srv_name">dynamixel_hardware_interface/set_dxl_data</param>
                <param name="reboot_dxl_srv_name">dynamixel_hardware_interface/reboot_dxl</param>
                <param name="set_dxl_torque_srv_name">dynamixel_hardware_interface/set_dxl_torque</param>
                <param name="disable_torque_at_init">true</param>
            </hardware>

            <!-- 1 -->
            <joint name="left_upper_arm_to_left_lower_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 2 -->
            <joint name="left_shoulder_to_left_upper_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 3 -->
            <joint name="right_shoulder_to_right_upper_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 4 -->
            <joint name="right_upper_arm_to_right_lower_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 5 -->
            <joint name="skeleton_to_head_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 6 -->
            <joint name="star_to_head_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- ID: 1 -->
            <gpio name="dxl1">
                <param name="type">dxl</param>
                <param name="ID">1</param>
                <param name="Profile Velocity">5000</param>
                <param name="Profile Acceleration">2000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2824</param>
                <param name="Min Position Limit">1272</param> -->

                <command_interface name="Goal Position"/>

                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 2 -->
            <gpio name="dxl2">
                <param name="type">dxl</param>
                <param name="ID">2</param>
                <param name="Profile Velocity">5000</param>
                <param name="Profile Acceleration">2000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2424</param>
                <param name="Min Position Limit">1672</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 6 -->
            <gpio name="dxl3">
                <param name="type">dxl</param>
                <param name="ID">6</param>
                <param name="Profile Velocity">5000</param>
                <param name="Profile Acceleration">2000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <command_interface name="Goal Position"/>              
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>
            
            <!-- ID: 7 -->
            <gpio name="dxl4">
                <param name="type">dxl</param>
                <param name="ID">7</param>
                <param name="Profile Velocity">5000</param>
                <param name="Profile Acceleration">2000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2824</param>
                <param name="Min Position Limit">1272</param> -->

                <command_interface name="Goal Position"/>

                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 12 (Neck) -->
            <gpio name="dxl5">
                <param name="type">dxl</param>
                <param name="ID">12</param>
                <param name="Profile Velocity">3000</param>
                <param name="Profile Acceleration">1000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2560</param>
                <param name="Min Position Limit">1535</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 13 (Star Connector) -->
            <gpio name="dxl6">
                <param name="type">dxl</param>
                <param name="ID">13</param>
                <param name="Profile Velocity">5000</param>
                <param name="Profile Acceleration">2000</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">4085</param>
                <param name="Min Position Limit">10</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

        </ros2_control>
        
    </xacro:macro>
</robot>

즉, 요악을 하자면 **Time-based(4) Drive Mode로써 Profile Velocity를 5000(ms), Profile Acceleration을 2000(ms)**으로 해주었습니다.

이렇게 했을 때, 하드웨어의 마찰 혹은 무게에 따라서 Move Group에서 Planning된 결과에 대해서 Execution을 시켜봤을 때 매우 느려지거나 혹은 힘이 부족해서인지 가다가 멈추는 문제가 발생합니다.

이에, 몇몇 방안을 조사해보고 다음의 링크를 확인하여서 Profile Velocity와 Acceleration을 모두 0으로 바꾸어 주었습니다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="kumdori_robot_ros2_control" params="name initial_positions_file">
        
        <xacro:property name="initial_positions" value="${load_yaml(initial_positions_file)['initial_positions']}"/>

        <ros2_control name="${name}" type="system">
            
            <hardware>
                <plugin>dynamixel_hardware_interface/DynamixelHardware</plugin>
                <param name="port_name">/dev/ttyDynamixel485</param>
                <param name="baud_rate">3000000</param>
                <param name="error_timeout_sec">0.5</param>
                <param name="dynamixel_model_folder">/param/dxl_model</param>
                <param name="number_of_joints">6</param>
                <param name="number_of_transmissions">6</param>
                <param name="transmission_to_joint_matrix">
                    1, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0,
                    0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 1
                </param>

                <param name="joint_to_transmission_matrix">
                    1, 0, 0, 0, 0, 0,
                    0, 1, 0, 0, 0, 0,
                    0, 0, 1, 0, 0, 0,
                    0, 0, 0, 1, 0, 0,
                    0, 0, 0, 0, 1, 0,
                    0, 0, 0, 0, 0, 1
                </param>
                
                <param name="ros_update_freq">50</param>
                <param name="dynamixel_state_pub_msg_name">dynamixel_hardware_interface/dxl_state</param>
                <param name="get_dynamixel_data_srv_name">dynamixel_hardware_interface/get_dxl_data</param>
                <param name="set_dynamixel_data_srv_name">dynamixel_hardware_interface/set_dxl_data</param>
                <param name="reboot_dxl_srv_name">dynamixel_hardware_interface/reboot_dxl</param>
                <param name="set_dxl_torque_srv_name">dynamixel_hardware_interface/set_dxl_torque</param>
                <param name="disable_torque_at_init">true</param>
            </hardware>

            <!-- 1 -->
            <joint name="left_upper_arm_to_left_lower_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 2 -->
            <joint name="left_shoulder_to_left_upper_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 3 -->
            <joint name="right_shoulder_to_right_upper_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 4 -->
            <joint name="right_upper_arm_to_right_lower_arm_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 5 -->
            <joint name="skeleton_to_head_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- 6 -->
            <joint name="star_to_head_joint">
                <command_interface name="position"/>
                <command_interface name="velocity"/>
                <state_interface name="position"/>
                <state_interface name="velocity"/>
                <state_interface name="effort"/>
            </joint>

            <!-- ID: 1 -->
            <gpio name="dxl1">
                <param name="type">dxl</param>
                <param name="ID">1</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2824</param>
                <param name="Min Position Limit">1272</param> -->

                <command_interface name="Goal Position"/>

                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 2 -->
            <gpio name="dxl2">
                <param name="type">dxl</param>
                <param name="ID">2</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2424</param>
                <param name="Min Position Limit">1672</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 6 -->
            <gpio name="dxl3">
                <param name="type">dxl</param>
                <param name="ID">6</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <command_interface name="Goal Position"/>              
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>
            
            <!-- ID: 7 -->
            <gpio name="dxl4">
                <param name="type">dxl</param>
                <param name="ID">7</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2824</param>
                <param name="Min Position Limit">1272</param> -->

                <command_interface name="Goal Position"/>

                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 12 (Neck) -->
            <gpio name="dxl5">
                <param name="type">dxl</param>
                <param name="ID">12</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">2560</param>
                <param name="Min Position Limit">1535</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

            <!-- ID: 13 (Star Connector) -->
            <gpio name="dxl6">
                <param name="type">dxl</param>
                <param name="ID">13</param>
                <param name="Profile Velocity">0</param>
                <param name="Profile Acceleration">0</param>
                <param name="Drive Mode">4</param>
                <param name="Operating Mode">3</param>
                <param name="Position P Gain">800</param>
                <param name="Position I Gain">0</param>
                <param name="Position D Gain">0</param>
                <param name="Goal PWM">885</param>

                <!-- <param name="Max Position Limit">4085</param>
                <param name="Min Position Limit">10</param> -->

                <command_interface name="Goal Position"/>
                
                <state_interface name="Present Position"/>
                <state_interface name="Present Velocity"/>
                <state_interface name="Present Current"/>
            </gpio>

        </ros2_control>
        
    </xacro:macro>
</robot>

위 방법을 적용했을 때, Move Group을 통한 제어시 Dyanmixel Actuator가 어느 정도의(?) 무게와 마찰을 극복하고 작동하는 것을 확인하였습니다. (사실상 성공적으로 작동하였습니다)

하지만, 이론 상으로는 Profile Velocity와, Profile Acceleration 값을 모두 0로 놓으면 속도가 매우 빨라져야 하는데, Move Group으로 제어를 하게 됨으로써 속도가 전혀 빨라짐 없이 안정적으로 움직이는 것을 보았는데요.

이에 따라서 저는 Dynamixel과 Move Group의 속도 설정(혹은 제한이) 중첩되었다고 생각을 하게 되었고, 따라서 조금의 무게 증가와 마찰이 있을 때 작동이 원활하지 않는다고 생각을 하게 되었는데요.

결론적으로, Dynamixel과 Move Group을 연결해서 사용할 때에는 Profile Velocity와 Profile Acceleration을 모두 0으로 설정해야 하는 것일 까요??

YunWonho · August 29, 2025, 7:39am

안녕하세요! moveit config 파일을 보시면 다음과 같은 파일이 있습니다.

github.com/ROBOTIS-GIT/open_manipulator

open_manipulator_moveit_config/config/open_manipulator_x/joint_limits.yaml

main

# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed

# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1

# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
  gripper_left_joint:
    has_velocity_limits: true
    max_velocity: 5.0
    has_acceleration_limits: true
    max_acceleration: 5.0
  gripper_right_joint:
    has_velocity_limits: true
    max_velocity: 5.0
    has_acceleration_limits: true
    max_acceleration: 5.0

This file has been truncated. show original

이 파일에서 moveit 에서 내보내는 velocity 값과 acceleration 값을 설정할 수 있고.

Profile Velocity와, Profile Acceleration은 모터단에서 velcoity와 acceleration을 설정할 수 있습니다.

따라서 Profile Velocity와, Profile Acceleration값이 작다면 moveit에서 보내는 속도와 가속도를 모터에서 못따라가는 것입니다.

khykim · September 25, 2025, 7:56am

안녕하세요,

그렇다면 moveit2를 사용해서 planning을 한다고 하면 Profile Velocity와, Profile Acceleration 값을 모두 0으로 설정해도 안전하다고 볼수도 있는 걸까요?

아니면 Moveit2에서 Time-based Mode일때의 Profile Velocity, Acceleration 시간을 조사해서 맞춰줘야 하는 걸 까요?

감사합니다!

roboteer · September 25, 2025, 2:10pm

If you don’t mind watching a YouTube video in English this one explains how to use Profile Acceleration and Profile Velocity for a typical Robotis actuator.

khykim · October 8, 2025, 4:07am

Thanks a lot for your support!!
I think it would be useful to make a progress in my project.