unitree_sdk2

The Unitree H1_2 robot uses a parallel mechanism for its left and right ankles. While the hardware consists of four physical joints (A and B parallel joints, and P and R serial joints), the P (Pitch) and R (Roll) joints cannot be driven directly by motors.

To simplify control, the SDK provides a PR mode that allows you to command the serial P and R joints directly. The robot's internal controller automatically converts these commands into the necessary movements for the A and B physical joints using kinematic and dynamic relationships.

When using PR mode, each serial joint accepts the following control parameters:

The total executed torque for the serial joint is calculated as: T = kp * (q - q_m) + kd * (dq - dq_m) + tau, where q_m and dq_m are the measured position and velocity.

The Unitree H1_2 robot uses a parallel mechanism for its ankle joints (left and right legs). While the hardware consists of four joints (Parallel joints A and B, and Serial joints Pitch P and Roll R), only the A and B joints are directly motor-driven.

To simplify control, the SDK provides PR Mode. This mode allows users to interact with the ankle as if it were a standard serial joint system. When PR Mode is enabled, the robot's internal controller automatically converts your commands for the P and R joints into the necessary motor commands for the A and B joints using kinematic and dynamic models.

In PR Mode, each serial joint accepts the following command variables:

The total applied torque is calculated as: T = kp * (q - q_m) + kd * (dq - dq_m) + tau, where q_m and dq_m are the measured position and velocity.

To perform tracking experiments on the H1_2 ankle, you must enable PR mode and then populate the dds_low_command motor commands for the specific ankle indices.

Note on Motor Indices (from example):

• 4: Left Ankle Pitch
• 5: Left Ankle Roll
• 10: Right Ankle Pitch
• 11: Right Ankle Roll

Example Workflow:

1. Set mode_ = PR.
2. Calculate desired trajectories (e.g., sine waves).
3. Update dds_low_command.motor_cmd().at(index) with q, dq, kp, kd, and tau.
4. Read actual values from low_state_.motor_state().at(index).q() to verify tracking.

// [Stage 2]: swing ankle's PR
mode_ = PR;  // Enable PR mode
// generate sin/cos trajectory
double max_P = 0.25;  // [rad]
double max_R = 0.25;  // [rad]
double t = time_ - duration_;
double L_P_des = max_P * std::cos(2.0 * M_PI * t);
double L_R_des = max_R * std::sin(2.0 * M_PI * t);
double R_P_des = max_P * std::cos(2.0 * M_PI * t);
double R_R_des = -max_R * std::sin(2.0 * M_PI * t);

// update ankle joint position targets
float Kp_Pitch = 80;
float Kd_Pitch = 1;
float Kp_Roll = 80;
float Kd_Roll = 1;

dds_low_command.motor_cmd().at(4).q() = L_P_des;  // 4: LeftAnklePitch
dds_low_command.motor_cmd().at(4).dq() = 0;
dds_low_command.motor_cmd().at(4).kp() = Kp_Pitch;
dds_low_command.motor_cmd().at(4).kd() = Kd_Pitch;
dds_low_command.motor_cmd().at(4).tau() = 0;

dds_low_command.motor_cmd().at(5).q() = L_R_des;  // 5: LeftAnkleRoll
dds_low_command.motor_cmd().at(5).dq() = 0;
dds_low_command.motor_cmd().at(5).kp() = Kp_Roll;
dds_low_command.motor_cmd().at(5).kd() = Kd_Roll;
dds_low_command.motor_cmd().at(5).tau() = 0;

dds_low_command.motor_cmd().at(10).q() = R_P_des;  // 10: RightAnklePitch
dds_low_command.motor_cmd().at(10).dq() = 0;
dds_low_command.motor_cmd().at(10).kp() = Kp_Pitch;
dds_low_command.motor_cmd().at(10).kd() = Kd_Pitch;
dds_low_command.motor_cmd().at(10).tau() = 0;

dds_low_command.motor_cmd().at(11).q() = R_R_des;  // 11: RightAnkleRoll
dds_low_command.motor_cmd().at(11).dq() = 0;
dds_low_command.motor_cmd().at(11).kp() = Kp_Roll;
dds_low_command.motor_cmd().at(11).kd() = Kd_Roll;
dds_low_command.motor_cmd().at(11).tau() = 0;