Manipulator and method for controlling the manipulator based on joint limits
A manipulator is provided that supports a surgical tool used in a surgical procedure. The manipulator includes a plurality of links and joints and a controller that evaluates an actual joint angle of each joint relative to one or more joint boundary angles for each joint. The controller computes forces and torques to apply to a virtual rigid body based on the evaluation. The controller then determines a commanded joint angle for each joint based on the computed forces and torques so that actuators move the surgical tool to a commanded pose associated with the commanded joint angles.
1. A manipulator for supporting a surgical tool, the manipulator comprising:
a plurality of links and joints;
a plurality of actuators with each actuator arranged to move one or more of the links;
a plurality of sensors with each sensor arranged to measure an actual joint angle for one of the joints; and
at least one controller configured to:
evaluate the actual joint angle relative to one or more joint boundary angles for each joint, wherein the one or more joint boundary angles comprise minimum and maximum joint boundary angles;
compare, for each joint, the actual joint angle to the minimum and maximum joint boundary angles to determine a boundary exceeded angle;
output a joint limit torque based on the boundary exceeded angle;
model the surgical tool as a virtual rigid body;
convert the joint limit torque into forces and torques to apply to the virtual rigid body; and
determine a commanded joint angle for each joint based on the forces and torques applied to the virtual rigid body so that the plurality of actuators move the surgical tool to a commanded pose associated with the commanded joint angles.
2. The manipulator of claim 1 , wherein the minimum and maximum joint boundary angles fall within a predetermined range of motion for each joint, the predetermined range of motion defined by minimum and maximum joint limit angles.
3. The manipulator of claim 1 , wherein the at least one controller is configured to output a value for the boundary exceeded angle of zero when the actual joint angle is greater than the minimum joint boundary angle and less than the maximum joint boundary angle.
4. The manipulator of claim 1 , wherein the at least one controller is configured to output a value for the boundary exceeded angle that is non-zero when the actual joint angle is less than the minimum joint boundary angle or greater than the maximum joint boundary angle.
5. The manipulator of claim 4 , wherein the at least one controller is configured to generate forces and torques to apply to the virtual rigid body based on evaluating the actual joint angles if the value for the boundary exceeded angle is non-zero.
6. The manipulator of claim 1 , wherein the sensors are rotary encoders.
7. The manipulator of claim 1 , wherein the joint limit torque has greater than first order relationship to the magnitude of the boundary exceeded angle.
8. The manipulator of claim 1 , wherein the joint limit torque is set using a look-up table.
9. The manipulator of claim 1 , wherein the controller is further configured to determine the commanded joint angle for each joint based on the forces and torques so that the plurality of actuators move the surgical tool to the commanded pose associated with the commanded joint angles when the actual joint angle is less than the minimum joint boundary angle or greater than the maximum joint boundary angle.
10. A method for controlling a manipulator supporting a surgical tool, wherein the manipulator includes a plurality of links and joints, a plurality of actuators with each actuator arranged to move one or more of the links, and a plurality of sensors with each sensor arranged to measure an actual joint angle for one of the joints, said method comprising the steps of:
evaluating the actual joint angle relative to one or more joint boundary angles for each joint;
comparing the actual joint angle to minimum and maximum joint boundary angles for each joint to determine a boundary exceeded angle for each joint;
outputting a joint limit torque based on the boundary exceeded angles;
modeling the surgical tool as a virtual rigid body;
converting the joint limit torque into forces and torques to apply to the virtual rigid body; and
determining a commanded joint angle for each joint based on the computed forces and torques applied to the virtual rigid body so that the plurality of actuators move the surgical tool to a commanded pose associated with the commanded joint angles.
11. The method of claim 10 , wherein the minimum and maximum joint boundary angles fall within a predetermined range of motion for each joint, the predetermined range of motion defined by minimum and maximum joint limit angles.
12. The method of claim 10 , including outputting a value for the boundary exceeded angle of zero when the actual joint angle is greater than the minimum joint boundary angle and less than the maximum joint boundary angle.
13. The method of claim 10 , including outputting a value for the boundary exceeded angle that is non-zero when the actual joint angle is less than the minimum joint boundary angle or greater than the maximum joint boundary angle.
14. The method of claim 13 , wherein computing forces and torques to apply to the virtual rigid body based on evaluating the actual joint angles relative to the one or more joint boundary angles includes computing the forces and torques if the value for the boundary exceeded angle is non-zero.
15. The method of claim 10 wherein the joint limit torque has greater than first order relationship to the magnitude of the boundary exceeded angle.
16. The method of claim 10 , wherein the joint limit torque is set using a look-up table.
17. The method of claim 10 , wherein the plurality of actuators move the surgical tool to the commanded pose associated with the commanded joint angles when the actual joint angle is less than the minimum joint boundary angle or greater than the maximum joint boundary angle.