Method and system for inverse kinematics with nullspace optimization for human interface device orientation control

Abstract

A method performed by a surgical robotic system that includes determining, while the surgical robotic system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a HID, an orientation of the HID that includes several joints. The system determines that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within 3D space, and responsive user movement of the HID, determines a new orientation of the HID. The system determines, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints to realign the HID from the new orientation to the orientation, and provides the nullspace joint commands to the joints to cause to the realignment of the HID.

Description

Attorney Docket No.: AUR6374WOPCT4 Electronically Filed Method and System for Inverse Kinematics with Nullspace Optimization for Human Interface Device Orientation Control RELATED APPLICATIONS

[0001] This application claims the benefit of priority of U.S. Provisional Patent Application No.63 / 707,181, filed October 14, 2024 and U.S. Provisional Patent Application No.63 / 707,187, filed October 14, 2024, which are herein incorporated by reference in their entirety. FIELD

[0002] Various embodiments of the disclosure relate generally to surgical systems, and more specifically to a surgical system for state-based inverse kinematics with nullspace optimization for smooth motion orientation control of an input device. Other embodiments are also described. BACKGROUND

[0003] Minimally-invasive surgery, MIS, such as laparoscopic surgery, uses techniques that are intended to reduce tissue damage during a surgical procedure. Laparoscopic procedures typically call for creating a number of small incisions in the patient, e.g., in the abdomen, through which several surgical tools such as an endoscope, a blade, a grasper, and a needle, are then inserted into the patient. A gas is injected into the abdomen which insufflates the abdomen thereby providing more space around the tips of the tools, making it easier for the surgeon to see (via the endoscope) and manipulate tissue at the surgical site. MIS can be performed faster and with less surgeon fatigue using a surgical robotic system in which the surgical tools are operatively attached to the distal ends of robotic arms, and a control system actuates the arm and its attached tool. The tip of the tool will mimic the position and orientation movements of an input device as the latter is being manipulated by the surgeon. The surgical robotic system may have multiple surgical arms, one or more of which has an attached endoscope, and others have attached surgical instruments for performing certain surgical actions.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed SUMMARY

[0004] According to one embodiment of the disclosure, a method performed by a surgical robotic system, the method including: determining, while the surgical robotic system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a human interface device (HID), an orientation of the HID that includes several joints; determining that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determining a new orientation of the HID; determining, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and providing the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.

[0005] In one embodiment, the HID includes a gimbal that includes the joints and an end user control arranged to be held by a user, where the orientation of the HID includes an orientation of the gimbal within three-dimensional (3D) space, where the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame. In another embodiment, the joints includes four joints, three joints that are each arranged to rotate about a different axes with respect to the others and a redundant joint with respect to the three joints, where the method further includes generating the nullspace joint motions to perform several secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or more joint motion constraints. In some embodiments, the method further includes determining each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

[0006] In one embodiment, the second operational mode includes at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three-dimensional (3D) space affects corresponding translationalAttorney Docket No.: AUR6374WOPCT4 Electronically Filed movement of the surgical camera. In another embodiment, the joints includes a first group of joints and the HID includes a second group of joints, where each joint includes a respective actuator, where the method further includes, while in the camera mode, determining, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second group of joints, where the one or more joint commands are to cause at least one actuator of the first group of joints to reorientate at least a portion of the HID responsive to the translational movement. In some embodiments, the method further includes, while in the camera mode and for each joint of the second group of joints, determining a gravity- compensation torque based on a joint position of the joint; and applying the gravity- compensation torque using an actuator of the joint.

[0007] In one embodiment, the method further includes, for each joint of the one or more joints, determining a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID; determining a unified constraint based on a current joint position, where the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and producing a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint. In another embodiment, the method further includes determining a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, where adjusting includes, for each joint, applying the scaling factor to a respective target nullspace joint motion.

[0008] In one embodiment, determining the one or more nullspace joint commands includes: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold. In another embodiment, determining the one or more joint commands further includes: determining a scaling factor based on a magnitude of the orientation error; scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions. In another embodiment, the method further includesAttorney Docket No.: AUR6374WOPCT4 Electronically Filed determining a velocity at which the HID has moved from the orientation to the new orientation, where the scaling factor is further based on the velocity. In some embodiments, the method further includes: determining a resulting orientation of the HID based on the determined nullspace joint motions; determining one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and applying the one or more gains to the nullspace joint motions.

[0009] According to another embodiment of the disclosure, a surgical system including: a surgical tool; a human interface device (HID) that includes several joints; at least one processor; and memory having instructions stored therein which when executed by the at least one processor causes the surgical system to: determine, while the surgical robotic system is in a first operational mode in which the surgical tool is configured to be manipulated according to movement of the HID, an orientation of the HID; determine that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determine a new orientation of the HID; determine, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and provide the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.

[0010] In one embodiment, the HID includes a gimbal that includes the joints and an end user control arranged to be held by a user, where the orientation of the HID includes an orientation of the gimbal within three-dimensional (3D) space, where the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame. In another embodiment, the joints includes four joints, three joints that are each arranged to rotate about a different axes with respect to the others and a redundant joint with respect to the three joints, where the memory includes further instructions to generate the nullspace joint motions to perform several secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or moreAttorney Docket No.: AUR6374WOPCT4 Electronically Filed joint motion constraints. In some embodiments, the memory includes further instructions to determine each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

[0011] In one embodiment, the second operational mode includes at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three-dimensional (3D) space affects corresponding translational movement of the surgical camera. In another embodiment, the joints includes a first group of joints and the HID includes a second group of joints, where each joint includes a respective actuator, where the memory includes further instructions to, while in the camera mode, determine, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second group of joints, where the one or more joint commands are to cause at least one actuator of the first group of joints to reorientate at least a portion of the HID responsive to the translational movement. In some embodiments, the memory includes further instructions to, while in the camera mode and for each joint of the second group of joints, determine a gravity- compensation torque based on a joint position of the joint; and apply the gravity- compensation torque using an actuator of the joint.

[0012] In one embodiment, the memory includes further instructions to, for each joint of the one or more joints, determine a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID; determine a unified constraint based on a current joint position, where the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and produce a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint. In some embodiments, the memory includes further instructions to determine a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, where adjusting includes, for each joint, applying the scaling factor to a respective target nullspace joint motion.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0013] In one embodiment, determining the one or more nullspace joint commands includes: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold. In another embodiment, determining the one or more joint commands further includes: determining a scaling factor based on a magnitude of the orientation error; scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions. In some embodiments, the memory includes further instructions to determine a velocity at which the HID has moved from the orientation to the new orientation, where the scaling factor is further based on the velocity. In another embodiment, the memory includes further instructions to: determine a resulting orientation of the HID based on the determined nullspace joint motions; determine one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and apply the one or more gains to the nullspace joint motions.

[0014] According to another embodiment of the disclosure, a non-transitory machine-readable medium having instructions which when executed by at least one processor of a surgical system causes the surgical system to: determine, while the surgical robotic system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a human interface device (HID), an orientation of the HID that includes several joints; determine that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determine a new orientation of the HID; determine, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and produce the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0015] In one embodiment, the HID includes a gimbal that includes the joints and an end user control arranged to be held by a user, where the orientation of the HID includes an orientation of the gimbal within three-dimensional (3D) space, where the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame. In another embodiment, the joints includes four joints, three joints that are each arranged to rotate about a different axes with respect to the others and a redundant joint with respect to the three joints, where the non-transitory machine-readable medium includes further instructions to generate the nullspace joint motions to perform several secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or more joint motion constraints. In some embodiments, the non-transitory machine-readable medium includes further instructions to determine each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

[0016] In one embodiment, the second operational mode includes at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three-dimensional (3D) space affects corresponding translational movement of the surgical camera. In another embodiment, the joints includes a first group of joints and the HID includes a second group of joints, where each joint includes a respective actuator, where the non-transitory machine-readable medium includes further instructions to, while in the camera mode, determine, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second group of joints, where the one or more joint commands are to cause at least one actuator of the first group of joints to reorientate at least a portion of the HID responsive to the translational movement. In some embodiments, the non- transitory machine-readable medium including further instructions to, while in the camera mode and for each joint of the second group of joints, determine a gravity- compensation torque based on a joint position of the joint; and apply the gravity- compensation torque using an actuator of the joint.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0017] In one embodiment, the non-transitory machine-readable medium including further instructions to, for each joint of the one or more joints, determine a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID; determine a unified constraint based on a current joint position, where the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and produce a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint. In another embodiment, the non-transitory machine-readable medium including further instructions to determine a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, where adjusting includes, for each joint, applying the scaling factor to a respective target nullspace joint motion.

[0018] In one embodiment, determining the one or more nullspace joint commands includes: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold. In another embodiment, determining the one or more joint commands further includes: determining a scaling factor based on a magnitude of the orientation error; scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions. In another embodiment, the non-transitory machine- readable medium including further instructions to determine a velocity at which the HID has moved from the orientation to the new orientation, where the scaling factor is further based on the velocity. In some embodiments, the non-transitory machine- readable medium including further instructions to: determine a resulting orientation of the HID based on the determined nullspace joint motions; determine one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and apply the one or more gains to the nullspace joint motions.

[0019] According to another embodiment of the disclosure, a system, an apparatus, or an electronic device as shown and as described herein. According to another embodiment of the disclosure, a method substantially as herein described.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed According to another embodiment of the disclosure, includes a processor configured to perform one or more operations as described herein. According to another embodiment of the disclosure, includes a non-transitory machine-readable medium that includes instructions which when executed by at least one processor causes a system to perform one or more operations as described herein.

[0020] The above summary does not include an exhaustive list of all embodiments of the disclosure. It is contemplated that the disclosure includes all systems and methods that can be practiced from all suitable combinations of the various embodiments summarized above, as well as those disclosed in the Detailed Description below and particularly pointed out in the claims. Such combinations may have particular advantages not specifically recited in the above summary.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed BRIEF DESCRIPTION OF THE DRAWINGS

[0021] The embodiments are illustrated by way of example and not by way of limitation in the figures of the accompanying drawings in which like references indicate similar elements. It should be noted that references to "an" or “one” embodiment of this disclosure are not necessarily to the same embodiment, and they mean at least one. Also, in the interest of conciseness and reducing the total number of figures, a given figure may be used to illustrate the features of more than one embodiment, and not all elements in the figure may be required for a given embodiment.

[0022] Fig.1 shows a pictorial view of an example surgical system in an operating arena.

[0023] Fig.2 shows an example of a user-side human interface device (HID) of the user console with which an operator may use to control a robotic component of the surgical system according to one embodiment of the disclosure.

[0024] Fig.3 shows a patient-side robotic arm that includes a surgical tool of the surgical system according to one embodiment of the disclosure.

[0025] Figs.4A and 4B illustrates an example of camera control using multiple HIDs according to one embodiment of the disclosure.

[0026] Fig.5 is a block diagram of the surgical system for performing state- based inverse kinematics (IK) with nullspace optimization for smooth motion orientation control of the HID according to one embodiment.

[0027] Figs.6A and 6B show a flowchart of one embodiment of a process for performing state-based IK for smooth motion orientation control of at least one HID.

[0028] Fig.7 is a flowchart of another embodiment of a process for performing state-based IK for smooth motion orientation control of at least one HID.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed DETAILED DESCRIPTION

