Systems and methods for avoiding collisions between manipulator arms using a null-space
Devices, systems, and methods for avoiding collisions between manipulator arms using a null-space are provided. In one aspect, the system calculates an avoidance movement using a relationship between reference geometries of the multiple manipulators to maintain separation between reference geometries. In certain embodiments, the system determines a relative state between adjacent reference geometries, determines an avoidance vector between reference geometries, and calculates an avoidance movement of one or more manipulators within a null-space of the Jacobian based on the relative state and avoidance vector. The joints may be driven according to the calculated avoidance movement while maintaining a desired state of the end effector or a remote center location about which an instrument shaft pivots and may be concurrently driven according to an end effector displacing movement within a null-perpendicular-space of the Jacobian so as to effect a desired movement of the end effector or remote center.
1. A robotic method for performing avoidance movements in a robotic system, the method comprising:
determining a first reference geometry corresponding to a structure of a first manipulator arm of the robotic system, the first manipulator arm including a first distal portion, a first proximal portion coupled to a first base, and a plurality of first joints between the first distal portion and the first base, and the plurality of first joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of first joints for a given state of the first distal portion;
determining a second reference geometry corresponding to a structure of a second manipulator arm of the robotic system, the second manipulator arm including a second distal portion, a second proximal portion coupled to a second base, and a plurality of second joints between the second distal portion and the second base, and the plurality of second joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of second joints for a given state of the second distal portion;
determining an avoidance vector based on a relative state of the first reference geometry and the second reference geometry in a workspace of the robotic system;
determining an avoidance movement of one or more joints of the pluralities of first joints and second joints to maintain a separation between the first and second reference geometries in the workspace, the avoidance movement being based on the avoidance vector , and the avoidance movement maintaining a desired state of the first distal portion and a desired state of the second distal portion; and
driving the one or more joints of the pluralities of first joints and second joints according to the determined avoidance movement.
2. The robotic method of claim 1 , wherein the first reference geometry includes one or more line segments corresponding to a structure of the first manipulator arm; and
the second reference geometry includes one or more line segments corresponding to a structure of the second manipulator arm.
3. The robotic method of claim 1 , wherein the avoidance vector corresponds to a clearance value for the separation between the first manipulator arm and the second manipulator arm, the clearance value being determined from values of the relative state of the first reference geometry and the second reference geometry; and
the avoidance movement is directed to increasing the clearance value for the separation between the first manipulator arm and the second manipulator arm.
4. The robotic method of claim 1 , wherein the avoidance vector is defined as a vector between a selected point of the first reference geometry and a selected point of the second reference geometry.
5. The robotic method of claim 1 , wherein the avoidance vector characterizes a minimum clearance distance between the first reference geometry and the second reference geometry.
6. The robotic method of claim 1 , wherein the avoidance movement is determined by calculating joint velocities of the one or more joints of the pluralities of first joints and second joints from directions that correspond to the first distal portion not moving and the second distal portion not moving.
7. The robotic method of claim 1 , wherein the avoidance movement is determined by calculating joint velocities of the one or more joints of the pluralities of first joints and second joints from a null space of a first Jacobian associated with the first manipulator arm or a null space of a second Jacobian associated with the second manipulator arm.
8. A robotic system comprising:
a first manipulator arm including a first distal portion, a first proximal portion coupled to a first base, and a plurality of first joints between the first distal portion and the first base, the plurality of first joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of first joints for a given state of the first distal portion;
a second manipulator arm including a second distal portion, a second proximal portion coupled to a second base, and a plurality of second joints between the second distal portion and the second base, the plurality of second joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of second joints for a given state of the second distal portion; and
one or more processors configured to perform operations including:
determining a first reference geometry corresponding to a structure of the first manipulator arm and a second reference geometry corresponding to a structure of the second manipulator arm;
determining an avoidance vector based on a relative state of the first reference geometry and the second reference geometry in a workspace of the robotic system;
determining an avoidance movement of one or more joints of the pluralities of first joints and second joints to maintain a separation between the first and second reference geometries in the workspace, the avoidance movement being based on the avoidance vector, and the avoidance movement maintaining a desired state of the first distal portion and a desired state of the second distal portion; and
driving the one or more joints of the pluralities of first joints and second joints according to the determined avoidance movement.
9. The robotic system of claim 8 , wherein the first reference geometry includes one or more line segments corresponding to a structure of the first manipulator arm; and
the second reference geometry includes one or more line segments corresponding to a structure of the second manipulator arm.
10. The robotic system of claim 8 , wherein the avoidance vector corresponds to a clearance value for the separation between the first manipulator arm and the second manipulator arm, the clearance value being determined from values of the relative state of the first reference geometry and the second reference geometry; and
the avoidance movement is directed to increasing the clearance value for the separation between the first manipulator arm and the second manipulator arm.
11. The robotic system of claim 8 , wherein the avoidance vector is defined as a vector between a selected point of the first reference geometry and a selected point of the second reference geometry.
12. The robotic system of claim 8 , wherein the avoidance vector characterizes a minimum clearance distance between the first reference geometry and the second reference geometry.
13. The robotic system of claim 8 , wherein the avoidance movement is determined by calculating joint velocities of the one or more joints of the pluralities of first joints and second joints from directions that correspond to the first distal portion not moving and the second distal portion not moving.
14. The robotic system of claim 8 , wherein the avoidance movement is determined by calculating joint velocities of the one or more joints of the pluralities of first joints and second joints from a null space of a first Jacobian associated with the first manipulator arm or a null space of a second Jacobian associated with the second manipulator arm.
15. A non-transitory readable memory storing a processor-implemented program for performing avoidance movements in a robotic system, the program including instructions that, when executed by one or more processors, cause the one or more processors to perform operations comprising:
determining a first reference geometry corresponding to a structure of a first manipulator arm of the robotic system, the first manipulator arm including a first distal portion, a first proximal portion coupled to a first base, and a plurality of first joints between the first distal portion and the first base, and the plurality of first joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of first joints for a given state of the first distal portion;
determining a second reference geometry corresponding to a structure of a second manipulator arm of the robotic system, the second manipulator arm including a second distal portion, a second proximal portion coupled to a second base, and a plurality of second joints between the second distal portion and the second base, and the plurality of second joints having a joint space with sufficient degrees of freedom to allow a range of differing joint states of the plurality of second joints for a given state of the second distal portion;
determining an avoidance vector based on a relative state of the first reference geometry and the second reference geometry in a workspace of the robotic system;
determining an avoidance movement of one or more joints of the pluralities of first joints and second joints to maintain a separation between the first and second reference geometries in the workspace, the avoidance movement being based on the avoidance vector , and the avoidance movement maintaining a desired state of the first distal portion and a desired state of the second distal portion; and
driving the one or more joints of the pluralities of first joints and second joints according to the determined avoidance movement.
16. The readable memory of claim 15 , wherein the first reference geometry includes one or more line segments corresponding to a structure of the first manipulator arm; and
the second reference geometry includes one or more line segments corresponding to a structure of the second manipulator arm.
17. The readable memory of claim 15 , wherein the avoidance vector corresponds to a clearance value for the separation between the first manipulator arm and the second manipulator arm, the clearance value being determined from values of the relative state of the first reference geometry and the second reference geometry; and
the avoidance movement is directed to increasing the clearance value for the separation between the first manipulator arm and the second manipulator arm.
18. The readable memory of claim 15 , wherein the avoidance vector is defined as a vector between a selected point of the first reference geometry and a selected point of the second reference geometry.
19. The readable memory of claim 15 , wherein the avoidance vector characterizes a minimum clearance distance between the first reference geometry and the second reference geometry.
20. The readable memory of claim 15 , wherein the avoidance movement is determined by calculating joint velocities of the one or more joints of the pluralities of first joints and second joints from directions that correspond to the first distal portion not moving and the second distal portion not moving.