Manipulator joint-limit handling algorithm

1. A system for controlling a robotic device, the robotic device comprising joints moving an end-effector that operates in an m degrees of freedom task space, the robotic device being driven by n actuators, the system in communications with actuator driver circuitry to control the actuators, the system comprising:

a processing unit in communications with the actuator driver circuitry; and

a memory in communications with the processing unit, the memory comprising program code executable by the processing unit to cause the processing unit to perform the following steps:

determining if a desired movement command for the robotic device would cause at least a first joint of the n actuated joints to violate a corresponding joint travel limit;

constructing a modified Jacobian matrix J mod by removing a matrix column directly from a Jacobian matrix J that is associated with the first joint which would violate the corresponding joint travel limit if the desired movement command is executed, where the Jacobian matrix J is an m×n matrix mathematically expressing current positions of the joints;

constructing a revised movement command for the robotic device by solving the following equation:

{dot over (q)} mod =J mod ( J mod T W 2 J mod ) −1 J mod T W 2 {dot over (x)} cmd

wherein {dot over (q)} mod is an (n−L)×1 joint velocity command for joints that are not currently being limited, J mod is an m×(n−L) matrix, J mod T is the transpose of J mod , W is an m×m matrix comprising weighting factors to weight respective movements of the n actuated joints, and {dot over (x)} cmd is the desired movement command for the end-effector of the robotic device, where the revised movement command prevents the first joint from violating the corresponding joint travel limit, is not identical to the desired movement command and if submitted to the actuator driver circuitry causes movement of at least a portion of the n actuated joints; and

utilizing the revised movement command in place of the desired movement command to control the actuator driver circuitry to cause corresponding movement of the robotic device.

2. The system of claim 1 , wherein the revised movement command is constructed by solving the following equation:

{dot over (q)} new =re ( {dot over (q)} mod ),

wherein {dot over (q)} new is an n×1 new joint velocity command, and re( ) is a function that inserts a respective predefined value for each of the L actively limited joint into {dot over (q)} mod to generate {dot over (q)} new .

3. The system of claim 2 , wherein the program code further causes the processing unit to determine if {dot over (q)} new would cause any of a plurality of joint travel limits of the robotic device to be violated.

4. The system of claim 1 , wherein the program code further causes the processing unit to generate the m×(n−L) matrix J mod by:

determining an m×n Jacobian matrix J for the robotic device utilizing the current state of the robotic device, the current state of the robotic device comprising current displacements of each of the n joints; and

utilizing the Jacobian matrix J to generate the matrix J mod , wherein J mod comprises all columns in J except for those columns corresponding to the L actively limited joints.