Skip to content

PAL Policy Deployer — Documentation

Repository: pal_policy_deployer

1. Start

Before you start a policy, check that the Kangaroo Doctor at http://kangaroo-5c:8001 is green.

roslaunch pal_policy_deployer kang_rl_deployer.launch model:=v60 task:=loco lab:=mjlab

If the subscriber_controller does not start, change_controllers times out or motors do not respond, open the Kangaroo Doctor and follow its suggested step before you debug the policy.

This launches two components at the same time:

  1. Policy node (pal_policy_inferer) — reads sensors, runs ONNX inference, and outputs joint positions
  2. Subscriber controller — receives target positions and forwards them to the impedance controller

Launch chain

kang_rl_deployer.launch
    └─► kang_mjlab_deployer.launch
            ├─► pal_policy_inferer_mjlab_node  (model=v60, task=loco)
            └─► kangaroo_subscriber_controller.launch
                    ├─► Loads YAML parameters (gains, estimator, IMU)
                    └─► Spawns: subscriber_controller

`pal_policy_deployer/launch/kang_rl_deployer.launch`
`pal_policy_deployer/launch/kang_mjlab_deployer.launch`
`subscriber_controller/launch/kangaroo_subscriber_controller.launch`

2. Package Structure

Package Role
pal_policy_deployer Launch files, ONNX models (v21–v60), debug script
pal_policy_inferer C++ node — this is where ONNX inference runs
pal_policy_preprocessor Joystick / RViz → /cmd_vel (currently commented out) 
pal_policy_preprocessor ──► pal_policy_deployer
  (Joystick / RViz)           (Models + launch)
                                      │
                                      ▼
                            pal_policy_inferer
                            (ONNX inference, 50 Hz)
                                      │
                                      ▼
                   /subscriber_controller/desired_state

3. Data Flow — Overview

Observations             Topics
─────────────────────    ─────────────
base_ang_vel         ┐
base_lin_acc         ├──  /base_imu
projected_gravity    ┘
joint_pos            ┐
joint_vel            ├──  /joint_states
                     ┘
command (vx,vy,ωz)   ───  /cmd_vel
actions              ───  internal (previous policy output)
──────────────────────────────────────────────────────────
                         │
                         ▼
             kang_mjlab_node (50 Hz, ONNX)
                         │
                         │  415 floats → 16 floats (raw [-1..1])
                         │  action[i] = scale[i] * output[i] + default_pos[i]
                         │  → target joint position [rad] per joint (NOT torques!)
                         ▼
     /subscriber_controller/desired_state  (JointState)
                         │
                         ▼
        subscriber_controller  (pass-through, p=i=d=0)
                         │
                         ▼
        pal_local_joint_control  (impedance control)
                         │
                         │  τ = K×(q_des−q_act) + D×(dq_des−dq_act)
                         │  joint_handle_.setCommand(τ)  ← no topic, shared memory
                         ▼
             EtherCAT → motor controller → motor

4. Policy Node — Execution Flow

pal_policy_inferer/src/kang_mjlab_node.cpp

4.1 Load ONNX model (loadPolicyConfig())

At startup, metadata is read directly from the ONNX model:

Metadata key Content
joint_names which joints are observed
default_joint_pos zero position (default pose)
action_scale scaling factors for the output
observation_names e.g. base_ang_vel, projected_gravity, joint_pos, joint_vel, actions

