A system and method for controlling a manipulator are provided. A manipulator controller measures an actual torque and calculates an expected torque for at least one joint motor of the manipulator. The manipulator controller computes a backdrive force for the at least one joint motor based on the actual torque and expected torque. A surgical tool of the manipulator is modeled as a virtual rigid body. The manipulator controller computes a total force or torque to apply to the virtual rigid body based on the backdrive force. The manipulator controller determines a target position of the surgical instrument based on the total force or torque and commands positioning of the joint motors to advance the surgical instrument to the target position.
