← Back to Pubs & Projects

Project 01 / Robot Control / 2026

Real-Time Compliant Half-Humanoid Control

A real-time target-to-command pipeline in which continuity- and safety-regulated whole-body IK feeds a coupled Cartesian impedance controller for both arms and the shared waist.

Area

Robotics Control Systems Impedance & Force Control Inverse Kinematics Rigid-Body Dynamics Numerical Optimization Real-Time Systems

Skill

C++17 Python Pinocchio Eigen URDF EtherCAT / CiA402 gRPC Multi-threading Damped Least Squares / SVD
1000 HzControl rate
<1.5 mmDual-hand TCP error
  1. 01Target TCPTwo desired 6D hand poses
  2. 02Whole-Body IKContinuous, bounded joint targets
  3. 03Cartesian ImpedanceCoupled arm and waist response
  4. 04Robot CommandCalculated torque mapped to motor effort

01 / Inverse Kinematics

Continuity and safety are part of the solve

Both 6D hand targets and their shared waist are solved together as one weighted least-squares problem. Alongside pose error, the objective penalizes motion from the previous command, deviation from a reference posture, and poor numerical conditioning.

Continuous solution branches

  • Seed from the previous solution instead of measured joints.
  • Weight forearm-roll and waist motion heavily to suppress branch flips.
  • Score candidates by TCP error, command continuity, and posture.
  • Reject any one-tick joint jump above approximately 0.35 radians.

Bounded behavior near limits

  • Shrink URDF limits by five degrees for a safety margin.
  • Use an active-set re-solve instead of naive per-joint clamping.
  • Project unreachable targets onto the feasible workspace.
  • Apply a total step budget, stale-target guards, and minimum-jerk engagement.

02 / Cartesian Impedance

Both arms are coupled; the waist is controlled deliberately

Each hand contributes a 6D Cartesian task using world-aligned Jacobians, translation error, and an SO(3) logarithmic orientation error. The two hand tasks are stacked into one 12×14 solve so the arms coordinate instead of correcting independently.

Coupled arm behavior

  • Map Cartesian wrench through τ = Jᵀw.
  • Clamp force and torque by norm to preserve correction direction.
  • Project posture torque through a damped task nullspace.
  • Compensate gravity, Coriolis effects, friction, and soft limits.

Shared-waist strategy

  • Hold the waist with a separate joint-space PD law to prevent sag.
  • Retain its Jacobian velocity column so both hands sense torso motion.
  • Feed waist-induced hand motion into the next 1 ms control update.
  • Achieve <1.5 mm TCP position error on both hands.

03 / Low-Level Driver

A real-time control foundation for VLA applications

The resulting joint targets feed the 1000 Hz impedance loop. The vendor SDK did not expose the required low-latency command path, so a narrow shared-memory interface talks directly to the robot's underlying control layer, where calculated joint torque is converted into commanded motor current. This pipeline serves as the robot's lower-level driver for later VLA applications, translating high-level target poses into continuous, compliant motor commands. The compliant response provides greater fault tolerance when a VLA produces an imperfect command: during unexpected obstacle contact, the robot can yield instead of immediately entering the tracking errors that rigid position control may trigger.