[0029] Several embodiments of the disclosure with reference to the appended drawings are now explained. Whenever the shapes, relative positions and other embodiments of the parts described in a given embodiment are not explicitly defined, the scope of the disclosure here is not limited only to the parts shown, which are meant merely for the purpose of illustration. Also, while numerous details are set forth, it is understood that some embodiments may be practiced without these details. In other instances, well-known circuits, structures, and techniques have not been shown in detail so as not to obscure the understanding of this description. Furthermore, unless the meaning is clearly to the contrary, all ranges set forth herein are deemed to be inclusive of each range’s endpoints.

[0030] Fig.1 shows a pictorial view of an example (e.g., laparoscopic) surgical system (which hereafter may be referred to as “system”) 100 in an operating arena. The system 100 includes a user console 120, a control tower 131, and one or more surgical robotic arms 110 at a surgical robotic table (surgical table or surgical platform) 151. In one embodiment, the arms 110 may be mounted to a table or bed on which the patient rests as shown in the example of Fig.1. In one embodiment, at least some of the arms 110 may be configured differently. For example, at least some of the arms may be mounted on a ceiling, sidewall, or in another suitable structural support, such as a cart separate from the table. The system 100 can incorporate any number of devices, tools, or accessories used to perform surgery on a patient 161. For example, the system 100 may include one or more surgical tools (instruments) 171 used to perform surgery (surgical procedure). A surgical tool 171 may be an end effector that is attached to a distal end of a surgical arm 110, for executing a surgical procedure.

[0031] Each surgical tool 171 may be manipulated manually, robotically, or both, during the surgery. For example, the surgical tool 171 may be a tool used to enter, view, or manipulate an internal anatomy of the patient 161. In an embodiment, the surgical tool 171 may include a grasper that can grasp tissue of the patient. In another embodiment, the surgical tool may include one or more cameras (e.g., an endoscopic camera), which may be configured to capture images of a surgical site in which one or more other surgical tools 171 may be used to perform one or moreAttorney Docket No.: AUR6374WOPCT4 Electronically Filed surgical tasks. For instance, the camera may be arranged to have a field of view that includes a surgical site with one or more other surgical tools, which may be manipulated by an operator. The surgical tool 171 may be controlled manually by a bedside operator 180; or it may be controlled robotically, via actuated movement of the surgical robotic arm 110 to which it is attached. For example, when manually controlled an operator may (e.g., physically) hold a portion of the tool (e.g., a handle), and may manually control the tool by moving the handle and / or pressing one or more input controls (e.g., buttons) on the (e.g., handle of the) tool. In another embodiment, when controlled robotically, the surgical system may manipulate the surgical tool-based user input (e.g., received via the user console 120, as described herein).

[0032] Generally, a remote operator 190, such as a surgeon or other operator, may use the user console 120 to remotely manipulate the arms 110 and / or the attached surgical tools 171, e.g., during a teleoperation. The user console 120 may be located in the same operating room as the rest of the system 100, as shown in Fig.1. In other environments however, the user console 120 may be located in an adjacent or nearby room, or it may be at a remote location, e.g., in a different building, city, or country. The user console 120 may include one or more components, such as a seat 119, one or more foot-operated controls (or foot pedals) 130, one or more human interface devices (HIDs) 140, and at least one display 150. In one embodiment, the user console may include less components. For example, the seat 119 may be separate from the user console. This may allow the user to sit on the seat and to move into a position that allows the user to use the console. In particular, the seat may be on casters that allow the seat to move on the floor.

[0033] The display 150 is configured to display, for example, a view of the surgical site inside the patient 161. The display may be configured to display image data (e.g., still images and / or video) that may be captured by a camera that may be used during a surgical procedure, as described herein. In one embodiment, the display may be any type of display, such as a liquid crystal display (LCD), a light-emitting diode (LED) display, an organic LED (OLED) display, etc. In some embodiments, the display may be a three-dimensional (3D) immersive display that is for displaying 3D (surgical) presentations. For instance, during a surgical procedure one or more endoscopic cameras may be capturing image data of a surgical site, which the displayAttorney Docket No.: AUR6374WOPCT4 Electronically Filed presents to the user in 3D. The display may include a viewer that includes one or two screens that may be configured to display stereoscopic images. In this case, the viewer of the display may include a contoured portion that may be arranged to come into contact with and contour to at least a portion of the operator’s face when the remote operator 190 moves towards and / or comes into contact with the display. Coming into contact and contouring with the operator’s face may prevent ambient light from interfering with the displayed surgical presentation, while the screen(s) of the display 150 may give the remote operator 190 a perception of a 3D space captured by one or more cameras. In one embodiment, the 3D display may be an autostereoscopic display that provides 3D perception to the user without the need for special glasses. As another example, the 3D display may be a stereoscopic display that provides 3D perception with the use of glasses (e.g., via active shutter or polarized).

[0034] In another embodiment, the display 150 may be configured to display at least one graphical user interface (GUI) that may provide informative and / or interactive content, to thereby assist a user in performing a surgical procedure with one or more instruments in the surgical system 100. For example, some of the content displayed may include image data captured by one or more endoscopic cameras, as described herein. In another embodiment, the GUI may include selectable UI items, which when manipulated by the user may cause the system to perform one or more operations. For instance, the GUI may include a UI item as interactive content to switch control between robotic arms. In one embodiment, to interact with the GUI, the system may include input devices, such as a keyboard, a mouse, etc. In another embodiment, the user may interact with the GUI using the HID 140. For instance, the user may manipulate the HID to navigate through the GUI, (e.g., with a cursor), and to make a selection may hover the cursor over a UI item and manipulate the HID (e.g., selecting a control or button). In some embodiments, the display may be a touch-sensitive display screen. In this case, the user may perform a selection by navigating and selecting through touching the display. In some embodiments, any method may be used to navigate and / or select a UI item.

[0035] As shown, the remote operator 190 is sitting in the seat 119 and viewing the user display 150 while manipulating a foot-operated control 130 and a handheldAttorney Docket No.: AUR6374WOPCT4 Electronically Filed HID 140 in order to remotely control one or more of the arms 110 and the surgical tools 171 (that are mounted on the distal ends of the arms 110.)

[0036] In some variations, the bedside operator 180 may also operate the system 100 in an “over the bed” mode, in which the bedside operator 180 (user) is now at a side of the patient 161 and is simultaneously manipulating a robotically-driven tool (end effector as attached to the arm 110), e.g., with a handheld HID 140 held in one hand, and a manual laparoscopic tool. For example, the bedside operator’s left hand may be manipulating the handheld HID to control a robotic component, while the bedside operator’s right hand may be manipulating a manual laparoscopic tool. Thus, in these variations, the bedside operator 180 may perform both robotic-assisted minimally invasive surgery and manual laparoscopic surgery on the patient 161.

[0037] During an example procedure (surgery), the patient 161 is prepped and draped in a sterile fashion to achieve anesthesia. Initial access to the surgical site may be performed manually while the arms of the system 100 are in a stowed configuration or withdrawn configuration (to facilitate access to the surgical site.) Once access is completed, initial positioning or preparation of the system 100 including its arms 110 may be performed. Next, the surgery proceeds with the remote operator 190 at the user console 120 utilizing the foot-operated controls 130 and the HIDs 140 to manipulate the various end effectors and perhaps an imaging system, to perform the surgery. Manual assistance may also be provided at the procedure bed or table, by sterile- gowned bedside personnel, e.g., the bedside operator 180 who may perform tasks such as retracting tissues, performing manual repositioning, and tool exchange upon one or more of the robotic arms 110. Non-sterile personnel may also be present to assist the remote operator 190 at the user console 120. When the procedure or surgery is completed, the system 100 and the user console 120 may be configured or set in a state to facilitate post-operative procedures such as cleaning or sterilization and healthcare record entry or printout via the user console 120.

[0038] In one embodiment, the remote operator 190 holds and moves the HID 140 to provide an input command to drive (move) one or more robotic arm actuators 170 (or driving mechanism) in the system 100 for teleoperation. The HID 140 may be communicatively coupled to the rest of the system 100, e.g., via a console computerAttorney Docket No.: AUR6374WOPCT4 Electronically Filed system 160 (or host). The HID 140 can generate spatial state signals corresponding to movement of the HID 140, e.g., position and orientation of the handheld housing of the HID, and the spatial state signals may be input signals to control motions of the robotic arm actuators 170. The system 100 may use control signals derived from the spatial state signals, to control proportional motion of the actuators 170. In one embodiment, a console processor of the console computer system 160 receives the spatial state signals and generates the corresponding control signals. Based on these control signals, which control how the actuators 170 are energized to drive a segment or link of the arm 110, the movement of a corresponding surgical tool that is attached to the arm may mimic the movement of the HID 140. Similarly, interaction between the remote operator 190 and the HID 140 can generate, for example, a grip control signal that causes a jaw of a grasper of the surgical tool 171 to close and grip the tissue of patient 161.

[0025] The system 100 may include one or more user-side (or surgeon-side) HIDs 140, where respective control signals are generated for each HID that control the actuators and the surgical tool (end effector) of a respective arm 110. For example, the user console 120 may include two HIDs 140, such as a left (or first) HID 140A arranged to be held and controlled by the operator’s left hand and a right (or second) HID 140B arranged to be held and controlled by the operator’s right hand, as shown in Figs 4A and 4B. In which case, the remote operator 190 may move the left HID 140A to control the motion of an actuator 170 that is in a one robotic arm, where the actuator responds by moving linkages, gears, etc., in that arm 110. Similarly, movement of the right HID 140B by the remote operator 190 controls the motion of another actuator 170, which in turn drives other linkages, gears, etc., of (e.g., another robotic component, such as a robotic arm of) the system 100. The system 100 may include a right arm 110 that is secured to the bed or table to the right side of the patient, and a left arm 110 that is at the left side of the patient. An actuator 170 may include one or more motors that are controlled so that they drive the rotation of a joint of the arm 110, to for example change, relative to the patient, an orientation of an endoscope or a grasper of the surgical tool 171 that is attached to that arm. Motion of several actuators 170 in the same arm 110 can be controlled by the spatial state signals generated from a particular HID 140. The HIDs 140 can also control motion of respective surgical tool graspers. For example, each HID 140 can generate a respective grip signal to control motion ofAttorney Docket No.: AUR6374WOPCT4 Electronically Filed an actuator, e.g., a linear actuator that opens or closes jaws of the grasper at a distal end of surgical tool 171 to grip tissue within patient 161.

[0026] In some embodiments, the communication between the surgical robotic table 151 and the user console 120 may be through a control tower 131, which may translate user commands that are received from the user console 120 (and more particularly from the console computer system 160) into robotic control commands that transmitted to the arms 110 on the surgical table 151. The control tower 131 may also transmit status and feedback from the surgical table 151 back to the user console 120. The communication connections between the surgical table 151, the user console 120, and the control tower 131 may be via wired (e.g., optical fiber) and / or wireless links, using any suitable one of a variety of wireless data communication protocols, such as BLUETOOTH protocol. Any wired connections may be optionally built into the floor and / or walls or ceiling of the operating room. The system 100 may provide video output to one or more displays, including displays within the operating room as well as remote displays that are accessible via the Internet or other networks. The video output or feed may also be encrypted to ensure privacy and all or portions of the video output may be saved to a server or electronic healthcare record system.

[0027] As described herein, this shows an example of the operator 190 using the user console 120 to control one or more robotic components, such as a robotic arm 110 and / or a surgical tool 171 coupled to a distal end of the robotic arm, of the surgical system 100. Specifically, this figure shows the operator 190 seated on the seat 119 in front of the user console 120, while controlling (or moving) one or more robotic components by manipulating control inputs, such as one or both of the HIDs 140 and / or the foot pedal(s) 130.

