ROS2 Low-Level Motion Reference Example

The low-level motion development example demonstrates how to reset the robot from any initial joint position to a ready state, and then control ankle joint swinging and finger positions of robot using two different control modes.
The example source code is located in pnd_ros2/example and can be accessed from here.
The source directory is organized by robot model, with corresponding sample code as shown below:

├── adam_lite
│   └── adam_lite_low_level_example.py
├── adam_sp
│   └── adam_sp_low_level_example.py
├── adam_pro
│   └── adam_pro_low_level_example.py
└── adam_u
    └── adam_u_low_level_example.py

How to Run

Make sure the ROS2 msg interfaces are built successfully. Users can define their own $ROOT directory.

cd $ROOT
git clone https://github.com/pndbotics/pnd_ros2.git
cd pnd_ros2
colcon build

2. Connect the robot via Ethernet cable. For details, refer to Environment Configuration
3. Run the reference example. The following steps use Adam_Pro as an example.

cd $ROOT/pnd_ros2

# Source ROS2 environment
source /opt/ros/humble/setup.bash

# Source pnd_adam ROS2 message interfaces
source install/setup.bash 

# Run the example
python3 example/adam_pro/adam_pro_low_level_example.py

Code Explanation

The following code targets the Adam Pro model. Examples for other models share a similar structure, with differences mainly in joint index definitions.

Core Types and Functional Overview

Type Name	Description
ADAMJointIndex	Joint indices sorted by joint name. Also serves as the index of each joint in the `LowCmd.MotorCmd` command array
LowCmd	ROS2 message structure related to motor commands
LowState	ROS2 message structure related to motor states
HandCmd	ROS2 message structure related to finger commands
DemonController	Core control logic class (ROS2 node)

The main program of the example is defined in adam_pro_low_level_example.py, where a DemonController ROS2 node is instantiated.
In the constructor of DemonController, two publishers, one subscriber, and a timer are created, along with the corresponding ROS2 message structures:

Define finger position parameters for a closed hand (DemonController.close_hand) and open hand (DemonController.open_hand)
Create motor position publisher for the lowcmd topic and finger position publisher for the handcmd topic
Subscribe to the lowstate topic for low-level state feedback, handled by DemonController.getLowState
Periodically call Custom.LowCmdWrite at the interval defined by DemonController.control_dt_

The corresponding code is shown below:

class DemonController(Node):
    def __init__(self):
        super().__init__('demon_controller')
        self.time_ = 0.0
        self.control_dt_ = 0.0025  # [2ms]
        self.duration_ = 3.0    # [3 s]
        self.counter_ = 0

        self.lowcmd_pub_ = self.create_publisher(LowCmd, 'lowcmd', 10)
        self.handcmd_pub_ = self.create_publisher(HandCmd, 'handcmd', 10)
        self.getstate_flag = False

        self.timer = self.create_timer(self.control_dt_, self.LowCmdWrite)
        self.mutex = threading.Lock()

        self.low_state = LowState()
        self.low_state.motor_state = [MotorState() for _ in range(ADAM_PRO_NUM_MOTOR)]
        # print("init len(low_state.motor_state)=", len(self.low_state.motor_state))

        self.low_cmd = LowCmd()
        self.low_cmd.motor_cmd = [MotorCmd() for _ in range(ADAM_PRO_NUM_MOTOR)]
        self.hand_cmd = HandCmd()

        self.close_hand = np.array([500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500], dtype=int)
        self.open_hand = np.array([1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000,  1000, 1000, 1000, 1000], dtype=int)


        self.lowstate_sub_ = self.create_subscription(
            LowState,
            "lowstate",
            self.getLowState,
            10)

Low-Level State Callback Function

When a lowstate message is received, the program automatically calls the CDemonController.getLowState callback function.
In this callback, motor states are extracted. The low-level state structure definition can be found in ROS2 Message Definitions.
This function implements the following logic:

Record the current position of each joint to support closed-loop control
Pad missing MotorState objects to avoid out-of-bounds access and crashes
Periodically print IMU Euler angles

