A controller for floating-base humanoid robots that can track motion capture data while maintaining balance. Briefly, the controller includes a proportional-derivative (PD) controller that is adapted to compute the desired acceleration to track a given reference trajectory at every degree-of-freedom (DOF) of the robot including the six unactuated ones of the floating base. Second, the controller includes a component (joint torque optimization module) that computes the optimal joint torques and contact forces to realize the desired accelerations given by the first component (i.e., the PD controller). The joint torque optimization module performs this computation considering the full-body dynamics of the robot and the constraints on contact forces. The desired accelerations may not be feasible for the robot due to limits in normal contact forces and friction (e.g., the robot sometimes cannot exactly copy or perform the modeled human motion defined by motion capture data).