[0028] The user console 120 includes a base 201 to which the foot pedals 130 may be attached and a support structure 202 that may be mounted on the base 201 and extends vertically upward from the base. Coupled to the support structure 202 includes the display 150 and the HIDs 140. As shown, the HIDs may be coupled (mounted) on a bottom side of a structure (e.g., an arm rest) that is coupled to the support structure 202. In which case, the HID may be a wired or “grounded” input device that may be connected to the user console via a wired connection (e.g., to exchange data), which isAttorney Docket No.: AUR6374WOPCT4 Electronically Filed in contrast to an “ungrounded’ input device that may be wirelessly connected to the user console (e.g., exchanging data via a wireless connection). As described herein, the HID 140 may include one or more joints that couple one or more links, which allow the HID to have one or more rotational degrees of freedom. In one embodiment, the HID may be grounded such that the joints of the HID may be arranged to hold their position and / or orientation absent to an external applied force upon the HID. More about the configuration of the HID is described herein.

[0029] In another embodiment, the HIDs may be coupled to the user console in other configurations. For example, the HIDs may be separate from the user console, but may be communicatively coupled (e.g., through wired-connection). In which case, the HIDs may be mounted on another device (e.g., a surgical cart), which may be coupled to the user console.

[0030] As described herein, the operator 190 may be using the user console to control a robotic arm during a teleoperation. As shown, the user may be viewing a surgical workspace through the display 150, while operating one or more components of the surgical system by manipulating one or both of the HIDs 140 and / or the foot pedals 130.

[0031] Fig.2 shows an example of the HID 140 of the user console 120 with which the operator 190 may control a robotic arm of the surgical system 100 during a surgical procedure. In particular, this figure shows the HID 140 that includes a support arm 200 and a gimbal 205, which may include one or more links coupled together by one or more joints. As described herein, the HID 140 may be a low-inertia such that as the user manipulates the HID, one or more joints may move (or be adjusted) such that the HID may be moved between positions (or poses) within a workspace surrounding the user console 120. The support arm 200 may be configured to couple the gimbal 205 to the user console 120, where the gimbal may be arranged to rotate about one or more axes and / or the support arm may be arranged to rotate about one or more axes to allow the user to move an end user control 220 of the gimbal within three-dimensional (3D) space. Movement of the end user control may allow the user to control (e.g., movement of) a robotic component, such as a robotic arm, of the surgical system. In oneAttorney Docket No.: AUR6374WOPCT4 Electronically Filed embodiment, each of the joints may be capable of rotating about and / or translating along one or more axes.

[0032] As shown, the HID 140 includes seven joints 215a-215g, which may provide the HID 140 with seven (rotational) degrees of freedom (DoF). In particular, the support arm 200 includes three links 210a-210c and three joints 215a-215c. In particular, link 210a may be coupled to link 210b, via joint 215b, and link 210b may be coupled to link 210c, via joint 215c. In one embodiment, the HID 140 may be arranged to couple to the support arm 200 at the joint 215a. The gimbal 205 may be coupled to the support arm 200 via joint 215d. The gimbal includes three links 210d-210f and four joints 215d-215g. Link 210d may be coupled to joint 215d and joint 215e. Link 210d may be coupled to link 210e, via joint 215e, and link 210e may be coupled to link 210f, via joint 215f. The gimbal 205 may also include an end user control 220 that may be coupled to link 210f via joint 215g.

[0033] As described herein, the HID may include seven DoFs. In one embodiment, one or more DoF may be redundant with respect to other DoF. For example, joint 215d may be redundant with respect to joint 215f, since both joints allow the end user control 220 to rotate about a same axis (e.g., a Z-axis). In which case, the system 100 may be configured to provide null space motion in which one or more joints of the HID may be moved, while a pose of an end effector (e.g., the end user control 220) may remain unaffected. As described herein, this redundancy may allow the system to perform one or more secondary tasks by manipulating one or more joints, without affecting the end user control 220. For instance, as described herein, the gimbal 205 may include four joints 215d, 215e, 215f, and 215g, where the last three may provide a 3D range of motion, and the first joint, 215d, may be redundant to provide a range of motion that may be redundant with respect to joint 215f, whereby motion of joint 215d may occur without affecting (or causing motion) to the other three joints. In one embodiment, a secondary task may include an adjustment of one or more joints, which may include joint 215d, in order to provide the user with a more ergonomic position of the end user control 220, without affecting the pose of the end user control.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0034] In one embodiment, the HID 140 may include more or fewer links and / or joints, which may provide more or fewer DoFs for the HID. For example, joint 215a may be arranged to rotate the entire HID about an axis (e.g., the Z-axis) with respect to the link 210a. Conversely, joint 215b may be arranged to rotate link 210b (and the links / joints to which the link 210b may be coupled) about an orthogonal axis (e.g., Y-axis) to the axis that runs through link 210a and joint 215b.

[0035] In one embodiment, at least some of the joints may include one or more motors (or actuators) that may allow the joint to rotate around and / or translate along one or more axes. In this way, the HID may provide a user with assisted movement in response to user input, such as a user applying an external force upon the end user control 220. In another embodiment, the HID 140 may be arranged to provide haptic feedback to the user. As described herein, the HID may be configured to control movement of a robotic component, such as a surgical tool coupled to a distal end of a robotic arm. As a result of this movement, external forces may be applied onto the surgical tool when the tool presses up against an object, such as pressing up against an abdominal wall of a patient during a surgical procedure. This force applied by the object may be haptically applied by the HID such that the operator may perceive the force. In one embodiment, the system 100 may be configured to determine whether an external force is being applied to the robotic arm, such as through one or more sensors (e.g., force sensors). In another embodiment, the system may determine that the robotic arm is pressing onto an object, based on a comparison between control commands from the HID and encoder data from one or more encoders of the robotic arm. Upon determining this external force, the system may be configured to provide haptic feedback to the user by controlling one or more motors of the HID to relay an applied force through the HID onto the user. In one embodiment, this applied force may be an opposite (and / or proportional) external force applied onto the robotic arm. More about providing haptic feedback force is described herein.

[0036] As shown in this figure, the HID 140 may be the left HID 140A that may be arranged to be held by the user’s left hand. In one embodiment, the right HID 140B may include at least some of the components of the left HID 140A, as described herein. For example, the right HID 140B may include the same number of joints and / or links of the left HID. The orientation of at least some of the components of the rightAttorney Docket No.: AUR6374WOPCT4 Electronically Filed HID, however, may be inverse to those of the left HID 140A. In either case, both HIDs may be manipulated either individually by the remote operator 190 or simultaneously to perform one or more surgical tasks.

[0037] Fig.3 shows a patient-side robotic arm 110 that includes the surgical tool 171 of the surgical system 100 according to one embodiment of the disclosure. The surgical arm includes ten links 303a-303j that are coupled together through nine joints 302a-302i, where each joint may be arranged to move a correspondingly coupled link by at least one of rotating the link about at least one axis and / or translating the link along at least one axis. For instance, each joint may include an actuator, which may move its corresponding joint based on user input (e.g., user commands) received through movement of the HID 140. In one embodiment, the robotic arm may include more or less joints and / or links.

[0038] The robotic arm may be coupled to a structure (such as the surgical table 151 or a surgical cart) at the most proximal joint 302a. Coupled to the most distal link 303j includes a tool drive 305 configured to control a surgical tool 171. In another embodiment, any type of end effector may be coupled to the tool drive, such as an endoscopic camera. Coupled to the tool drive 305 is a cannula 304 that may be inserted into a patient’s cavity (e.g., abdominal region), where the (e.g., end effector of the) surgical tool may be received through the cannula.

[0039] Figs.4A and 4B illustrates an example of camera control using multiple HIDs 140A and 140B according to one embodiment of the disclosure. Each figure includes a perspective view of a portion of the user console 120 that shows both the left HID 140A and the right HID 140B, and includes a top-down view of a surgical site 401 that includes a camera (e.g., endoscope) 471 that may be inserted into a patient in order for the remote operator 190 to view anatomy 430 to perform a surgical task. The camera 471 may be coupled to a (e.g., tool drive 305) of a robotic arm 110, and may include one or more joints that enable the camera to be moved within three-dimensional (3D) space in the surgical site 401 by at least one HID 140.

[0040] The system 100 may be capable of entering into a camera control (or camera) mode (or state) in which at least one of the HIDs may be configured to control the position of the camera 471. For instance, once the system enters this camera controlAttorney Docket No.: AUR6374WOPCT4 Electronically Filed state, the joints of each HID’s support arm 200 may enter into a manual control mode or a gravity compensation mode (e.g., impedance mode) in which actuators of each joint may apply torque to counteract the force of gravity, and may be able to be manually manipulated by an external force (e.g., the user pushing and / or pulling on a portion of the HID). For instance, if a user were to move the left HID 140A in the X- direction, the HID (e.g., the gimbal 205) may move or translate within 3D space about the axis of joint 215a. In one embodiment, camera movement may be due to dual movement of both HIDs in the desired direction.

[0041] In one embodiment, movement of the camera 471 may be in response to translational movement (e.g., in Cartesian space about the X, Y, and / or Z axes) of one or both of the HIDs 140A, 140B, and in which case the orientation of each gimbal 205 may not be considered or used for camera movement. For instance, the system 100 may determine, while in the camera mode, responsive to user input to one or both of the HIDs, translational movement based on sensor data of one or more sensors (e.g., 551A and / or 551B) of one or more joints of the HIDs. In this case, the joints may include 215a, 215b, and 215c of each support arm 200. Figs.4A and 4B illustrate an example, whereby in Fig.4A the camera 471 is misaligned with respect to the anatomy 430, since the field of view 410 of the camera 471 is only capturing a portion of the anatomy. As a result, the user may wish to move the camera in order for all of the anatomy to be in the view of the camera. In Fig.4B, the user has moved the camera 471 in the Y-direction in order to capture the whole anatomy 430 in its field of view by moving both HIDs 140A and 140B along the same direction.

[0042] As described herein, the system 100 may be configured to control movement of the robotic arm based on user input through the HID 140. In particular, the system may be configured to cause one or more joints of the robotic arm to move in order to match motion of the HID. To match motion, the system may attempt to align (or match) a pose of the (e.g., end user control 220 of the) HID 140 with that of the surgical tool 171 of the robotic arm with respect to at least one reference frame. For instance, the system 100 may be configured to receive user input through the HID, where the user input may indicate one or more spatial state signals corresponding to movement of the HID, as described herein. From this input, the system may determine a pose of the HID, or more specifically a pose of the end user control of the HID, andAttorney Docket No.: AUR6374WOPCT4 Electronically Filed may be configured to generate one or more control (or joint) commands to cause one or more joints of the robotic arm to actuate in order for a pose of the surgical tool coupled to the robotic arm to match the pose of the end user control, as perceived by the user through the display 150 of the user console 120. The system may perform one or more conversations of the HID pose with respect to one or more reference frames into the pose of the tool 171 with respect to the reference frames. One reference frame may be with respect to the display. As described herein, the user may control the surgical instrument by manipulating the HID, while viewing the surgical instrument through the display 150 that may receive video images from one or more cameras. In which case, the system may define tool motion with respect to the camera to follow the HID motion with respect to the display. Thus, the system may convert a user command (e.g., HID pose based on position data from sensors 551A or 551B, such as encoders of the HID) that may with respect to the display 150 to a target (or desired) tool pose with respect to a camera of the system 100. As a result, the system may ensure that the pose of the HID matches the pose of the surgical tool, where both may share a global reference frame. In one embodiment, the system may be configured to adjust a pose of the surgical tool according to movement (or changing poses of the HID) in real-time, whereby motion of the tool may track motion of the HID in real-time and during a surgical procedure.

