Systems and methods for using the null space to emphasize manipulator joint motion anisotropically
Devices, systems, and methods for providing commanded movement of an end effector of a manipulator while providing a desired movement of one or more joints of the manipulator. Methods include calculating weighted joint velocities using a weighting matrix within the joint space to anisotropically emphasize joint movement within a null-space to provide the desired movement of a first set of joints. Methods may include calculating joint velocities that achieve the desired end effector movement using a pseudo-inverse solution and adjusting the calculated joint velocities using a potential function gradient within the joint space corresponding to the desired movement of the first set of joints. Methods may include use of a weighted pseudo-inverse solution and also an augmented Jacobian solution. One or more auxiliary movements may also be provided using joint velocities calculated from the pseudo-inverse solution. Various configurations for systems utilizing such methods are provided herein.
1. A method for moving a manipulator arm, the manipulator arm comprising a distal portion that supports an instrument that includes an end effector, a proximal portion coupled to a base, and a plurality of joints between the distal portion and the base, the plurality of joints providing sufficient degrees of freedom to allow a range of differing joint states for a given state of the end effector, the method comprising:
receiving a manipulation command to move the end effector with a desired end-effector movement;
determining an end-effector displacing movement of the plurality of joints to effect the desired end-effector movement by calculating joint velocities of the plurality of joints that achieve the desired end-effector movement;
determining an additional movement of the plurality of joints by calculating weighted joint velocities based on a weighting within a joint space of the plurality of joints, the weighting being applied to the calculated joint velocities of the plurality of joints for the end-effector displacing movement, and the additional movement corresponding to the end-effector not being in motion; and
driving the plurality of joints according to a combination of the end-effector displacing movement of the plurality of joints and the additional movement of the plurality of joints.
2. The method of claim 1 , wherein the additional movement is directed towards a desired value comprising a value selected from the group consisting of: a joint state, a combination of joint states, a relative joint state, a range of joint states, a profile of joint states, and a kinetic energy of joint states.
3. The method of claim 1 , wherein the weighted joint velocities for the additional movement of the plurality of joints lie in a null space of a Jacobian of the manipulator arm.
4. The method of claim 1 , wherein calculating the weighted joint velocities includes projecting values based on the joint velocities of the plurality of joints for the end-effector displacing movement onto a null space of a Jacobian of the manipulator arm.
5. The method of claim 1 , wherein the weighting comprises a weighting matrix in the joint space of the plurality of joints.
6. The method of claim 1 , wherein the weighting comprises a quadratic surface within the joint space of the plurality of joints.
7. The method of claim 1 , wherein the weighting comprises a paraboloid surface within the joint space of the plurality of joints.
8. The method of claim 1 , wherein
determining the end effector displacing movement comprises calculating a pseudo-inverse solution of a Jacobian of the manipulator arm; and
determining the additional movement comprises calculating a difference between the pseudo-inverse solution and a potential function gradient of the pseudo-inverse solution.
9. The method of claim 8 , wherein determining the additional movement further comprises projecting the difference onto a null space of the Jacobian to determine the weighted joint velocities of the additional movement.
10. A system comprising:
a manipulator arm comprising a distal portion that is configured to releasably support an instrument that includes an end effector, a proximal portion coupled to a base, and a plurality of joints between the distal portion and the base, the plurality of joints providing sufficient degrees of freedom to allow a range of joint states for a given state of the end effector;
an input device configured to receive a manipulation command to move the end effector with a desired end-effector movement; and
a processor that couples the input device to the manipulator arm, the processor being configured to perform operations including:
determining an end-effector displacing movement of the plurality of joints to effect the desired end-effector movement by calculating joint velocities of the plurality of joints that achieve the desired end-effector movement;
determining an additional movement of the plurality of joints by calculating weighted joint velocities based on a weighting within a joint space of the plurality of joints, the weighting being applied to the calculated joint velocities of the plurality of joints for the end-effector displacing movement, and the additional movement corresponding to the end-effector not being in motion; and
transmitting a command to the manipulator arm in response to the end effector displacing movement to drive the plurality of joints according to a combination of the end-effector displacing movement of the plurality of joints and the additional movement of the plurality of joints.
11. The system of claim 10 , wherein the additional movement is directed towards a desired value comprising a value selected from the group consisting of: a joint state, a combination of joint states, a relative joint state, a range of joint states, a profile of joint states, and a kinetic energy of joint states.
12. The system of claim 10 , wherein the weighted joint velocities for the additional movement of the plurality of joints lie in a null space of a Jacobian of the manipulator arm.
13. The system of claim 10 , wherein calculating the weighted joint velocities includes projecting values based on the joint velocities of the plurality of joints for the end-effector displacing movement onto a null space of a Jacobian of the manipulator arm.
14. The system of claim 10 , wherein the weighting comprises a weighting matrix in the joint space of the plurality of joints.
15. The system of claim 10 , wherein the weighting comprises a quadratic surface within the joint space of the plurality of joints.
16. The system of claim 10 , wherein the weighting comprises a paraboloid surface within the joint space of the plurality of joints.
17. The system of claim 10 , wherein
determining the end effector displacing movement comprises calculating a pseudo-inverse solution of a Jacobian of the manipulator arm; and
determining the additional movement comprises calculating a difference between the pseudo-inverse solution and a potential function gradient of the pseudo-inverse solution.
18. The system of claim 17 , wherein determining the additional movement further comprises projecting the difference onto a null space of the Jacobian to determine the weighted joint velocities of the additional movement.
19. A system comprising:
a manipulator arm comprising a distal portion that is configured to releasably support an instrument that includes an end effector, a proximal portion coupled to a base, and a plurality of joints between the distal portion and the base, the plurality of joints providing sufficient degrees of freedom to allow a range of joint states for a given state of the end effector;
an input device configured to receive a manipulation command to move the end effector with a desired end-effector movement; and
means for determining an end-effector displacing movement of the plurality of joints to effect the desired end-effector movement by calculating joint velocities of the plurality of joints that achieve the desired end-effector movement;
means for determining an additional movement of the plurality of joints by calculating weighted joint velocities based on a weighting within a joint space of the plurality of joints, the weighting being applied to the calculated joint velocities of the plurality of joints for the end-effector displacing movement, and the additional movement corresponding to the end-effector not being in motion; and
a processor-based command unit that is configured to transmit a command to the manipulator arm in response to the end effector displacing movement to drive the plurality of joints according to a combination of the end-effector displacing movement of the plurality of joints and the additional movement of the plurality of joints.
20. The system of claim 19 , wherein the additional movement is directed towards a desired value comprising a value selected from the group consisting of: a joint state, a combination of joint states, a relative joint state, a range of joint states, a profile of joint states, and a kinetic energy of joint states.