pal_policy_deployer/models/loco/loco_v60_mjlab/*.onnx

4.2 Read sensor data (callbacks)

Topic Callback Observation
/base_imu imuCB() base_ang_vel, base_lin_acc, projected_gravity
/joint_states jsCB() joint_pos (−default), joint_vel
/cmd_vel velCmdCB() command (vx, vy, ωz)

4.3 50 Hz timer — inference (timerCallback())

1. Update history buffer (ring buffer, 5 time steps)
2. Build observation vector: 5 × 83 = 415 floats (term-major)
3. Run ONNX inference: 415 → 16 outputs
4. Scale output: action[i] = scale[i] * output[i] + default_pos[i]
5. Publish to /subscriber_controller/desired_state

5. Impedance Control

5.1 Basic principle

This is not a pure position controller — it simulates a spring + damper for each joint:

ToDo: We need to figure out, if it actually performs impedance control. PAL mentioned that it doesn't use a model...

Symbol Name Meaning
K stiffness spring stiffness — how strongly the joint is pulled toward the target
D damping damping — suppresses velocity deviations
q_desired target position provided by the policy (50 Hz)
q_actual actual position measured encoder value

Advantage: the joint can yield under external forces such as ground contact, which is essential for walking.

5.2 Two controller types

Type Motors Joints Transmission
DirectImpedanceControl 1 motor → 1 joint leg_*_1, leg_*_length gearbox + ball screw
DifferentialImpedanceControl 2 motors → 2 joints leg_*_2+3 (hip), leg_*_4+5 (ankle) differential gearbox

For the differential setup: both motors in the same direction → roll, opposite directions → pitch: 

Motor A ──┐
           ├─► Differential ─► Joint Roll
Motor B ──┘                 └─► Joint Pitch


kangaroo_controller_configuration/launch/local_joint_control/legs_impedance_control.launch
kangaroo_controller_configuration/launch/local_joint_control/leg_left.launch
pal_local_joint_control/launch/direct_impedance_control.launch
pal_local_joint_control/include/pal_local_joint_control/impedance_base_effort_controller.h

5.3 From torque to motor current

Rotational joint:   I = τ / (Kt × i)
Linear joint:       I = τ × p / (Kt × i × η × 2π)
Symbol YAML parameter Unit Meaning
Kt motor_torque_constant Nm/A torque per ampere
i reduction_ratio — gearbox reduction ratio (5)
η ball_screw_efficiency — ball screw efficiency (0.9)
p ball_screw_lead m/rev ball screw lead (0.01 / 0.005) 
`kangaroo_controller_configuration/config/local_joint_control/actuator_parameters/leg_left_length_joint.yaml`
`kangaroo_controller_configuration/config/local_joint_control/actuator_parameters/leg_left_1_joint.yaml`

5.4 Impedance parameters per joint

stiffness_factor: 1600.0      # K [Nm/rad]
damping_factor:   101.859     # D [Nm·s/rad]
feedforward_prev_cmd: true    # use previous command as feedforward → reduces phase lag
integral_factor:  0.0         # not used

kangaroo_controller_configuration/config/local_joint_control/impedance_control/*.yaml

5.5 RL gains vs. default gains

Joint Role Default K RL K Default D RL D
leg_*_length_joint leg length (linear actuator) 11000 1600 650.0 101.859
leg_*_1_joint hip Z (rotation) 40 100 1.0 6.366
leg_*_2_joint hip pitch (differential) 100 100 4.0 6.366
leg_*_3_joint hip roll (differential) 100 100 4.0 6.366
leg_*_4_joint ankle roll (differential) 300 30 5.0 1.91
leg_*_5_joint ankle pitch (differential) 300 30 5.0 1.91

The gains used for RL differ from the default gains

  • leg_*_length_joint with K=11000: a 0.1 rad error would result in 1100 Nm
  • leg_*_4/5_joint with K=300: ankle too stiff → no compliance during ground contact
  • The RL gains match the virtual spring-damper setup used during MuJoCo training
Workspace: `kangaroo_controller_configuration/config/local_joint_control/impedance_control/*.yaml`
Robot: `/opt/pal/ferrum/share/kangaroo_controller_configuration/config/local_joint_control/impedance_control/*.yaml`

6. Known Sim-to-Real Issues

6.1 IMU frame mismatch

  • In MuJoCo training, the IMU is mounted with quat="0 -0.707107 0.707107 0" (180° rotation)
  • The real IMU driver publishes raw sensor data without compensating for this mounting rotation
  • Result: projected_gravity z = +1.0 instead of -1.0 → the policy believes the robot is upside down
`Stand_alone/robots/mjLab/pal_kangaroo/xmls/kangaroo_accurate.xml` — IMU site (line 67)
`pal_policy_inferer/src/kang_mjlab_node.cpp` — `imuCB()` where the correction is missing

6.2 Incorrect default impedance gains

  • The default installation uses gains that are about 7–10× too stiff (see table above)
  • Fix: clone kangaroo_robot on branch gains/rl into /catkin_ws/src/kangaroo_robot
  • Simulation: the workspace override is applied automatically after catkin_make
  • Real robot: copy the YAML files manually to /opt/pal/ferrum/share/...