[0043] As described herein, user input through the gimbal 205 of the HID 140 by the user manipulating the HID may cause the surgical tool 171 to perform a similar motion in order for the user to control and manipulate the surgical tool during a surgical procedure. In some situations, however, the user may wish to control other aspects of the system 100 with the HID 140. In which case, the system may be configured to operate in one of several operational modes, where in each mode the HID may be used to control or perform various tasks. One operational mode may be a tool control mode in which the HID may be used to control one or more surgical tools, as described herein. Other operational modes may include a camera mode in which one or more HIDs may be used to control one or more cameras based on translational movement, as described herein. Another example of an operational mode may include a clutch mode, in which the HID may move independently from other robotic components. In this mode, the user may move the HID about the user console, without effectuating movement of a surgical tool or a camera. The user may enter such a mode in order toAttorney Docket No.: AUR6374WOPCT4 Electronically Filed adjust the position of the HID. In both of these situations, the user may manipulate the HID by translating the gimbal 205 (while holding onto the end user control 220) along one or more axes in 3D space, due to movement of one or more joints of the support arm 200.

[0044] In robot-assisted surgical procedures, surgeons rely on precise control of robotic tools to perform delicate operations. It is important that when switching between the tool control mode and one of the other modes that the gimbal 205 remain aligned with the last orientation of the gimbal before the user exited the tool control mode. For example, when switching from the tool control mode to another mode, the user’s manipulation of the HID while in this other mode may cause the orientation of the gimbal 205 to change. Once the user wishes to switch back to the tool control mode, the system 100 will need to reorient the gimbal to its last determined orientation from the last time the system was in the tool control mode in order to align the HID with the surgical tool. Such a realignment may result in the HID performing abrupt or unstable movements, which may cause the remote operate 190 to inadvertently release control of the HID. In addition, realignments may force the HID into a “dead zone” (e.g., positions where flexibility is lost), areas with different manipulability (e.g., lacking freedom to move in a direction the remote operator may want), or imperfect resulting alignment with the surgical tool. Moreover, user movement of the gimbal 205 may result in the user reaching unintentional positions that may be physically uncomfortable for the user or exceed the robot’s motion limits. Therefore, there is a need for a surgical system for state-based inverse kinematics (IK) with nullspace optimization for smooth motion orientation control of the HID while the HID is in an operational mode in which the HID moves while the surgical tool remains stationary in order to allow a smooth transition back into the tool control mode.

[0045] The present disclosure provides a method and system for performing an IK algorithm with nullspace optimization for smooth motion orientation control between HID orientations as the HID moves within 3D space. For instance, the system may determine nullspace joint motion, e.g., joint velocities that may not affect the controlling pose of the HID (e.g., the pose of the end user control 220), in order maintain HID orientation as the user manipulates the HID between system operational modes. For instance, while in tool control mode, the system 100 may monitor theAttorney Docket No.: AUR6374WOPCT4 Electronically Filed orientation of the gimbal 205 of the HID. Once the user enters a different operational mode, such as clutch mode, the system may perform the IK algorithm with nullspace optimization in order to keep the gimbal orientation aligned with the last measured orientation in the tool control mode. For instance, the system 100 may implement an optimization method (e.g., damped least squares) to achieve secondary task objectives, such as maximizing manipulability to address singularities, object avoidance, maximizing user comfort (ergonomics), and guaranteeing smooth motion continuity by staying within joint limits, such as joint velocity limits and acceleration limits. Thus, unlike conventional systems that struggle to transition smoothly between operational modes, especially in confined surgical workspaces, the present disclosure provides an intelligent motion control framework that ensures seamless, natural repositioning of the HID, avoids unstable mechanical limits, and preserves precise alignment with the HID and the surgical tool.

[0046] Fig.5 is a block diagram of the surgical system 100 for performing state-based IK with nullspace optimization for multi-objective smooth motion orientation control of a HID according to one embodiment. The controller 500 includes a state transition handler 501, a HID / tool pose estimator 510, an IK 520, which includes a HID orientation error estimator 530, a secondary task joint motion constraint generator 540, and a trajectory planner 560. In one embodiment, at least some of the operations performed by the controller 500 may be performed on a different device. For instance, when the controller 500 is a part of an electronic device within the surgical operating room, such as the console computer system 160, at least some operations may be performed by the console computer system 160. In which case, the control tower 131 may perform at least some of the operations of the controller 500, and may provide data (e.g., through a computer network) to the controller 500.

[0047] The HIDs 140A and 140B include one or more sensors 551A and 551B, and one or more actuators 515A and 515B, respectively. In one embodiment, the HIDs sensors and / or actuators may perform similar operations as the sensors 590 and / or actuators 170, respectively, of the robotic arm 110. For example, the sensors 551A may be configured to produce position data of one or more joints, while the actuators 515A may be configured to move one or more corresponding joints of the HID 140A, based on joint commands produced by the system 100.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0048] The state transition handler 501 may be configured to determine which operational mode in which the system is to operate. In particular, the handler 501 may be configured to receive user input through at least one of the HID 140 and / or a foot pedal 130, based on which the handler may determine which mode the system is to operate. For example, the user console 120 may include a foot pedal designated as a “clutch foot pedal”, where if pressed by the user, the system will enter into the clutch mode and may stay in the mode while input is received. As a result, the handler may be configured to switch between modes by enabling / disabling control signals to be exchanged between one or more of the HIDs 140 and one or more robotic arms 110, based on the user input.

[0049] The HID / Tool pose estimator 510 may be configured to estimate a pose of the HID 140. The HID pose may include a six degrees of freedom (6 DoF) pose of the end user control 220 (e.g., in three-dimensional (3D) space) that is being held by the user while moving the HID. The estimator 510 may be configured to convert one or more joint commands for one or more joints of the HID into a HID pose. For instance, when the joint commands includes position data of one or more joints of the HID, the estimator 510 may use a forward kinematics algorithm to convert the data into the HID pose. The estimator 510 may be configured to determine a pose of the robotic arm 110. In particular, the estimator 510 may be configured to receive sensor data from one or more sensors 590, which may indicate positions of one or more joints of the robotic arm 110, and may be configured to convert the sensor data into a (e.g., tool) pose of the surgical tool 171. In one embodiment, both poses may be with respect to a same global reference frame.

[0050] The HID / tool pose estimator 510 may be configured to determine a target HID pose and / or a target orientation of the HID 140 based on movement of the robotic arm 110, as described herein. The estimator 510 may be configured to convert the tool pose into the target HID pose, which may be in response to the tool pose changing independently from any user input through the HID. The conversion may allow for the pose of the HID and the pose of the surgical tool to be the same (converge with each other within a tolerance) with respect to one or more reference frames. As described herein, the user may control the surgical tool by manipulating the HID, while viewing the surgical tool through the display 150 that may receive video images fromAttorney Docket No.: AUR6374WOPCT4 Electronically Filed one or more cameras. In which case, the system may convert the HID pose into a desired tool pose such that tool motion with respect to the camera follows the HID motion with respect to the display. Thus, the system may convert a user command (or HID pose based on position data from the HID) that may with respect to the display 150 to a desired tool pose with respect to a camera of the system 100. The system may perform this conversion by processing the HID pose through one or more transformations into the desired tool pose. Similarly, the estimator 510 may convert a tool pose of the surgical tool 171 of the robotic arm 110 into a target HID pose through one or more (e.g., inverse) transformations. As a result, the HID / tool pose estimator 510 may be configured to determine a current tool pose of the (surgical tool 171 of the) robotic arm 110 based on sensor data from the sensors 590, by performing a forward kinematics process to convert the sensor data into a 6 DoF tool pose, and may use the tool pose to determine a target (or desired) HID pose, which may match (within a tolerance, for example) the tool pose with respect to a global reference frame.

[0051] In another embodiment, the HID / Tool pose estimator 510 may be configured to determine an orientation of the HID 140, which may be based on the estimated (or target) pose. For instance, the estimator 510 may be configured to determine an orientation using a rotational matrix derived from a determined pose of the HID. In one embodiment, the orientation of the HID may include multiple orientations of multiple joints of the HID. In one embodiment, the estimator may monitor sensor data from either or both devices, and may estimate their orientations in real time. For instance, the estimator 510 may estimate the orientation of the HID, as the user holds the end user control 220 and moves the end user control within 3D space.

[0052] The IK 520 may be configured to perform an iterative process for generating primary task joint motion and / or nullspace joint motion for one or more secondary tasks, where the nullspace joint motion may be for one or more joints of the HID 140 to maintain alignment between the (e.g., gimbal 205 of the) HID 140 and the surgical tool 171, while the system operates in an operational mode other than the tool control mode. To perform at least some of these operations, for each iteration, the IK 520 includes a HID orientation error estimator 530, a secondary task joint motion constraint generator 540, and a trajectory planner 560.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0053] The HID orientation error estimator 530 may be configured determine a HID orientation error based on a difference between the current HID orientation, which may be based on user movement of the HID, and a target HID orientation that may correspond to the last recorded orientation before the system transitioned from the tool control mode. The HID orientation error may indicate a deviation or estimate of how different the actual HID orientation is from the target HID orientation. In one embodiment, the HID orientation error may represent a rotation required to transform a current orientation of the gimbal 205 of the HID 140 into a target orientation. In one embodiment, this error may be based on determined poses of each device, where the error may be based on a difference between each rotational matrix. By keeping alignment during HID manipulation enables the system to smoothly transition back into the tool control mode, as described herein.

[0054] For the HID 140 to rotate about one or more axes in order to maintain alignment may require a considerable amount of movement by one or more joints of the HID. Computing the joint movements of the HID based on a large HID orientation error (e.g., above a threshold) may cause jerking motions in the HID as the one or more joints of the gimbal moves into a target orientation. Therefore, the HID orientation error estimator 530 may be configured to perform orientation error handling operations to adjust the orientation error into smaller or larger motions for moving the HID towards its ultimate target orientation. To do this, the estimator 530 may be configured to determine an adjusted target orientation by performing one or more operations, whereby the IK algorithm may iteratively compute motions for the HID to converge its orientation with the target orientation. By iteratively computing smaller motions or larger motions, the system 100 may be configured to enable smooth motion between the current (original) pose to the target orientation.

[0055] The estimator 530 may adjust an orientation error based on one or more limits of the HID 140. For instance, the estimator 530 may determine the adjustment by taking into account at least one of a velocity limit and / or a position limit of the HID. In one embodiment, the limits may be predefined limits based on the physical configuration of the HID. To determine how much to adjust the orientation error, the estimator 530 may be configured to perform one or more optimization operations to find an orientation error that satisfies an objective function to maximize movement ofAttorney Docket No.: AUR6374WOPCT4 Electronically Filed the (e.g., gimbal 205 of the) HID, while not exceeding the HID limits (e.g., staying equal to or below the limits). These operations may include a neighborhood line search in which a pose of the end user control 220 may be determined in which the objective function converges to a local minimum.

