DDS Low-Level Motion Reference Example

The low-level motion development example implements resetting the robot from any initial joint positions to ready position, then controlling the ankle and finger joints of robot using two different modes. The source code is located in pnd_sdk_python/example/low_level and can be accessed here. For running instructions, see Quick Development. The source directory is organized by robot model:

├── 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

Code Analysis

The following code is for the Adam Pro version. Other versions follow a similar structure, with differences only in joint index definitions.

Core Types and Functions

Type Name	Description
ADAMJointIndex	Index of all joints sorted by name, also used as indices in LowCmd.MotorCmd array
LowCmd	Motor command structure
LowState	Motor state structure
HandCmd	Hand command structure
Custom	Core control logic class
ChannelPublisher	DDS publisher
ChannelSubscriber	DDS subscriber

The main program is defined in adam_pro_low_level_example.py and instantiates the Custom class. In the Custom constructor, two publishers and one subscriber are created along with corresponding DDS message structures:

Defines the positions of 12 fingers when closed in Custom.close_hand
Creates rt/lowcmd topic publisher for motor positions, and rt/handcmd topic publisher for hand positions
Receives low-level state from rt/lowstate topic callback, handled by Custom.LowStateHandler

Example code:

class Custom:
    def __init__(self):
        self.time_ = 0.0
        self.control_dt_ = 0.0025  # [2.5ms]
        self.duration_ = 3.0    # [3 s]
        self.counter_ = 0
        self.low_cmd = pnd_adam_msg_dds__LowCmd_(ADAM_SP_NUM_MOTOR)  
        self.low_state = pnd_adam_msg_dds__LowState_(ADAM_SP_NUM_MOTOR)

        self.hand_cmd = pnd_adam_msg_dds__HandCmd_()
        self.close_hand = np.array([500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500, 500], dtype=int)
        self.getstate_flag = False

    def Init(self):
        self.lowcmd_publisher_ = ChannelPublisher("rt/lowcmd", LowCmd_)
        self.lowcmd_publisher_.Init()

        self.hand_pub = ChannelPublisher("rt/handcmd", HandCmd_)
        self.hand_pub.Init()

        # create subscriber #
        self.lowstate_subscriber = ChannelSubscriber("rt/lowstate", LowState_)
        self.lowstate_subscriber.Init(self.LowStateHandler, 10)

Create Thread for Periodic Motor Command Publishing

After the program starts, a thread is launched to call Custom.LowCmdWrite at intervals defined by Custom.control_dt_:

def Start(self):
        self.lowCmdWriteThreadPtr = RecurrentThread(
            interval=self.control_dt_, target=self.LowCmdWrite, name="control"
        )
        self.lowCmdWriteThreadPtr.Start()

Low-Level State Callback Function

When the rt/lowstate topic is received, the Custom.LowStateHandler function is automatically called. Refer to DDS Message Definition. It extracts the motor states and implements:

Recording the current joint positions for closed-loop control
Printing IMU Euler angles periodically

def LowStateHandler(self, msg: LowState_):
        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

Custom.LowCmdWrite periodically sends low-level commands:

Stage 1: Reset Adam robot from any initial joint positions to ready position (first 3 seconds)
Stage 2: Swing ankle joints using PR control mode for 3 seconds
Stage 3: Open fingers until program termination

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

            if self.time_ < self.duration_ :
                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.hand_pub.Write(self.hand_cmd)
        self.lowcmd_publisher_.Write(self.low_cmd)