Joint Control Full Example

This example demonstrates the complete workflow for motor PD gain configuration and joint group interpolation control: switch to UserCmd state, configure gains, read current positions, move to target positions with smooth interpolation, then return to zero.

States used: UserCmd State

APIs used: set_group_cmd, get_group_state, set_motor_cfg_pd, get_group_motor_cfg

Python

from fourier_aurora_client import AuroraClient
import time

client = AuroraClient.get_instance(domain_id=123, robot_name='gr3')

print("Initializing robot for joint control...")

# Step 1: Switch to UserCmd state
input("Press Enter to set FSM to User Command State (state 10)...")
client.set_fsm_state(10)
time.sleep(0.5)

# Step 2: Configure motor PD gains
input("Press Enter to configure motor PD gains...")
kp_config = {
    'left_manipulator':  [400, 200, 200, 200, 50, 50, 50],
    'right_manipulator': [400, 200, 200, 200, 50, 50, 50],
    'waist': [200, 300, 200],
    'head':  [100, 100],
}
kd_config = {
    'left_manipulator':  [20, 10, 10, 10, 2.5, 2.5, 2.5],
    'right_manipulator': [20, 10, 10, 10, 2.5, 2.5, 2.5],
    'waist': [10, 15, 10],
    'head':  [10, 10],
}
client.set_motor_cfg_pd(kp_config=kp_config, kd_config=kd_config)
time.sleep(1.0)

actual_kp = client.get_group_motor_cfg('left_manipulator', 'pd_kp')
actual_kd = client.get_group_motor_cfg('left_manipulator', 'pd_kd')
print(f"Configured Kp: {kp_config['left_manipulator']}")
print(f"Actual Kp:     {actual_kp}")
print(f"Configured Kd: {kd_config['left_manipulator']}")
print(f"Actual Kd:     {actual_kd}")

# Step 3: Read current positions and move to target
input("Press Enter to move to target joint positions...")
current_pos = {
    'left_manipulator':  client.get_group_state('left_manipulator', key='position'),
    'right_manipulator': client.get_group_state('right_manipulator', key='position'),
    'waist':             client.get_group_state('waist', key='position'),
}
target_pos = {
    'left_manipulator':  [0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0],
    'right_manipulator': [0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0],
    'waist':             [0.5, 0.0, 0.0],
}

total_steps = 200
dt = 0.01
print("Moving to target position...")
for step in range(total_steps + 1):
    pos_cmd = {
        g: [c + (t - c) * step / total_steps for c, t in zip(current_pos[g], target_pos[g])]
        for g in target_pos
    }
    client.set_group_cmd(position_cmd=pos_cmd)
    time.sleep(dt)

print("Target position reached")
time.sleep(1.0)

# Step 4: Return to zero position
input("Press Enter to return to zero position...")
start_pos = {
    'left_manipulator':  client.get_group_state('left_manipulator', key='position'),
    'right_manipulator': client.get_group_state('right_manipulator', key='position'),
    'waist':             client.get_group_state('waist', key='position'),
}
zero_pos = {
    'left_manipulator':  [0.0] * 7,
    'right_manipulator': [0.0] * 7,
    'waist':             [0.0] * 3,
}

print("Returning to zero position...")
for step in range(total_steps + 1):
    pos_cmd = {
        g: [s + (z - s) * step / total_steps for s, z in zip(start_pos[g], zero_pos[g])]
        for g in zero_pos
    }
    client.set_group_cmd(position_cmd=pos_cmd)
    time.sleep(dt)

print("Zero position reached")
time.sleep(0.5)

final_left  = client.get_group_state('left_manipulator', 'position')
final_right = client.get_group_state('right_manipulator', 'position')
final_waist = client.get_group_state('waist', 'position')
print(f"Final left manipulator:  {[f'{p:.3f}' for p in final_left]}")
print(f"Final right manipulator: {[f'{p:.3f}' for p in final_right]}")
print(f"Final waist:             {[f'{p:.3f}' for p in final_waist]}")

print("Joint control demonstration complete")
client.close()