[0056] In another embodiment, the estimator 530 may use other methods to determine an adjustment to the orientation error, such as gain scheduling and error clamping that may be based on the determined orientation error. For instance, the estimator 530 may be configured to apply one or more gains (e.g., a non-linear scaling factor) to the orientation error based on the magnitude (size) of the orientation error. For example, when the orientation error is greater than a first threshold, the estimator 530 may apply a first scaling gain to reduce the orientation error. Conversely, when the orientation error is less than the threshold, the estimator 530 may apply a second scaling gain to increase the orientation error. For instance, when the orientation error is small, the estimator 530 may apply a second gain, which may be greater than the first gain, to be closer to the small orientation error. Scaling the error may enable the system to reduce the overall convergence time when the orientation error is small (e.g., below a threshold). In addition, scaling the error may ensure that motion remains smooth by not causing the system to move the HID over great distances. As a result, the system 100 may provide a high success rate for converging the orientation error (e.g., minimizing the error) with very low convergence threshold that provides a precise HID motion. In another embodiment, the estimator 530 may apply error clamping, whereby the estimator may define one or more orientations between maximum (or minimum) amounts of orientation within the orientation error. As a result, the system 100 may improve the IK’s success rate by reducing the area of the HID movement for deriving corresponding joint movements, as described herein.

[0057] The estimator 530 may be configured to determine one or more gains to be applied to the orientation based on the velocity at which the HID is moving from a previous pose to its current pose. For instance, the estimator may be configured to determine a velocity at which the HID has moved between a previous orientation to its current orientation, and may be configured to determine the scaling gain based on the velocity. As described herein, while in an operational mode in which the HID moves independently from the surgical tool 171, such as clutch mode or camera mode, theAttorney Docket No.: AUR6374WOPCT4 Electronically Filed user may translate the gimbal 205 of the HID within 3D space. During translation, the estimator 530 may be configured to estimate a velocity at which the user is moving the gimbal. The faster the HID is moving, the higher the gain may be applied in order for the movement of the HID to better continue matching the target orientation of the HID. As a result, the estimator 530 may determine one or more gains and may apply the one or more gains to the orientation error.

[0058] As described herein, the IK 520 may be configured to perform an optimization process to perform a primary task of determining one or more primary task joint motions for adjusting a pose of the (e.g., end user control 220 of the) HID 140 to match a target pose of the surgical tool and / or to perform nullspace optimization in which the orientation of the HID matches a previously determined orientation without causing movement of the end user control (or without changing the orientation of the end user control) with respect to a global reference frame. In one embodiment, these operations may be performed in order to ensure that the orientation of the gimbal of one or more HIDs remains aligned with its last recorded orientation before existing tool control mode.

[0059] The IK 520 may be configured to perform an optimization process, such as damped least squares, to determine joint motions for maintaining orientation alignment with the gimbal 205. The IK 520 may set a joint vector of the gimbal as: ^^ ൌ ^^^ௗ ,^^் ^, ^^^, ^^^൧

[0060] where qd is theqe is the position of joint 215e, qf is a position of joint 215f, and qgis a position of joint 215g. To solve the primary task, such as keeping the end user control 220 aligned with the surgical tool, the following may be minimized ‖^^∆^^ െ ^^‖ଶ ^ ^^^‖∆^^‖ଶ

[0061] where ksis a small constant, J is the Jacobian matrix relating joint velocities to end-effector (e.g., end user control) velocities, Δq is the vector of joint velocities. This equation may be rewritten asAttorney Docket No.: AUR6374WOPCT4 Electronically Filed ∆^^ ൌ ^^்^^^^^் ^ ^^ ି^^^^ ^^^

[0062] where e may be the gimbal orientation error between the gimbal current orientation and the target orientation, I is the identity matrix, and JTis the transpose of the Jacobian matrix. From this the system may directly tell the inverse part is non- singular, and secondly, a proper ks may reduce the joint velocity output, Δq, when the configuration is close to a singularity. This may allow the IK to determine the joint motions to complete the primary task. The IK 520 may perform a damped least squares to find a local minim solution of joint motions (e.g., joint velocities) for one or more joints of the HID for keeping the end user control 220 in a same orientation as the surgical tool, while the user moves the HID. Written another way, this primary task joint motion may be a function of the Jacobian of the robotic arm (including the tool), as ^^^^^^^^^^^^^^^^^ ൌ ^^ା^^

[0063] where J+is the pseudoinverse matrix of the Jacobian, which may be written as ^^ା ൌ ^^^^^^^் ^ ^^ ି^^^^ ^0 , ^^ ^ ^^^^^^ ଶ

[0064] where ^^

[0065] In addition to or in lieu of performing a primary task, the IK 520 may be configured to perform one or more secondary tasks by performing nullspace optimization to cause movement of one or more joints (e.g., of the gimbal 205), as described herein. In one embodiment, to perform secondary tasks, a pseudoinverse method may be used, where a nullspace of J has the property of: ^^^^^ െ ^^ା^^^ ൌ 0Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0066] where ^^^^^ െ ^^ା^^^ is a nullspace projection that maps joint motion (e.g., avector in joint space), to joint motion of one or more joints of the HID, and ^^^^ includesone or more secondary tasks that is to be achieved for the nullspace motion. As a result, Nullspace motion may be defined based on the Jacobian from the primary task to ensure that the motion in nullspace does not interfere with the primary task. Nullspace motion may include ^^^ ^^^^^^^^^^^^^^^^^^ ൌ ^^^ െ ^^ା^^^^^0^

[0067] As a result, Δq may be minimized to ∆^^ ൌ ^^^^^^^^^^^^^^^^^ ^ ^^^^^^^^^^^^^^^^^^^^^ ൌ ^^ା^^ ^ ^^^^^^ െ ^^ା^^^^^^^

[0068] where kris a real scaler, which can be written as either positive ornegative, and ^^^^ may include desired (or target) nullspace joint motion for achievingone or more secondary tasks. This target nullspace joint motion may include one or more secondary goals (or tasks) which may include a cost function for hand comfort and ergonomics, joint limit avoidance, object avoidance, and / or manipulability of the HID. The target nullspace joint motion may be proportional to the negative gradient of these cost functions such that ^^^^^^^^ ^^^^ ൌ^^^^

[0069] where w(q) may include one or more cost functions, such that ^^^^^^ ൌ ^^^^^^^^^^ ^ ^^ଶ^^ଶ^^^^ ^ ⋯

[0070] where k1and k2are tunable coefficients (e.g., weights) and are positivevalues. As a result, ^^^^ may move along a gradient of w(q) as well as the direction ofmaximizing the w(q). Since the gimbal 205 has four DoF, the system has a redundant DoF which is provided by joint 215d. This redundant joint allows the IK 520 to produce multiple solutions that maintain the same (or do not interfere with) end user control 220 orientation. For example, referring to Fig.2, the end user control 220 is shown as having an orientation directed in the Y-direction. Since the HID 140 includes the redundant joint, the HID may be configured in multiple joint configurations, whileAttorney Docket No.: AUR6374WOPCT4 Electronically Filed still maintaining the orientation shown in this figure. Some configurations, however, may be less desirable than others. For instance, some configurations may strain the user’s hand, or may result in one or more components (e.g., links) of the HID colliding with the user’s hand. Therefore, the system 100 may perform a secondary task that includes a cost function for maximizing hand comfort and ergonomics. In order to achieve this task, the system may use one or more (e.g., secondary task) joint motion constraints for achieving a desired joint configuration of at least some of the joints of the HID. In one embodiment, these motion constraints may be selected based on the current (or target) joint positions. For instance, the IK may be configured to perform a table lookup into a data structure that associates motion constraints that are to be applied in the nullspace with the current joint position. The motion constraints may include joint limit motion constraints, as described herein. For example, the constraints may indicate that the redundant joint is to remain stationary as the HID changes orientations, the joint 215f angle may be at a rotational joint position (e.g., 90°), and / or the joint 215e may be less than the rotational joint position of joint 215f. In which case, the secondary task may be defined as a cost function with respect to joint positions while satisfying one or more constraints in order to maximize user comfort and / or ergonomics. For example, a cost function for user comfort and ergonomics may include ^ ^ ^^^^^^^ ൌ െ1 ^^^ െ ^^^ଶ →^^^^^^^ ௧ ^^^^ൌ ^^^௧ െ ^^^

[0071] positions. With the cost function, the IK may apply motion constraints such that gradient solutions of the gimbal joints from the optimization process may be constrained to result in a desired joint configuration, such that ∇^^^^ ൌ ^^ௗ^^^^^^^^^^ௗ ^ ^^, 2^^^ െ ^^^ì െ ^^ / 2^ ^^^ ^^^Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0072] Thus, as shown above, the first gradient of the cost function, ^^^^^^is constrained such that joint 215d is 0°, ∇^^^ଶis constrained such that joint 215e may be within a range of -90° to 90°, and ∇^^^ଷis constrained such that joint 215f may be 90° for the gimbal of the left HID 140A and a corresponding joint may be -90° for the gimbal of the right HID 140B.

[0073] The IK 520 may also include a cost function for joint limit avoidance in order to ensure that joints of the gimbal do not move within a threshold of their limits, which may result in the gimbal reaching a singularity. For example, the IK may optimize the following cost function to prevent the joints of the gimbal from reaching their limits by attempting to shift the joint positions towards the middle of their joint limits. The cost function includes: ସ ^^ଶ^^^^ ൌ െ1൫^^^,^^௫ െ ^^ଶ ^,^^^൯^

[0074] ^^ଶ ^^^^ଶ^^^^൫^ ^,^^௫ െ ^^,^^^൯ ൫2^^^ െ ^^ൌ െ ^,^^௫ െ ^^^,^^^൯^^^^

[0075] joint velocities to negative around a joint’s maximum limit and to increase the joint velocity to positive around a minim limit so that the gimbal joint has a trend to move away from its joint limits. Moreover, when the joints are far away from their limits (e.g., beyond a threshold), this gradient is approximately zero.

[0076] As described herein, nullspace joint motion may be based on (or include) secondary task joint motion constraints of one or more joints for achieving a desired joint configuration. In particular, the IK 520 may be configured to optimize a secondary task objective function according to the secondary task joint motion constraints for achieving a desired joint configuration. Such motion constraints may be configured to maximize a distance between one or more objects and one or more joints of the HID. In another embodiment, the objective function may be based on user ergonomics, whereby the function may be optimized in order to provide a jointAttorney Docket No.: AUR6374WOPCT4 Electronically Filed configuration that maximizes user comfort. The optimized nullspace secondary task joint motions may be projected into the nullspace using a nullspace projection in order to produce the nullspace motion

[0077] The secondary task joint motion constraint generator 540 may be configured to determine the secondary task joint motion constraints (or motion constraints) which may be used by the IK 520 to achieve nullspace joint motion to cause one or more joints of the HID to reach a desired joint configuration. The secondary tasks may include an objective function for optimizing object collision avoidance and ensuring ergonomics for enhancing (improving ) user comfort while a user manipulates the HID 140 by satisfying one or more motion constraints. The secondary task joint motion constraints may be based on joint motion limits, such as joint position limits and / or joint velocity limits, which may be used to achieve a desired joint configuration. The constraints may be based on the type of HID. As described herein, the user console 120 may include a left HID arranged to be manipulated by a user’s left hand and a right HID arranged to be manipulated by a user’s right hand. In which case, the generator 540 may determine a first set of motion constraints for the left HID and a second set of motion constraints for the right HID. The motion constraints may be inverses of each other. For example, a motion constraint of the left HID may be to limit a joint’s motion to a limit threshold (e.g., 90°), while another motion constraint of the right HID may be an inverse of that limit threshold, such as - 90°. In one embodiment, the motion constraints may be predefined constraints determined in a control setting (e.g., laboratory). In which case, the motion constraints may be stored in memory (e.g., of the controller 500). The generator may determine the constraints by retrieving them from the memory.

