A system, method, and computer program product for controlling an articulated system are described. The system estimates kinematics of body segments of the articulated system and constructs a weighted pseudo-inverse matrix to enforce kinematic constraints as well as achieve dynamic consistency based on the estimated kinematics. The system converts task descriptors to joint commands using the weighted pseudo-inverse matrix and controls the articulated system at both the velocity level and the acceleration level and enforces kinematic constraints using the joint commands.