A method of controlling a robot manipulator which is redundant and under-actuated having at least seven degrees of freedom, whereby equations of motion of said robot manipulator have a redundancy, said robot manipulator comprising plural links connected at respective movable joints therebetween whereby each link is associated with a respective joint, some of said joints being active and having their movements controlled by respective joint servos, remaining ones of said joints being passive and not having their movements controlled by respective joint servos whereby said passive joints are generally freely moveable, an innermost one of said links comprising a relatively stationary base and an outermost one of said links comprising a moveable tip link, said method comprising the steps of:
- obtaining current positions of said links, specified active joint accelerations of said active joints and passive joint forces of said passive joints;
- computing articulated body quantities for each of said joints from said current positions of said links;
- computing from said articulated body quantities and from said active joint accelerations and said passive joint forces, a disturbance Jacobian operator relating said active joint accelerations of said active joints to passive joint accelerations of said passive joints;
- computing said passive joint accelerations by combining said disturbance Jacobian operator and said active joint accelerations;
- establishing a set of constraints resolving said redundancy, and computing active joint forces from said set of constraints and from said active and passive joint accelerations and transmitting joint servo commands corresponding to said active joint forces to said active joints so as to correct motion of said robot manipulator.