[0078] In one embodiment, the secondary task joint motion constraint generator 540 may be configured to determine one or more motion constraints based on one or more conditions. For example, motion constraints may be based on the (current) joint positions of one or more joints of the HID at which the joint motion optimization is being performed. In which case, the generator 540 may be configured to determine the joint position of one or more joints and determine corresponding motion constraints based on those positions. For example, the motion constraints may be stored in memory in a data structure, where motion constraints may be associated with joint position. As aAttorney Docket No.: AUR6374WOPCT4 Electronically Filed result, the motion constraint generator 540 may be configured to perform a table lookup into the data structure to select motion constraints of one or more joints based on their positions.

[0079] The motion constraints may be predefined. For instance, the motion constraints may be derived in a laboratory, while the HID is manipulated by a user. In which case, the data may be derived based on user observations. In another embodiment, the motion constraints may be derived by a hand posture model that may be configured to determine an estimate of a user’s hand position with respect to a joint configuration based on a target HID orientation. In particular, the model may be a machine learning (ML) model that was trained using joint configuration data associated with user hand positions while holding the end user control 220 as training data. As a result, the hand posture model may be configured to output motion constraints based on target joint positions to achieve a desired joint configuration through nullspace motion. In one embodiment, the generator 540 may be configured to perform the hand posture model (e.g., in real time) so as to be configured to determine and / or adapt secondary task joint motion constraints during system use.

[0080] As a result, the secondary task joint motion generator 540 may be configured to produce motion constraints based on undesirable joint configurations, such that the IK 520 may produce nullspace joint motion in order to use the constraints to produce more desirable joint configurations during optimization of these secondary tasks. For instance, joint configurations may be associated with motion constraints of one or more joints, whereby the IK 520 may be configured to assign a particular joint configuration as a nullspace command by selecting one or more corresponding motion constraints that affect such a configuration of the joints.

[0081] The nullspace optimization may include other cost functions. For example, the nullspace optimization may minimize one or more joint’s energy in the nullspace. For instance, the optimization may include a secondary task to take into account the joint energy used to complete other secondary tasks, and may ensure that the joint’s energy in the nullspace is reduced to minimum. As another example, the secondary task may include a cost function to optimize manipulability of the HID. For instance, this secondary task may determine whether the HID if it is moving into aAttorney Docket No.: AUR6374WOPCT4 Electronically Filed singularity in which motion in one or more directions is being lost. For instance, as the gimbal 205 is moving, the joint positions of one or more joints may move closer to their joint limits. As a result, the manipulability function may determine whether a joint is coming close to a joint limit or losing joint motion, and may be configured to adjust joint velocity of one or more other joints in order to move away from the singularity. In another embodiment, the IK may perform object avoidance, whereby the system may include a cost function that takes into account the distance between one or more joints and one or more objects, and may optimize the cost function to maximize the distance. In one embodiment, the objects may include components of the HID, such as a link or joint. In another embodiment, the objects may be other components of the system, such as a robotic arm 110. In another embodiment, the object may be an unknown object. In which case, the system may use sensor data (e.g., a camera) to determine the distance between the object and the HID, and may define joint motion in order to keep the distance at a maximum.

[0082] The IK 520 may use an optimization method, such as damped least squares, to derive joint motion that minimizes orientation error for a primary task and optimizes nullspace joint motion for one or more secondary tasks. Although described as capable of deriving both solutions, the IK may be configured to only perform nullspace optimization. In which case, the IK may define the primary task solution as zero, indicating that joint motion commands due to the performance of the IK optimization may result in performing secondary tasks by reconfiguring the orientation of the gimbal, while the joint motion commands may not include joint motion for optimizing a primary task, such as converging a pose of the end user control 220 with that of the surgical tool.

[0083] As described thus far, the IK 520 may derive an optimized nullspace solution. This solution may be derived using gradient descent in which a solution that minimizes the orientation error is derived from one or more local minimum solutions. In some instances, the IK may perform this optimization multiple times in order to effectively derive the global minimum as the most optimal solution (e.g., where the orientation error is effectively zero). In some cases, however, the IK 520 may be unable to perform multiple iterations due to time constraints, such as when motion is being generated in real time, e.g., as the user is moving the HID 140. Therefore, the IK mayAttorney Docket No.: AUR6374WOPCT4 Electronically Filed perform one or more additional conditional operations in order to have the solution better converge the orientation error. For example, the IK 520 may be configured to determine one or more joint-level gains to be applied to nullspace joint motion (e.g., joint velocities) solution for the one or more joints based on a difference between the local minimum solution and the target HID orientation. For instance, if the resulting orientation error is greater than a threshold, the IK may apply one or more gains to joint velocities, whereas if the resulting orientation error is less than the threshold, the IK may apply gains that are greater than the one or more gains. In other words, to help the resulting HID orientation to converge, the IK may apply proportional gains to one or more joint motions.

[0084] As described thus far, the system 100 may produce target nullspace joint motion for one or more joints of the gimbal in order to converge the gimbal orientation with a target orientation, while at the same time modify joint configuration to achieve one or more secondary tasks. These target nullspace joint motions, however, may not take into account joint motion limits, such as joint potion limits, joint velocity limits, and / or joint acceleration limits of one or more joints of the HID. As a result, the trajectory planner 560 may determine, for each joint, a unified constraint of the joint based on a current position of the joint. In particular, the planner 560 may compute a unified constraint at each joint position to define the upper and lower velocity bounds for the joint position, where the unified constraint can account for limits, such as position, velocity, acceleration and / or jerk together with respect to each other. The unified constraint may indicate feasible joint motion that satisfies one or more limits ofthe joint. The unified constraint as a range or boundary, ^^^min,i to ^^^ max,i, where “I”represents a joint of the robotic arm, such as where i=1, may correspond to a firstactuator arranged to drive joint 302a of the robotic arm 110. Thus, both ^^^ min,i and ^^^max,iindicate one or more velocities based on one or more limits. For example, both may indicate three velocities, a first velocity limit with respect to joint position, a second velocity limit with respect to joint velocity, and a third velocity limit with respect tojoint acceleration, where ^^^ min,i identifies the maximum negative velocity, and ^^^max,iidentifies the minimum positive velocity, thereby indicating a feasible velocity range for a corresponding joint. As a result, all of the limits may be combined in this unifiedAttorney Docket No.: AUR6374WOPCT4 Electronically Filed constraint range so that planner 560 may apply a single “limit” per joint to determine a feasible joint motion command (e.g., a feasible joint velocity).

[0085] The trajectory planner 560 may be configured to determine joint motion based on the target nullspace joint motion for moving one or more joints, where the joint movements is to satisfy feasible joint motion associated with a position (e.g., the position at which the joint movement is to occur) while the unified constraint of the joint is maintained. In particular, the trajectory planner 560 may be configured to determine joint motion for each joint according to respective unified constraints based on the target nullspace joint motion (e.g., target joint velocity) derived by the nullspace optimization process performed by the IK 520. These resulting joint motions may include joint velocities, for moving a joint, and / or may indicate joint positions of one or more joints with respect to time. Thus, the planner 560 may be configured to determine feasible joint motion (e.g., feasible velocity) of each joint at each joint position that satisfies the unified constraint, and may be configured to scale a target nullspace joint motion for that joint. In one embodiment, the planner may be configured to maximize the possible joint motion by solving an optimization problem that may be simultaneously constrained by the unified constraints. The problem may include: maximize ^^ subject to^^^^^^,^^^^^^ ^ ^^^^^ ^ ^^^^^௫,^^^^^^^ ^^ ^

[0086] The trajectorysolve the optimization problem, such as a Quadratic Programming (QP) optimization problem, by determininga scaling factor, ^^, to scale ^^^ representing the target nullspace joint motion of the jointsuch that the target nullspace joint motion is within the lower bound and the upper bound of the unified constraint. The trajectory planner 560 may perform this determination for each joint, thereby identifying a scaling factor that ensures that target joint motion satisfies the joints respective unified constraint. The trajectory planner may then identify a maximum scaling factor such that when applied to a joint motion will satisfy the corresponding joint’s unified constraint. In other words, the planner may maximize the scaling factor, according to the bounds of at least some unified constraints at a time, tk, to estimate each joint’s motion for its given position. Thus, theAttorney Docket No.: AUR6374WOPCT4 Electronically Filed planner 560 may determine the feasible joint motion of all joints, determine respective scaling factors based on the joint motion and feasible joint motion, and then apply a scaling factor ensures the nullspace joint motion satisfies the unified constraints (e.g., of all moving joints) in order to provide synchronized motion across the joints. As a result, the system 100 may be configured to produce a smooth synchronized motion across the joints by applying gain scheduling and by bounding joint limits to ensure that joint motion maintains or satisfies unified constraints.

[0087] Figs.6A, 6B, and 7 are flowcharts of processes 600 and 700, respectively, for performing state-based HID IK for determining smooth motion orientation for at least one HID. At least some of the operations of these processes may be performed by the controller 500 of the system 100. In another embodiment, at least some of the operations may be performed by another electronic device, which may communicate with the controller 500. Some of these processes may be performed in “real-time”, during a surgical procedure in which an operator is controlling a robotic arm to perform one or more surgical tasks.

[0088] Turning to Fig.6A, this figure illustrates a flowchart of one embodiment of a process for performing state-based IK for smooth motion orientation control of at least one HID. The process 600 begins determining a pose (e.g., a position and orientation) of the HID ( at block 604). In particular, the system may determine the orientation and / or position of the gimbal 205 of the HID, which may be based on position data from one or more sensors of the HID. For instance, the orientation may be the joint position of joint 215d with respect to a global reference frame. In another embodiment, the orientation may include one or more joint positions of at least one of the joints of the gimbal 205, with respect to a reference frame. In one embodiment, the pose of the HID may be determined as a solution to an optimization problem that minimizes a primary task of minimizing the error between the surgical tool 171 and the HID 140, and optimizes one or more secondary tasks, such as object avoidance and user comfort. The system determines whether the system is in a tool control mode (at decision block 606). For instance, the system 100 may determine the state of the surgical system based on user input, such as through the HID 140 and / or a foot pedal 130. If so, the system 100 may control the surgical tool based on the pose of the HID (at block 608). For instance, the system 100 may perform a constraint-aware IKAttorney Docket No.: AUR6374WOPCT4 Electronically Filed trajectory planning algorithm to identify a trajectory along which the surgical tool may move based on a pose error of a difference between the pose of the HID and a pose of the surgical tool. In particular, the trajectory planner 560 may be configured to determine a trajectory that includes joint movements, where each joint movement may satisfy a unified constraint of the joint that enables for synchronous smooth motion of the joints of the surgical tool.

[0089] If, however, the system is not in the tool control mode, the system enables gravity mode for one or more joints of the HID in order to allow translational movement (at block 609). For instance, the system may cause actuators of one or more joints to apply a gravity compensation torque in order to compensate for the force of gravity, and allow for movement of the joints based on an applied external force, such as the user pulling or pushing on the HID. For instance, the system may determine a gravity-compensation torque based on joint positions of the one or more joints, and may apply the gravity compensation torque using a respective actuator of the joint. As described herein, these joints may include those of the support arm 200, such as joints 215a, 215b, and 215c. In one embodiment, once the system determines that the system is not in the control mode, the system may prevent spatial control signals from other joints, such as those of the gimbal 205 to manipulate another robotic component, such as the robotic arm 110. The system, however, may continue to receive sensor data from these joints, such as position data, and may continue to provide joint commands, as described herein. In one embodiment, this operation may be optional (illustrated as the block having a dashed boarder), if the joints are already in gravity mode.