def getLowState(self, msg):
        # Get the actual number of received motors
        received_len = len(msg.motor_state)

        if received_len < ADAM_PRO_NUM_MOTOR:
            # If received data is insufficient, copy existing data first
            self.low_state = msg
            # Calculate missing count
            missing_count = ADAM_PRO_NUM_MOTOR - received_len
            # Pad with default MotorState objects
            # This ensures self.low_state.motor_state[i].q defaults to 0.0
            self.low_state.motor_state.extend([MotorState() for _ in range(missing_count)])

            # Optional: print warning (only once or with limited frequency)
            # print(f"Warning: Received only {received_len} motor states. Padding with {missing_count} default states.")
        else:
            # If sufficient or more, assign directly
            self.low_state = msg

        self.getstate_flag = True
        self.counter_ +=1
        if (self.counter_ % 500 == 0) :
            self.counter_ = 0
            print(self.low_state.imu_state.ypr)

Low-Level Command Sending Function

CDemonController.LowCmdWrite is used to periodically send low-level commands and implements the following stages:

Stage 1: Reset the Adam robot from any initial joint position to the ready posture (first 3 seconds after startup)
Stage 2: Control ankle joints using PR control mode to perform continuous swinging for 3 seconds
Stage 3: Open both hands and keep this state until the program terminates

def LowCmdWrite(self):
        if(self.getstate_flag):
            self.time_ += self.control_dt_

            if self.time_ < self.duration_ :
                # [Stage 1]: set robot to zero posture
                for i in range(ADAM_PRO_NUM_MOTOR):
                    ratio = np.clip(self.time_ / self.duration_, 0.0, 1.0)
                    self.low_cmd.motor_cmd[i].mode =  1 # 1:Enable, 0:Disable
                    self.low_cmd.motor_cmd[i].tau = 0. 
                    self.low_cmd.motor_cmd[i].q = (1.0 - ratio) * self.low_state.motor_state[i].q 
                    self.low_cmd.motor_cmd[i].dq = 0. 
                    self.low_cmd.motor_cmd[i].kp = Kp[i] 
                    self.low_cmd.motor_cmd[i].kd = Kd[i] 
                for i in range(12):
                    self.hand_cmd.position[i] = self.close_hand[i]

            elif self.time_ < self.duration_ * 2 :
                max_P = np.pi * 30.0 / 180.0
                max_R = np.pi * 10.0 / 180.0
                t = self.time_ - self.duration_
                L_P_des = max_P * np.sin(2.0 * np.pi * t)
                L_R_des = max_R * np.sin(2.0 * np.pi * t)
                R_P_des = max_P * np.sin(2.0 * np.pi * t)
                R_R_des = -max_R * np.sin(2.0 * np.pi * t)

                self.low_cmd.motor_cmd[ADAMJointIndex.LeftAnklePitch].q = L_P_des
                self.low_cmd.motor_cmd[ADAMJointIndex.LeftAnkleRoll].q = L_R_des
                self.low_cmd.motor_cmd[ADAMJointIndex.RightAnklePitch].q = R_P_des
                self.low_cmd.motor_cmd[ADAMJointIndex.RightAnkleRoll].q = R_R_des

            elif self.time_ < self.duration_ * 3 :
                for i in range(ADAM_PRO_NUM_MOTOR):
                    self.low_cmd.motor_cmd[i].mode =  1 # 1:Enable, 0:Disable
                    self.low_cmd.motor_cmd[i].tau = 0. 
                    self.low_cmd.motor_cmd[i].q = 0.
                    self.low_cmd.motor_cmd[i].dq = 0. 
                    self.low_cmd.motor_cmd[i].kp = Kp[i] 
                    self.low_cmd.motor_cmd[i].kd = Kd[i] 
                for i in range(12):
                    self.hand_cmd.position[i] = self.open_hand[i]

            self.lowcmd_pub_.publish(self.low_cmd) 
            self.handcmd_pub_.publish(self.hand_cmd)