Manipulator joint-limit handling algorithm
First Claim
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 Jmod 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=Jmod(JmodTW2Jmod)−
1JmodTW2{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, Jmod is an m×
(n−
L) matrix, JmodT is the transpose of Jmod, 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.
1 Assignment
0 Petitions
Accused Products
Abstract
A desired movement command (203) for a robotic device (100) having n joints (112) operating in an m degrees of freedom task space is analyzed to determine if it would cause any of the joint angular limits to be violated. In the case where a non-zero number L (241) of the joints (112) have angular limits that are violated, a revised movement command (254) is then constructed using Jmod (251), which includes all columns in a Jacobian matrix J (211) except for those columns corresponding to L actively limited joints.
-
Citations
4 Claims
-
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 Jmod 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=Jmod(JmodTW2Jmod)−
1JmodTW2{dot over (x)}cmdwherein {dot over (q)}mod is an (n−
L)×
1 joint velocity command for joints that are not currently being limited, Jmod is an m×
(n−
L) matrix, JmodT is the transpose of Jmod, 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; andutilizing 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. - View Dependent Claims (2, 3, 4)
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 Jmod 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; andutilizing the Jacobian matrix J to generate the matrix Jmod, wherein Jmod comprises all columns in J except for those columns corresponding to the L actively limited joints.
- (n−
Specification