[0090] In one embodiment, the pose of the HID determined in block 604 may a last determined pose of the HID before the system has switched out of the control mode. In another embodiment, the pose may be determined responsive to determining that the switch has occurred. As described herein, to enter clutch mode, the user may provide user input through one or more controls. In which case, upon receiving a control signal indicating that user input is being received, the system may measure the pose of the HID, and may use this pose as a target pose (or target orientation), as described herein.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0091] The system 100 determines a new pose (e.g., a new position and a new orientation) of the HID based on user input (at block 610). For instance, the user may have performed one or more translational movements of the HID, and in response, the system may determine a new pose of the HID based on updated position data from one or more sensors of the HID. In particular, the new orientation may include a new orientation of the gimbal 205, which may be due to the user moving the HID. The system 100 determines whether the system is in a clutch mode (at decision block 612). In one embodiment, this operation may be performed before the determination of the new pose. In particular, the system is determining whether user input has been received (and / or is continuing to be received) such that the system is in a clutch mode in which the HID is to move responsive to an applied external force independently of other surgical components, thereby allowing the user to move the HID without affecting the surgical tool 171 and the camera 471, or the system is in a camera mode in which translational movement of the HID within 3D space affects corresponding translational movement of the surgical camera 471, while the surgical tool 171 remains stationary. Thus, if the system is in clutch mode, the system computes an orientation error based on a difference between the new orientation of the HID and a previous orientation of the HID (at block 616). In one embodiment the orientation error may include one or more rotational transforms of one or more joints of the gimbal 205. Otherwise, if the system is in camera mode, the system controls the camera 471 based on translational movement of the HID within 3D space according to the new pose of the HID (at block 614). The system 100 then proceeds to determine the orientation error.

[0092] In one embodiment, if the orientation error is below or equal to a tolerance, such as zero, the system may return back to block 610 to determine whether the HID has a new pose, which may be due to user input. An orientation error below the tolerance may indicate that the orientation of the gimbal 205 may not have changed (significantly) from the HID’s previous position to its new position, due to the translation movement. In another embodiment, even if the orientation error is below the tolerance, the system 100 may proceed with the process 600 in order to optimize one or more secondary tasks to provide nullspace joint motion, as described herein.

[0093] The system 100 determines one or more gains for adjusting the orientation error (at block 618). For instance, the system may scale the orientation errorAttorney Docket No.: AUR6374WOPCT4 Electronically Filed based on a velocity at which the HID has moved. In this case, the system may determine a velocity at which the HID has moved from its previous orientation to its new orientation, which may be based on received position data, and may determine the scaling factor based on the velocity. As described herein, this movement may be the result of the user moving one or both HIDs in 3D space to control the camera 471, or may be due to the user moving the HID to a different configuration, based on which mode the system is operating. For example, if the velocity is above a threshold, the system may determine a first gain, whereas, if the velocity is below the threshold, the system may determine a different gain that is less than the first gain. As a result, the applied gain may be proportional to the velocity at which the user is moving the HID. In another embodiment, the system may scale the orientation error based on a magnitude of the orientation error, as described herein. The system 100 adjusts the orientation error by applying the one or more gains to the orientation error (at block 620). As a result, the orientation error, which may be in Cartesian space (or tool space), as opposed to joint space, may be scaled based on one or more criteria.

[0094] Turning to Fig.6B, the system 100 may be configured to perform an optimization process (e.g., damped least squares) to solve for joint motion to ensure that the orientation of the HID maintains alignment with its orientation from the last time the system was in tool control mode. This process may be an iterative process performed by the IK 520 that performs gradient descent to identify a joint motion solution as one or more local minimums of primary and / or secondary tasks. The system 100 determines an initial guess of joint positions based on the orientation error of the HID (at block 622). In one embodiment, this initial guess may be a random guess of one or more joint positions of one or more joints of the HID. This initial guess may be derived based on an optimization function that may minimize a cost function based on one or more terms, which may include an energy cost based on the energy necessary for moving the HID and / or a movement cost required for moving the HID from one pose to another. As a result, the initial guess may be the “best” guess of joint positions that minimizes the overall cost function. The system 100 (optionally) determines a joint motion for satisfying the primary task of keeping the HID orientated with the surgical tool (at block 624). In particular, the system may determine ^^^^^^^^^^^^^^^^^ as joint motionsAttorney Docket No.: AUR6374WOPCT4 Electronically Filed (e.g., joint velocities) for one or more joints of the HID for keeping the end user control 220 in the same pose as the surgical tool, while the user is moving the HID.

[0095] In one embodiment, joint motions defined by the system 100 may be optimized to provide good user comfort and to avoid hand collisions. These objectives are secondary tasks, which are for producing nullspace joint motions that do not impact the primary task. In this case, the joint motion for the primary task may be set to zero, since adjusting the orientation of the gimbal may not affect the pose of the end user control 220. Therefore, defining e as zero and ^^^^^^^^^^^^^^^^^ ൌ 0

[0096] the resulting joint motion produced by the IK 520 may not affect the pose of the end user control 220. Thus, the system may determine each of one or more joint motions for the joints of the HID to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity, since the surgical tool may remain still as the user operates in one of the other modes. The system determines this nullspace joint motion for satisfying one or more secondary tasks, Stask(at decision block 626). As described herein, this motion may be for one or more joints of the gimbal 205, which may be based on the initial guess of joint positions and / or based on secondary task joint motion constraints. In one embodiment, Stask may include or be based on one or more secondary tasks, as described herein. In one embodiment, the system may produce one or more nullspace motions that indicate one or more desired joint configurations. As an example, for user comfort and ergonomics, the system may determine nullspace joint motion of one or more joints of the gimbal that satisfies at least one of the secondary task joint motion constraint, as described herein. For example, the system 100 may perform an optimization function to determine multiple solutions for nullspace motion, which may be based on the fact that the HID includes one or more redundant joints. The system may select a solution that satisfies the one or more secondary task joint motion constraints in order to derive a desired joint configuration. For example, the nullspace joint motions may perform secondary tasks that include adjusting the redundant joint, joint 215d, for gimbal realignment and adjusting one or more other joints of the gimbal for a secondary task, such as object avoidance that may be subject to one or more secondary task motion constraints inAttorney Docket No.: AUR6374WOPCT4 Electronically Filed order to achieve a desired joint configuration for user comfort. From these solutions, the system 100 may select the solution that satisfies at least one of the motion constraints.

[0097] The system 100 determines the resulting joint motion based on a combination of the joint motion and the nullspace motion (at block 628). In particular, with the primary task joint motion set to zero, the resulting joint motion may be solely based on a combination of all nullspace joint motion. As a result, the system determines joint motions for the nullspace joint motions using a damped least squares algorithm to minimize the orientation error starting at the initial guess of joint positions.

[0098] The system determines a new orientation of the HID based on the resulting joint motion (at block 630). In particular, the system 100 may determine a new orientation of the gimbal 205 based on nullspace joint motion of one or more joints of the gimbal 205. The system determines one or more joint-level gains based on a difference between the new HID orientation and the previous HID orientation and applies the gains to determine target joint motions (at block 632). As described herein, the IK may perform gradient descent to calculate the gradient of one or more nullspace cost functions, and use the gradient to update joint motion in order to determine a local minimum of orientation error. In which case, the solution may not be the global minimum. Thus, the system may be configured to determine one or more joint velocities based on the resulting orientation error. For example, each joint velocity may be a function of the orientation error, where the greater the error, the higher the velocity in order to ensure that the HID converges within a reasonable time period.

[0099] At this stage, the joint motion may be adjusted based on the resulting orientation error. This joint motion, however, may not take into account joint limits, such as velocity limits and acceleration limits. As a result, the system may determine, for each joint, a unified constraint of the joint based on a current position of the joint (at block 634). In particular, the IK may compute a unified constraint at each joint position to define the upper and lower velocity bounds for the joint position, where the unified constraint can account for limits, such as position, velocity, acceleration and / or jerk together with respect to each other, as described herein.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0100] The system determines joint motion based on the target joint motion for moving the joints from their current positions, while satisfying the unified constraint (at block 636). As described herein, the trajectory planner 560 may identify a scaling factor that ensures that the target joint motion satisfies the joint’s respective unified constraint, and may identify a maximum scaling factor such that when applied to each joint motion satisfies all joint constraints. The planner 560 may produce resulting nullspace joint motions by adjusting the target nullspace joint motion to satisfy the unified constraints by applying the scaling factor. The system 100 outputs joints commands based on the joint motion to cause the HID to adjust its orientation (at block 638). Thus, the nullspace joint motion are to cause the gimbal 205 of the HID to maintain alignment (or realign with) the orientation of the gimbal from before the system entered this operational mode, without causing movement of the end user control with respect to a global reference frame. These joint motions may cause the gimbal to achieve a desired orientation, which may be responsive to translational movement of the HID. In one embodiment, the IK 520 may be configured to perform one or more pose tracking operations in order to produce primary task joint motion in addition to the nullspace joint motions in order to maintain alignment of the gimbal 205 and maintain pose alignment of the end user control 220 with the tool 171. The system 100 may then return to block 640 of Fig.6A to update the pose (e.g., update the orientation) of the HID based on the joint commands (at block 640). If the operational mode has not changed, the system may return to block 616 to compute the resulting orientation error due to the updated pose of the HID. From this resulting error, the system may repeat at least some of the operations of process 600 in order to perform additional nullspace motion, as needed. If, however, after the pose of the HID is updated at block 640, the user continues to move the HID, the system may proceed to block 610 to determine a new pose based on that movement, and may repeat at least some of the operations, as needed for more nullspace motion.

[0101] In one embodiment, the system 100 may perform at least some of the operations of process 600 in real-time and while the user moves the HID while in one of the operational modes described herein. As a result, the system may continuously adjust the orientation of the gimbal, as the user moves the HID in 3D space in order to optimize user comfort and avoid object collisions, for example.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed

[0102] Fig.7 is a flowchart of another embodiment of a process 700 for performing state-based IK for smooth motion orientation control of at least one HID. The system 100 determines, while the surgical robotic system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a HID, an orientation of the HID that includes several joints (at block 710). The system 100 determines that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space (at block 720). The system, responsive user movement of the HID, determines a new orientation of the HID (at block 730). The system determines, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation (at block 740). The system provides the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID (at block 750).

[0103] Some aspects may perform variations to the processes 600 and / or 700 described herein. For example, the specific operations of at least some of the processes might not be performed in the exact order shown and described. The specific operations might not be performed in one continuous series of operations, and different specific operations might be performed in different embodiments.

[0104] As previously explained, an embodiment of the disclosure may be a non-transitory machine-readable medium (such as microelectronic memory) having stored thereon instructions, which program one or more data processing components (generically referred to here as a “processor”) to (automatically) perform state-based IK for input device orientation control operations, as described herein. In other embodiments, some of these operations might be performed by specific hardware components that contain hardwired logic. Those operations might alternatively be performed by any combination of programmed data processing components and fixed hardwired circuit components.

[0105] To aid the Patent Office and any readers of any patent issued on this application in interpreting the claims appended hereto, applicants wish to note that theyAttorney Docket No.: AUR6374WOPCT4 Electronically Filed do not intend any of the appended claims or claim elements to invoke 35 U.S.C.112(f) unless the words “means for” or “step for” are explicitly used in the particular claim.

[0106] While certain embodiments have been described and shown in the accompanying drawings, it is to be understood that such embodiments are merely illustrative of and not restrictive on the broad disclosure, and that the disclosure is not limited to the specific constructions and arrangements shown and described, since various other modifications may occur to those of ordinary skill in the art. The description is thus to be regarded as illustrative instead of limiting.

[0107] In some embodiments, this disclosure may include the language, for example, “at least one of [element A] and [element B].” This language may refer to one or more of the elements. For example, “at least one of A and B” may refer to “A,” “B,” or “A and B.” Specifically, “at least one of A and B” may refer to “at least one of A and at least one of B,” or “at least of either A or B.” In some embodiments, this disclosure may include the language, for example, “[element A], [element B], and / or [element C].” This language may refer to either of the elements or any combination thereof. For instance, “A, B, and / or C” may refer to “A,” “B,” “C,” “A and B,” “A and C,” “B and C,” or “A, B, and C.”

Claims

Attorney Docket No.: AUR6374WOPCT4 Electronically Filed CLAIMS What is claimed is:

1. A method performed by a surgical robotic system, the method comprising: determining, while the surgical robotic system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a human interface device (HID), an orientation of the HID that includes a plurality of joints; determining that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determining a new orientation of the HID; determining, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and providing the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.

2. The method of claim 1, wherein the HID comprises a gimbal that includes the plurality of joints and an end user control arranged to be held by a user, wherein the orientation of the HID comprises an orientation of the gimbal within three-dimensional (3D) space, wherein the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame.

3. The method of claim 2, wherein the plurality of joints comprises four joints, three joints that are each arranged to rotate about a different axes with respect to the others and a redundant joint with respect to the three joints, wherein the method further comprises generating the nullspace joint motions to perform a plurality of secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or more joint motion constraints.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed 4. The method of claim 3 further comprising determining each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

5. The method of claim 1, wherein the second operational mode comprises at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three- dimensional (3D) space affects corresponding translational movement of the surgical camera.

6. The method of claim 5, wherein the plurality of joints comprises a first plurality of joints and the HID comprises a second plurality of joints, wherein each joint comprises a respective actuator, wherein the method further comprises, while in the camera mode, determining, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second plurality of joints, wherein the one or more joint commands are to cause at least one actuator of the first plurality of joints to reorientate at least a portion of the HID responsive to the translational movement.

7. The method of claim 6 further comprising, while in the camera mode and for each joint of the second plurality of joints, determining a gravity-compensation torque based on a joint position of the joint; and applying the gravity-compensation torque using an actuator of the joint.

8. The method of claim 1 further comprising, for each joint of the one or more joints, determining a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID;Attorney Docket No.: AUR6374WOPCT4 Electronically Filed determining a unified constraint based on a current joint position, wherein the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and producing a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint.

9. The method of claim 8 further comprising determining a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, wherein adjusting comprises, for each joint, applying the scaling factor to a respective target nullspace joint motion.

10. The method of claim 1, wherein determining the one or more nullspace joint commands comprises: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold.

11. The method of claim 10, wherein determining the one or more joint commands further comprises: determining a scaling factor based on a magnitude of the orientation error; scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions.

12. The method of claim 11 further comprising determining a velocity at which the HID has moved from the orientation to the new orientation, wherein the scaling factor is further based on the velocity.

13. The method of claim 11 further comprising: determining a resulting orientation of the HID based on the determined nullspace joint motions;Attorney Docket No.: AUR6374WOPCT4 Electronically Filed determining one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and applying the one or more gains to the nullspace joint motions.

14. A surgical system comprising: a surgical tool; a human interface device (HID) that includes a plurality of joints; at least one processor; and memory having instructions stored therein which when executed by the at least one processor causes the surgical system to: determine, while the surgical system is in a first operational mode in which the surgical tool is configured to be manipulated according to movement of the HID, an orientation of the HID; determine that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determine a new orientation of the HID; determine, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and provide the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.

15. The surgical system of claim 14, wherein the HID comprises a gimbal that includes the plurality of joints and an end user control arranged to be held by a user, wherein the orientation of the HID comprises an orientation of the gimbal within three- dimensional (3D) space, wherein the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame.

16. The surgical system of claim 15, wherein the plurality of joints comprises four joints, three joints that are each arranged to rotate about a different axes with respect toAttorney Docket No.: AUR6374WOPCT4 Electronically Filed the others and a redundant joint with respect to the three joints, wherein the memory comprises further instructions to generate the nullspace joint motions to perform a plurality of secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or more joint motion constraints.

17. The surgical system of claim 16, wherein the memory comprises further instructions to determine each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

18. The surgical system of claim 14, wherein the second operational mode comprises at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three- dimensional (3D) space affects corresponding translational movement of the surgical camera.

19. The surgical system of claim 18, wherein the plurality of joints comprises a first plurality of joints and the HID comprises a second plurality of joints, wherein each joint comprises a respective actuator, wherein the memory comprises further instructions to, while in the camera mode, determine, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second plurality of joints, wherein the one or more joint commands are to cause at least one actuator of the first plurality of joints to reorientate at least a portion of the HID responsive to the translational movement.

20. The surgical system of claim 19, wherein the memory comprises further instructions to, while in the camera mode and for each joint of the second plurality of joints, determine a gravity-compensation torque based on a joint position of the joint; andAttorney Docket No.: AUR6374WOPCT4 Electronically Filed apply the gravity-compensation torque using an actuator of the joint.

21. The surgical system of claim 14, wherein the memory comprises further instructions to, for each joint of the one or more joints, determine a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID; determine a unified constraint based on a current joint position, wherein the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and produce a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint.

22. The surgical system of claim 21, wherein the memory comprises further instructions to determine a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, wherein adjusting comprises, for each joint, applying the scaling factor to a respective target nullspace joint motion.

23. The surgical system of claim 14, wherein determining the one or more nullspace joint commands comprises: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold.

24. The surgical system of claim 23, wherein determining the one or more joint commands further comprises: determining a scaling factor based on a magnitude of the orientation error; scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed 25. The surgical system of claim 24, wherein the memory comprises further instructions to determine a velocity at which the HID has moved from the orientation to the new orientation, wherein the scaling factor is further based on the velocity.

26. The surgical system of claim 24, wherein the memory comprises further instructions to: determine a resulting orientation of the HID based on the determined nullspace joint motions; determine one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and apply the one or more gains to the nullspace joint motions.

27. A non-transitory machine-readable medium having instructions which when executed by at least one processor of a surgical system causes the surgical system to: determine, while the surgical system is in a first operational mode in which a surgical tool is configured to be manipulated according to movement of a human interface device (HID), an orientation of the HID that includes a plurality of joints; determine that the system has switched from the first operational mode to a second operational mode in which the surgical tool is to remain stationary as the HID moves within three-dimensional (3D) space; responsive user movement of the HID, determine a new orientation of the HID; determine, based on the new orientation of the HID, one or more nullspace joint commands that includes nullspace joint motions of one or more joints of the HID to realign the HID from the new orientation to the orientation; and produce the one or more nullspace joint commands to the one or more joints to cause to the realignment of the HID.

28. The non-transitory machine-readable medium of claim 27, wherein the HID comprises a gimbal that includes the plurality of joints and an end user control arranged to be held by a user, wherein the orientation of the HID comprises an orientation of the gimbal within three-dimensional (3D) space, wherein the one or more nullspace joint motions are to cause gimbal realignment without causing movement of the end user control with respect to a global reference frame.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed 29. The non-transitory machine-readable medium of claim 28, wherein the plurality of joints comprises four joints, three joints that are each arranged to rotate about a different axes with respect to the others and a redundant joint with respect to the three joints, wherein the non-transitory machine-readable medium comprises further instructions to generate the nullspace joint motions to perform a plurality of secondary tasks that include adjusting at least a joint position of the redundant joint for gimbal realignment and adjusting one or more joint positions of the three joints for object avoidance, subject to one or more joint motion constraints.

30. The non-transitory machine-readable medium of claim 29 comprises further instructions to determine each of one or more joint motions for the one or more joints to satisfy a primary task of converging a pose of the end user control with the surgical tool as a zero velocity.

31. The non-transitory machine-readable medium of claim 27, wherein the second operational mode comprises at least one of: a clutch mode in which the HID is to move responsive to an applied external force independently of the surgical tool; or a camera mode in which translational movement of the HID within three- dimensional (3D) space affects corresponding translational movement of the surgical camera.

32. The non-transitory machine-readable medium of claim 31, wherein the plurality of joints comprises a first plurality of joints and the HID comprises a second plurality of joints, wherein each joint comprises a respective actuator, wherein the non-transitory machine-readable medium comprises further instructions to, while in the camera mode, determine, responsive to user input at the HID, the translational movement based on sensor data of one or more sensors of the second plurality of joints, wherein the one or more joint commands are to cause at least one actuator of the first plurality of joints to reorientate at least a portion of the HID responsive to the translational movement.Attorney Docket No.: AUR6374WOPCT4 Electronically Filed 33. The non-transitory machine-readable medium of claim 32 comprising further instructions to, while in the camera mode and for each joint of the second plurality of joints, determine a gravity-compensation torque based on a joint position of the joint; and apply the gravity-compensation torque using an actuator of the joint.

34. The non-transitory machine-readable medium of claim 27 comprising further instructions to, for each joint of the one or more joints, determine a target nullspace joint motion for moving the joint to cause the HID to rotate about at least one axis towards the orientation of the HID; determine a unified constraint based on a current joint position, wherein the unified constraint accounts for at least one of a velocity limit of the joint or an acceleration limit of the joint at the current joint position; and produce a respective nullspace joint motion by adjusting the target nullspace joint motion to satisfy the unified constraint.

35. The non-transitory machine-readable medium of claim 34 comprising further instructions to determine a scaling factor that enables unified motion across the one or more joints based on all unified constraints of the one or more joints, wherein adjusting comprises, for each joint, applying the scaling factor to a respective target nullspace joint motion.

36. The non-transitory machine-readable medium of claim 27, wherein determining the one or more nullspace joint commands comprises: determining an orientation error based on a difference between the orientation of the HID and the new orientation of the HID; and determining the nullspace joint motions for the one or more joints of the HID that minimizes the orientation error within a threshold.

37. The non-transitory machine-readable medium of claim 36, wherein determining the one or more joint commands further comprises: determining a scaling factor based on a magnitude of the orientation error;Attorney Docket No.: AUR6374WOPCT4 Electronically Filed scaling the orientation error by applying the scaling factor; and determining the nullspace joint motions as a nullspace solution to an optimization problem to minimize the scaled orientation error and to solve one or more nullspace objective functions.

38. The non-transitory machine-readable medium of claim 37 comprising further instructions to determine a velocity at which the HID has moved from the orientation to the new orientation, wherein the scaling factor is further based on the velocity.

39. The non-transitory machine-readable medium of claim 37 comprising further instructions to: determine a resulting orientation of the HID based on the determined nullspace joint motions; determine one or more gains based on a difference between the resulting orientation of the HID and the new orientation of the HID; and apply the one or more gains to the nullspace joint motions.

Images