Method and system for human interface device inverse kinematics for a surgical robot

Abstract

A method of a surgical system that includes determining a tool pose of a surgical tool coupled to a robotic arm. The method determines that a HID pose of the HID is different than the tool pose, where the HID includes several joints. The method determines one or more joint commands for one or more of the HID joints to move the HID pose into a new HID pose that converges within a threshold of the tool pose while satisfying one or more joint motion constraints, and provides the joint commands to the joints to actuate movement of the joints of the HID.

Description

Attorney Docket No.: AUR6374WOPCT3 Electronically Filed Method and System for Human Interface Device Inverse Kinematics for a Surgical Robot 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 performing inverse kinematics for a human interface device (HID) based on changes to a surgical robot of which the HID is arranged to control. 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.: AUR6374WOPCT3 Electronically Filed SUMMARY

[0004] According to one embodiment of the disclosure includes a method performed by at least one programmed processor of a surgical robotic system that includes a human interface device (HID) configured to control a robotic arm, the method including: determining a tool pose of a surgical tool coupled to the robotic arm; determining that a HID pose of the HID is different than the tool pose, where the HID includes several joints; determining one or more joint commands for one or more joints of the joints to move the HID pose into a new HID pose that converges within a threshold of the tool pose while satisfying one or more joint motion constraints; and providing the one or more joint commands to the one or more joints to actuate movement of the one or more joints the HID.

[0005] In one embodiment, determining the tool pose of the surgical tool includes receiving the tool pose of the surgical tool responsive to the surgical tool having been moved from a previous tool pose. In another embodiment, the surgical tool is moved responsive to user input at the robotic arm or the surgical tool during a surgical procedure.

[0006] In one embodiment, determining that the HID pose is different includes determining a pose error based on a difference between the HID pose and the tool pose, where the one or more joint commands are provided responsive to the pose error being less than the threshold. In another embodiment, the method further including: determining joint motions for the joints associated with minimizing the pose error; and determining nullspace joint motion for at least one joint of the joints based on the determined joint motion and based on the one or more joint motion constraints, where the one or more joint commands includes the joint motions and the nullspace joint motion. In some embodiments, the joints includes a redundant joint, a first joint, and a second joint, where determining the nullspace joint motion includes determining a desired joint configuration of the joints resulting from the joint motion using the one or more joint motion constraints that includes: 1) the redundant joint is to remain stationary between the HID pose and the new HID pose, 2) the first joint is at a rotational joint position; and 3) the second joint is less than the rotational joint position.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0007] In one embodiment, the method further including determining the one or more joint motion constraints as output of a hand posture model responsive to input based on the pose of the HID. In some embodiments, the one or more joint motion constraints further includes a joint motion constraint indicating that the second joint has a range of motion that includes joint positions that are greater than a negative rotational joint position and includes joint positions that are less than a positive rotational joint position. In another embodiment, the joints includes a first set of joints and a second set of joints, where HID includes a gimbal having an end user control and a first set of joints that includes the redundant joint, the first joint, and the second joint, and a support arm having a second set of joints, where the arm support is coupled to the gimbal via the redundant joint. In some embodiments, the pose of the HID includes a pose of the end user control, where the pose of the end user control converges with the pose of the tool pose responsive to the output of the joint commands and while a hand of a user is holding the end user control.

[0008] According to another embodiment of the disclosure includes a surgical system including: a surgical tool; a human interface device (HID) configured to control the surgical tool; at least one processor; and memory having instructions which when executed by the at least one processor causes the surgical system to: determine a target HID pose; determine one or more joint commands for one or more joints of the joints to perform a primary task of converging a current HID pose into the target HID pose within a threshold while satisfying one or more secondary task joint motion constraints; and providing the one or more joint commands to the one or more joints to actuate movement of the one or more joints the HID.

[0009] In one embodiment, the memory includes further instructions to: determine a tool pose of the surgical tool based on a first set of position data received from one or more sensors of the surgical tool; and determine the current HID pose based on a second set of position data received from one to more sensors of the HID, where the target HID pose is based on the tool pose. In another embodiment, the memory includes further instructions to receive the first set of position data responsive to the surgical tool having moved from a previous tool poses due to an applied external force. In some embodiments, the memory includes further instructions to: determine a pose error based on a difference between the target HID pose and the current HID pose;Attorney Docket No.: AUR6374WOPCT3 Electronically Filed determine, for the primary task, joint motion for the one or more joints as a solution to an optimization problem that includes minimizing the pose error, where the joint commands are based on the joint motion. In another embodiment, the memory includes further instructions to determine nullspace joint motion for at least one joint based on the determined joint motion and based on the one or more secondary task joint motion constraints, where the joint commands are based on a combination of the determined joint motion and the determined nullspace joint motion. In some embodiments, the joints includes a redundant joint, a second joint, and a third joint, where determining the nullspace joint motion includes determining a desired joint configuration of the joints resulting from the joint motion using the one or more joint motion constraints that includes: 1) the redundant joint is to remain stationary between the HID pose and the target HID pose, 2) the first joint is at a rotational joint position; and 3) the second joint is less than the rotational joint position.

[0010] In one embodiment, the memory includes further instructions to determine the one or more secondary task joint motion constraints as output of a hand posture model responsive to input based on the pose of the HID. In another embodiment, the one or more joint motion constraints further includes a joint motion constraint indicating that the second joint has a range of motion that includes joint positions that are greater than a negative rotational joint position and includes joint positions that are less than a positive rotational joint position. In some embodiments, the joints includes a first set of joints and a second set of joints, where HID includes a gimbal having an end user control and a first set of joints that includes the redundant joint, the first joint, and the second joint, and a support arm having a second set of joints, where the arm support is coupled to the gimbal via the redundant joint. In another embodiment, the pose of the HID includes a pose of the end user control, where the pose of the end user control converges with the pose of the tool pose responsive to the output of the joint commands and while a hand of a user is holding the end user control.

[0011] According to another embodiment of the disclosure includes a method including: determining a pose of a human interface device (HID) arranged to control a robotic arm, where the HID includes several joints; determining a target pose of HID; performing an inverse kinematics (IK) process to determine joint commands for theAttorney Docket No.: AUR6374WOPCT3 Electronically Filed joints to converge the pose of the HID into the target pose of the HID within a threshold while satisfying at least one nullspace motion constraint associated with one or more joints of the joints; and providing the joint commands to the joints.

[0012] In one embodiment, performing the IK process includes: determining a pose error based on a difference between the target pose and the pose; and determining the joint commands as joint motions for the joints based on the pose error to cause the HID to move into the target pose. In another embodiment, performing the IK process further includes: scaling the pose error based on a magnitude of the pose error; determining an intermediate HID pose that is between the pose of the HID and the target pose of the HID based on the scaled pose error; determining the joint motions for the joints as a primary task solution to an optimization problem to minimize the scaled pose error within the threshold and to determine a desired joint configuration of at least one of the joints that satisfies the at least one nullspace motion constraints. In some embodiments, the optimization problem includes a damped least squares problem, where determining the joint motions includes: iteratively solving the optimization problem until a new HID pose converges with the intermediate HID pose within at least one threshold by determining an initial guess of joint positions of the joints based on the intermediate pose of the HID; determining a set of joint motions for the primary task solution and a set of nullspace joint motions using the damped least squares algorithm to minimize the scaled pose error starting at the initial guess of joint positions; determining the new HID pose based on the set of joint motions and the set of nullspace joint motions.

[0013] In one embodiment, the HID includes an end user control arranged to be held and manipulated by the user for controlling the robotic arm. In another embodiment, the joints includes three joints that are orthogonal with respect to each other that are coupled in series with the end user control to allow the control to move within three-dimensional (3D) space. In another embodiment, the at least one nullspace motion constraints includes: a first joint motion constraint of a first joint of the three joints being less than a rotational threshold; and a second joint motion constraint of a second joint of the three joints being at the rotational threshold. In some embodiments, the joints includes a redundant joint that is arranged to rotate about a same axis of the first joint, where the at least one nullspace motion constraints further includes a thirdAttorney Docket No.: AUR6374WOPCT3 Electronically Filed joint motion constraint of the redundant joint in which the redundant joint is to remain within a threshold from a previous joint position as the pose of the HID converges with the target pose of the HID.

[0014] 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. 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.

[0015] 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.: AUR6374WOPCT3 Electronically Filed BRIEF DESCRIPTION OF THE DRAWINGS

[0016] 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.

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

[0018] 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.

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

[0020] Figs.4A-4D show several different joint configurations of the same HID according to one embodiment of the disclosure.

[0021] Fig.5 is a block diagram of the surgical system for performing an inverse kinematics (IK) algorithm for joint movement of the HID according to one embodiment.

[0022] Figs.6A and 6B show a flowchart of one embodiment of a process for performing multi-objective IK for joint motion of the HID.

[0023] Fig.7 is a flowchart of another embodiment of another process for performing IK for joint movement of the HID.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed DETAILED DESCRIPTION

[0024] 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.

[0025] 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.

[0026] 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.: AUR6374WOPCT3 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).

[0027] 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.

[0028] 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.: AUR6374WOPCT3 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).

[0029] 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.

[0030] 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.: AUR6374WOPCT3 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.)

[0031] 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.

[0032] 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.

[0033] 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.: AUR6374WOPCT3 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.

[0034] 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, a first (or left) HID arranged to be held and controlled by the operator’s left hand and a second (or right) HID arranged to be held and controlled by the operator’s right hand. In which case, the remote operator 190 may move the left HID 140 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 140 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 respectiveAttorney Docket No.: AUR6374WOPCT3 Electronically Filed grip signal to control motion of 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.

[0035] 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.

[0036] 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.

[0037] 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.: AUR6374WOPCT3 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.

[0038] 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.

[0039] 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.

[0040] 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 arm support 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.: AUR6374WOPCT3 Electronically Filed embodiment, each of the joints may be capable of rotating about and / or translating along one or more axes.

[0041] 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.

[0042] Having multiple DoF enable the HID to move in 3D space to allow the system to manipulate one or more robotic arms, as a primary task. For instance, having six DoF enable the HID to manipulate the 6 DoF pose (or pose) of the (surgical tool of the) robotic arm. As described herein, however, the HID may include more DoF, such as 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 nullspace 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. This nullspace motion of the HID 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 may be redundant by having a redundant range of motion as joint 215f, whereby motion of joint 215d may occur without affecting (or causing motion) to the other three joints. In one embodiment, nullspace motion may occur based on movement of the redundant joint and / or other joints such that the system 100 may perform a secondary task while not affecting the “primary” task adjusting the end user control 220 based on a manipulation of a surgicalAttorney Docket No.: AUR6374WOPCT3 Electronically Filed tool. In another embodiment, nullspace motion may occur while the user adjusts the HID to control the pose of the surgical tool. In which case, the system may implement one or more secondary tasks while not affecting the pose of the end user control. In one embodiment, a secondary task may include an adjustment of one or more joints in order to provide the user with a more ergonomic position of the end user control 220 and / or object collision avoidance, without affecting the pose of the end user control.

[0043] 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.

[0044] 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 anAttorney Docket No.: AUR6374WOPCT3 Electronically Filed opposite (and / or proportional) external force applied onto the robotic arm. More about providing haptic feedback force is described herein.

[0045] 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.

[0046] 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.

[0047] 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, and 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 userAttorney Docket No.: AUR6374WOPCT3 Electronically Filed through the display 150 of the user console 120. The system may perform one or more conversions (e.g., transformations) 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 410) 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.

[0048] The gimbal 205 may include the last several joints of an HID, where the gimbal provides several DoF. For instance, the gimbal 205 includes four joints, 215d, 215e, 215f, and 215g, that provides four DoF, where one of the joints may be a redundant joint (e.g., 215d) to accommodate secondary tasks without changing the orientation of the end user control 220 (thereby not changing the matching surgical tool orientation). As described herein, user input through the gimbal by the user manipulating the pose of the end user control 220 causes the surgical tool 171 to perform a similar motion. In some cases, however, the system may be configured to adjust the pose of the HID based on one or more criteria. For example, if the surgical tool 171 were to be repositioned manually, which may be the case before the start of a surgical procedure by a bedside operator 180, both the pose of the HID and the tool may be misaligned. In order to align the gimbal with the manually positioned tool, as well as to accommodate some spatial constraints, such as nullspace motion constraints, the system 100 may need to compute inverse kinematics with redundancy resolution for the HID.

[0049] In many robotic manipulation tasks, particularly those requiring grasping interactions, optimizing both primary task performance and secondary taskAttorney Docket No.: AUR6374WOPCT3 Electronically Filed performance, such as ensuring operator comfort, is essential. For the HID, the primary task may include ensuring that the pose of the end user control match the pose of the surgical tool (within a tolerance). Robotic systems, however, often struggle to balance multiple objectives simultaneously, especially when a robotic component includes redundant joints. In these situations, achieving secondary tasks, such as comfortable hand posture for the user, while ensuring precise control and coordination of the HID may become a complex challenge. Moreover, HID motion algorithms face several other challenges. Firstly, maintaining a low misalignment between the surgical tool and the HID, while solving inverse kinematics presents significant challenges due to the nonlinear nature of IK optimization. Conventional IK algorithms often relax this alignment constraint by allowing a higher misalignment to increase the likelihood of finding a solution. This relaxation, however, may lead to configurations that compromise the system’s ergonomics, and potentially cause the robotic arm to shake during movement. This relaxation of alignment may compromise operational efficiency since an operator may spend more time adjusting the position of the surgical robot. Secondly, in surgical robotic environments, ensuring ergonomic joint configurations is essential. Misalignment or suboptimal HID postures may not only compromise precision and control, but also may lead to situations in which the HID may collide with objects in the surgical arena, such as the operator’s hand, resulting in discomfort, inadvertent robotic movements, or event scenarios in which the operator may accidently release the HID. Therefore, there is a need for a surgical robotic system for performing inverse kinematics for a redundant HID that may achieve high success rate with converging with a pose of a surgical tool, while achieving a desired joint configuration to provide user comfort and avoid inadvertent collisions.

[0050] The present disclosure provides a method and system for performing IK operations for a redundant HID arranged to control a surgical robot, where the IK operations satisfies a primary objective of ensuring precise alignment with a surgical tool and one or more secondary objectives such as user comfort and object collision avoidance. The system may determine a pose of the HID, which may be arranged to control a robotic arm (with a surgical tool), where the HID may include several joints. As described herein, the joints may include one or more redundant joints that enable the system to perform secondary tasks. The system determines a tool pose of a surgical toolAttorney Docket No.: AUR6374WOPCT3 Electronically Filed coupled to a robotic arm, where the tool pose may be based on movement of the robotic arm. This movement may be due to the surgical tool being manually manipulated by another operator. The system may determine that a HID pose of the HID is different than the tool pose. As a result, the system may determine one or more joint commands for one or more joints to move the HID pose into a new HID pose that may converge within a threshold to the tool pose while satisfying one or more joint motion constraints, where the constraints ensure that the HID’s pose results in a desired joint configuration for user comfort and collision avoidance, which also ensures that the pose of the HID converges with the pose of the surgical tool. The system may then provide the one or more joint commands to actuate movement of the joints of the HID. Conventional IK may derive multiple possible joint configuration solutions based on the HID’s number of DoF. Turning to Figs.4A-4D, these figures illustrate multiple joint configurations of joints 215d, 215e, 215f, and 215g of the gimbal 205 for reaching a same pose of the end user control 220 (with respect to a global reference frame). Some of these configurations, however, may be less comfortable and / or more prone to object collisions than others. More about these joint configurations are described herein. As a result, the system of the present disclosure may perform an IK process to determine joint commands for the HID’s joints to converge the pose of the HID with the target pose of the HID within a threshold in order to maintain minimal (or below a tolerance of) misalignment between the HID and the surgical tool, while satisfying at least one of the motion constraints. By satisfying the motion constraints, the system may derive a joint configuration that satisfies user ergonomics and / or object collision avoidance (e.g., avoiding a link and / or joint of the HID from coming into contact with the user’s hand that is holding the end user control 220), while at the same time converging the pose of the HID to the target pose. The system may provide the joint commands to the HID joints to actuate movement.

[0051] Fig.5 is a block diagram of the surgical system 100 for performing an HID IK algorithm that converges with a target HID pose while satisfying joint motion constraints, according to one embodiment. The system includes the HID 140, the robotic arm 110, and a controller 400. In one embodiment, the system may include more or less components, such as having two or more robotic arms, each with one or more tools 171 that may be arranged to be manipulated by one or more HIDs.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0052] The robotic arm 110 includes one or more sensors 405, one or more actuators 170, and one or more surgical tools 171. As described herein, the actuators may be a part of and / or cause one or more joints of the arm to move based on joint commands. This movement may cause corresponding joints to rotate and / or translate about one or more axes. The sensors may be configured to produce position data of the robotic arm 110. For instance, the sensors may include encoders, each of which may be configured to measure a joint position (e.g., translational and / or rotational values) as encoder (or position) data of a respective joint. In which case, the position data from the sensors 405 may indicate the actual position of a respective joint in space (with respect to a reference point or frame). As described herein, this position data may be used to determine an actual tool pose of the tool 171 coupled to the robotic arm 110.

[0053] In one embodiment, the actuators 170 of the robotic arm may be configured to provide torque to perform gravity compensation in order for the arm to stay in place. The robotic arm may be arranged to be manually manipulated by an operator. For example, the sensors 405 may be configured to measure forces applied externally upon the robotic arm 110, and in response to those forces, the system 100 may be configured to cause one or more actuators 170 to move in the direction of that force. This may provide an operator with assisted movement in response to manual input. This may occur at the beginning of a surgical procedure, whereby the bedside operator 180 may move the robotic arm 110 in order to position the surgical tool 171 into a particular surgical workspace and having a particular pose, without needing the remote operator 190 to move the arm using the HID 140.

[0054] The HID 140 includes one or more sensors 410 and one or more actuators 415. In one embodiment, the HID’s sensors 4 and / or actuators 415 may perform similar operations as the sensors 405 and / or actuators 170, respectively, of the robotic arm 110. For example, the sensors 410 may be configured to produce position data of one or more joints, while the actuators 415 may be configured to move one or more corresponding joints of the HID 140, based on joint commands produced by the system 100.

[0055] In one embodiment, the controller 400 may be a special-purpose processor such as an application-specific integrated circuit (ASIC), a field-Attorney Docket No.: AUR6374WOPCT3 Electronically Filed programmable gate array (FPGA), a general-purpose microprocessor, a digital signal controller, or a set of hardware logic structures (e.g., arithmetic logic units, filters, and dedicated state machines). In one embodiment, the controller 400 may be a part an electronic device, such as the console computer system 160, the control tower 131, and / or the user console 120. Although illustrated as being a single component, the controller may include one or more electronic components (e.g., processors, memory, etc.) that may be communicatively coupled on a single electronic device (such as the console computer system 160), or across multiple devices (e.g., communicating over a wireless computer network). In some embodiments, the controller 400 may be a part of a separate device, such as a part of a remote server that may be in communication with one or more electronic devices of the surgical system 100. In which case, the remote server may be configured to communicate between the user console 120 and the control tower 131 for receiving sensor data from sensors 405 and providing the joint commands to the HID based on the sensor data.

[0056] The controller 400 may be configured to perform multi-objective HID IK operations to converge a HID pose to a target HID pose that may correspond to a surgical tool pose, while satisfying motion constraints. For instance, the controller 400 may perform a primary task (or objective) of ensuring that the HID 140 tracks movement of the robotic arm 110, while also satisfying one or more secondary tasks that may include optimizing ergonomics and object collision avoidance. The controller 400 includes an IK 460 and a HID / Tool pose estimator 450. The controller 400 may be configured to receive sensor data from the sensors 405 of the robotic arm, responsive to user input, such as a user (e.g., bedside operator 180) moving and manipulating at least a portion of the robotic arm 110.The sensor data, as described herein, may include position data indicating the positions (e.g., position and / or orientation) of one or more joints of the robotic arm 110. In addition to, or in lieu of, receiving sensor data, the controller 400 may be configured to transmit joint commands based on user input through the HID to adjust a pose of the robotic arm 110.

[0057] The HID / Tool pose estimator 450 may be configured to estimate a pose of the HID 140 based on the user command from the HID. 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) Cartesian space) that is being held by the user while moving the HID.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed The estimator 450 may be configured to convert one or more joint commands for one or more joints of the HID into the HID pose. For instance, when the joint commands includes position data of one or more joints of the HID, the estimator 450 may use a forward kinematics algorithm to convert the data into the HID pose. The estimator 450 may be configured to determine a pose of the robotic arm 110. In particular, the estimator 450 may be configured to receive sensor data from one or more sensors 405, 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.

[0058] The HID / tool pose estimator 450 may be configured to determine a target HID pose of the HID 140 based on movement of the robotic arm 110, as described herein. The estimator 450 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 from 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 450 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 450 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 405, 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.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0059] In one embodiment, at least some of the operations of the estimator 450 may be performed on a different device. For instance, when the controller 400 is a part of an electronic device within the surgical operating room, such as the console computer system 160, the target HID pose may be received (as a user command) from a separate electronic device, such as the console computer system 160. In which case, the control tower 131 may perform at least some of the operations of the estimator 450 to produce a target HID pose responsive to user input through the surgical arm 110, and may provide the target HID pose as a control command (e.g., through a computer network) to the controller 400.

[0060] The IK 460 may be configured to perform an iterative process for generating joint motion for one or more joints of the HID 140 to cause the end user control 220 to move into the target HID pose, while satisfying one or more secondary task joint (e.g., nullspace) motion constraints in order to achieve multiple objectives. To perform at least some of these operations, for each iteration, the IK 460 includes a HID pose error estimator 462, a secondary task joint motion constraint generator 470, and a local minimum handler 461.

[0061] The HID pose error estimator 462 may be configured determine a HID pose error based on a difference between the current HID pose and the target HID pose. The HID pose error may indicate a deviation or estimate of how different the actual HID position and / or orientation are from the target HID position and / or orientation. In one embodiment, the HID pose error may represent a path (e.g., straight-line pattern) along which the HID may traverse in order to reach the target HID pose. In particular, the HID pose error may represent an estimate of the movement through which the HID 140 is to move from its current position to the target position.

[0062] For the HID 140 to traverse the HID pose error in order to reach the target HID pose may require a considerable amount of movement by the end user control 220. Computing the joint movements of the HID based on a large HID pose error (e.g., above a threshold) may cause jerking motions in the HID as the end user control moves into a target position / orientation. Therefore, the HID pose error estimator 462 may be configured to perform pose error handling operations to determine a series of smaller motions for moving the HID towards its ultimate targetAttorney Docket No.: AUR6374WOPCT3 Electronically Filed position. As described herein, the pose error may be defined as a path along which the end user control 220 may traverse. From this path, the estimator 462 may be configured to derive portions of the path along which the end user control may be driven. To do this, the estimator 462 may be configured to determine one or more intermediate poses of the end user control 220 between a starting pose of the end user control (e.g., the pose of the control from before user input is received at the robotic arm 110) and the target pose of the end user control 220, whereby the IK algorithm may iteratively compute motions for the HID to converge towards the target pose. By iteratively computing smaller motions, the system 100 may be configured to enable smooth motion between the current (original) pose to the target pose. In which case, the HID pose error estimator 462 may be configured to determine an intermediate pose of the HID to account for at least a portion of the HID pose error.

[0063] The estimator 462 may determine an intermediate pose based on one or more limits of the HID 140. For instance, the estimator 462 may determine the intermediate pose 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 the intermediate pose, the estimator 462 may be configured to perform one or more optimization operations to find the intermediate pose that satisfies an objective function to maximize movement of the end user control 220, 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.

[0064] In another embodiment, the estimator 462 may use other methods to determine the intermediate pose, such as gain scheduling and error clamping that may be based on the determined pose error. For instance, the estimator 462 may be configured to apply a non-linear scaling factor to the pose error based on the magnitude (size) of the pose error. For example, when the pose error is greater than a first threshold, the estimator 462 may apply a first scaling gain to reduce the pose error, where the intermediate pose may be based on the reduced pose error. For example, when the pose error is 10 mm, the estimator 462 may apply a first gain to reduce the error to 2 mm, where a 2 mm movement may be more desirable than a 10 mmAttorney Docket No.: AUR6374WOPCT3 Electronically Filed movement, which may cause jerking. Conversely, when the pose error is less than the threshold, the estimator 462 may apply a second scaling gain to increase the pose error. For instance, when the pose error is small, such as 0.2 mm, the estimator 462 may apply a second gain, which may be greater than the first gain, to be closer to the small pose error. Scaling the error may enable the system to reduce the overall convergence time when the pose 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. In another embodiment, the estimator 462 may apply error clamping, whereby the estimator may define one or more intermediate poses between maximum (or minimum) amounts of pose within the pose 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.

[0065] The local minimum handler 461 may be configured to determine joint motion for moving one or more joints of the HID in order for the system to achieve a primary task of ensuring the end user control 220 of the HID to converge with a target HID pose and to achieve one or more secondary tasks as nullspace commands that implement one or more secondary task joint motion constraints for having the joints of the HID reach a desired joint configuration. The handler 461 may perform a numerical iterative process for generating joint motion that converges a pose of the HID within at least a threshold responsive to changes to the robotic arm 110, which may be the result of external forces (e.g., user input through a bedside operator 180 physically moving at least a portion of the arm 110). The one or more joint motions may include joint velocities that enable the joints of the HID to move from their current position to new positions in order for the HID to achieve a target pose, as its primary task. In another embodiment, the joint motion produced by the local minimum handler 461 may include joint positions of one or more joints with respect to time.

[0066] The local minimum handler may implement a non-linear optimization process to determine joint-level motion that causes the HID to converge with the target HID pose. In particular, the optimization process may be an iterative numerical process that produces at least one IK solution (e.g., joint motions) by minimizing the determined pose error. In one embodiment, the planner may implement a gradient descent-based method by identifying joint motion solutions that are associated with aAttorney Docket No.: AUR6374WOPCT3 Electronically Filed pose error minimum. For instance, the local minimum handler may implement a damped least squares optimization process in order to determine joint motion associated with a local minimum by using a damping factor that may govern the level of approximation. In one embodiment, the damping factor may be adjustable based on a proximity to a singularity, and as a result, the handler may be configured to determining the damping factor as a function of joint configuration. In which case, the system 100 may be configured to determine joint motion, for a primary task, as a solution to an employed optimization problem by iteratively performing a local minimum optimization process to determine joint motion corresponding to a pose error minimum, which may correspond to a solution associated with a global minimum.

[0067] In one embodiment, the local minimum handler 461 may be configured to determine joint motion associated with the solution that minimizes the pose error. For instance, the local minimum handler may perform an optimization process to determine a solution (joint motion) associated with a minimal pose error from initial joint positions to target joint positions. This solution may be associated with a global minimum, the most reduced pose error, such as being below a threshold or zero. To ensure that the optimization process does not get stuck at a local minimum, which may not be the solution with the most minimal pose error, the handler 461 may iteratively perform the optimization process, where the process may be restarted at different target joint positions to move the HID towards a target pose. For instance, for each iteration, the handler may randomly select an initial guess of joint positions for which an optimized solution is to be determined. Thus, for each iteration, the handler may determine an initial guess of joint positions, which may be based on the pose error, and then may perform the optimization process to determine an optimized pose error as a local minimum.

[0068] In one embodiment, the local minimum handler 461 may perform one or more iterations of the optimization process until a global minimum is determined. For example, the handler may determine whether the resulting pose associated with a solution converges (e.g., within a threshold) to a target pose. If so, that solution (joint motion) may be used to move the joints. In another embodiment, the system 100 may implement the optimization process a threshold number of times (e.g., 100 times), and may select the pose of the solution that converges the closest to the target pose.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0069] As described thus far, the local minimum handler 461 may perform an optimization process for determining target joint motion as a solution in which pose error is at a minimum (e.g., below a threshold). This may ensure that the IK 460 performs a primary task of converging the resulting HID pose with the target HID pose. In one embodiment, the optimization process (e.g., damped least squares) may depend on the number of joints of the HID. For instance, if the HID includes six joints, the solution of the handler may produce six joint motions (e.g., joint velocities). If, however, the HID includes one or more redundant joints, such as joint 215d, the IK 460 may produce a nullspace command for performing one or more secondary tasks, such as achieving a desired joint configuration so as to enable user comfort and / or to avoid object collisions. In which case, the local minimum handler may be configured to perform an optimization process for finding the joint motion solution for the primary task in addition to nullspace optimization, which may be a function of a Jacobian matrix of the IK. The nullspace optimization may produce one or more nullspace joint motions for one or more joints of the HID in order to achieve a desired joint configuration. For example, a damped least squares local optimization may optimize the joint motion solution based on the pose error and a nullspace command for optimizing nullspace joint motion for achieving a particular joint configuration for one or more secondary tasks by satisfying one or more secondary task motion constraints. As a result of the optimization, the handler may produce primary task joint motions in combination with nullspace joint motion (according to the nullspace command). The nullspace joint motion may not affect the resulting HID pose due to the primary task joint motion.

[0070] The nullspace commands may include secondary task joint motion constraints of one or more joints for achieving a desired joint configuration. In particular, the handler 461 may be configured to optimize one or more objective functions according to one or more secondary task joint motion constraints for achieving a desired joint configuration. Such motion constraints may be optimized in order 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 joint configuration that maximizes user comfort. The optimized nullspace secondary task joint motions may beAttorney Docket No.: AUR6374WOPCT3 Electronically Filed projected into the nullspace using a nullspace projection, where the resulting joint motion (e.g., joint velocities) may be added to the primary task joint velocities, as described herein.

[0071] The IK 460 may be configured to generate nullspace joint motion according to nullspace motion commands based on one or more desired joint configurations in order to achieve the secondary task. As described herein, the IK 460 may be configured to optimize this nullspace joint motion by optimizing the secondary task joint motion constraints projected using a nullspace projection based on the primary task convergence. The secondary task joint motion constraint generator 470 may be configured to determine the secondary task joint motion constraints (or motion constraints) which may be used by the handler 461 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 470 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 400). The generator may determine the constraints by retrieving them from the memory.

[0072] In one embodiment, the secondary task joint motion constraint generator 470 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) jointAttorney Docket No.: AUR6374WOPCT3 Electronically Filed positions of one or more joints of the HID at which the joint motion optimization is being performed (e.g., at which joint motion is being determined by the local minimum handler 461). In which case, the generator may be configured to determine the joint position of one or more joints during primary task optimization and may be configured to 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 a result, the motion constraint generator 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.

[0073] 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 pose. 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 470 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.

[0074] Motion constraints may promote collision avoidance and ergonomics by limiting (or emphasizing) nullspace motion of one or more joints of the HID 140 based on known hand positions of the remote operator 190 while holding and manipulating one or more HIDs. For instance, joints may be capable of rotating within a wide joint motion range. Some joint motion, however, may result in one or more links of the HID bumping up against the user’s hand while holding the end user control. Referring to Fig.2, joint 215d, which may be a redundant joint, as described herein, may be arranged to rotate link 210d at a joint angle, θd, of approximately 0° with respect to joint 215d’s axis. This is also illustrated in Fig.4A. Rotation of link 210d by joint 215d, while the operator 190 is holding the end user control 220 may result in link 210dAttorney Docket No.: AUR6374WOPCT3 Electronically Filed bumping up against the user’s hand. This may be undesirable as it may result in the user inadvertently releasing the control 220. Therefore, the generator 470 may be configured to generate a motion constraint for keeping link 210d as far away from the user’s hand that is holding the control 220. This may include keeping θdat approximately (e.g., within a threshold) 0° during operation, which is shown in Figs.2 and 4A. Thus, it may be desirable to ensure that the redundant joint remains still during operation of the HID 140.

[0075] Figs.4C and 4D, however, show several undesirable joint configurations that would result in a potential collision with the user’s hand. For instance, these figures show that the rotational angle of joint 215d, θd, has been rotated to approximately 180°, resulting in link 210d being close to the user, where the user would be positioned in front of the gimbal 205. These orientations would result in the user’s palm bumping up against link 210d and / or joint 215e, if the user were to be holding the end user control 220 at this joint configuration.

[0076] In another embodiment, rather than keeping θd of joint 215d at 0°, the motion constraint generator 470 may be configured to determine a motion constraint to try to hold joint 215d still between the HID’s current pose to the HID’s target pose. For example, the nullspace command optimized with the joint motion solution may include a motion constraint to try not to move joint 215d, by keeping θd between joint configurations required to move the end user control 220 between a starting pose to a target pose, but upon determining that one or more joints potentially bump into the user’s hand, the system may relax such a constraint. This determination may be based on the hand posture model, which may estimate the position of the user’s hand with respect to the target HID pose, and upon determining that a portion of the HID is to come into contact with the user’s hand, the IK may relax the joint 215d constraint.

[0077] Some motion constraints may limit motion of one or more joints to optimize ergonomics. Again, referring to Fig.4A, the joint angle of joint 215e, θe, is shown as being at (approximately) 0°, resulting in link 210e curving below the user’s hand that may be holding the control 220, and the joint angle of joint 215f, θf, is shown as being at (approximately) 90°, resulting in link 210f being approximately in the direction of the user’s hand that is holding the control 220. The system may beAttorney Docket No.: AUR6374WOPCT3 Electronically Filed configured to limit motion of these joints, since excessive rotations of these joints may cause the user’s wrist to strain as the user is holding the control 220. Therefore, the generator 470 may produce a motion constraint such that |θe| < 90°. In particular, joint 215e may be within this range, such that the link 210e may not rotate upward towards the user’s hand. Joint 215e is shown exceeding this range in Figs.4B and 4C, both of which show θeat approximately 180°. Fig.4C may also not be a preferrable joint configuration, since in this configuration, the user’s hand would bump up against joint 215e, as described herein. Furthermore, Fig.4D may not be preferrable since it would require the user’s hand to make a full rotation, thereby straining the user’s wrist, if the pose of the HID were to move from the pose illustrated in Fig.4A to the pose in Fig. 4D.

[0078] In another embodiment, the generator 470 may be configured to generate a motion constraint based on the orientation of joint 215f. For instance, this joint may include a motion constraint such that the joint θfremain approximately near 90°, as shown in Fig.4A. Otherwise, if this joint were to rotate far beyond this range, such as 180°, this would result in the end user control 220 facing away from the user or towards joint 215e. In addition to not being a comfortable pose, this may also result in the user’s hand (e.g., knuckles) bumping up against the joint 215e. In one embodiment, θf may be within a tolerance of 90°, such as approximately 35°, such that θf may be within a range of motion between 55° and 125°. As described herein, the system may include a left HID and a right HID. In which case, at least some of the motion constraints described herein may be listed as corresponding to the left HID. As a result, the system 100 may include inverted motion constraints for the right HID, which may have a similar structure (same joints and / or links) as the HID 140 described herein. In which case, a joint angle θfof a corresponding joint 215f may remain approximately - 90°.

[0079] As a result, the secondary task joint motion constraint generator 470 may be configured to produce motion constraints based on undesirable joint configurations, such that the IK 460 may produce nullspace commands 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 460 may be configured to assign aAttorney Docket No.: AUR6374WOPCT3 Electronically Filed particular joint configuration as a nullspace command by selecting one or more corresponding motion constraints that affect such a configuration of the joints.

[0080] As described herein, the IK 460 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

[0081] An example of an IK optimization process is as follows. The IK 460 may be configured to perform an optimization process, such as damped least squares, to determine joint motions for satisfying the primary task and / or secondary tasks. The IK 460 may set a joint vector of the gimbal as: ^^ ൌ ^^^ௗ ,^^^ , ^^^, ^^^൧்

[0082] where qd is theqe is the position of joint 215e, qf is a position of joint 215f, and qg is 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 ‖^^∆^^ െ ^^‖ଶ ^ ^^^‖∆^^‖ଶ

[0083] 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 as ∆^^ ൌ ^^்^^^^^் ^ ^^ ି^^^^^ ^^

[0084] where e may be the error, 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 460 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 theAttorney Docket No.: AUR6374WOPCT3 Electronically Filed surgical tool. Written another way, this primary task joint motion may be a function of the Jacobian of the robotic arm (including the tool), as ^^^^^^^^^^^^^^^^^ ൌ ^^^^^

[0085] where J+is the pseudoinverse matrix of the Jacobian, which may be written as ^^ା ൌ ^^^^^^^் ^ ^^ ି^^^^ ^0 , ^^ ^ ^^^^^^ ൌ ^^^ ଶ ^^ ^^ ^task, the IK 460 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: ^^^^^ െ ^^ା^^^ ൌ 0

[0088] 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 ^^^^^^^^^^^^^^^^^^^^^ ൌ ^^^ െ ^^^^^^^^^^

[0089] As a result, Δq may be minimized to ∆^^ ൌ ^^^^^^^^^^^^^^^^^ ^ ^^^^^^^^^^^^^^^^^^^^^ ൌ ^^ା^^ ^ ^^^^^^ െ ^^ା^^^^^^^Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0090] where kr is 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 ^^^^^^^^ ^^^^ ൌ^^^^

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

[0092] where k1 and k2 are 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 460 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, while 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 constraintsAttorney Docket No.: AUR6374WOPCT3 Electronically Filed 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 2^^^௧ െ ^^^ଶ →^^^^^^^ ^^^^ൌ ^^^௧ െ ^^^

[0093] 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 ^^^ ^2

[0094] Thus, ascost 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 a left HID and a corresponding joint may be -90° for the gimbal of a right HID.

[0095] The IK 460 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 reachingAttorney Docket No.: AUR6374WOPCT3 Electronically Filed their limits by attempting to shift the joint positions towards the middle of their joint limits. The cost function includes: ଶ ସ ଶ^ ^1൫^^^^ ^^ ൌ ^,^^௫ െ ^^െ^ ^,^^^൯

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

[0097] joint velocities tonegative 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.

[0098] 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 460 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 joint 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.

[0099] Figs.6A, 6B, and 7 are flowcharts of processes 600 and 700, respectively, for performing HID IK for determining joint motion that satisfies primary and secondary tasks. At least some of the operations of these processes may be performed by the controller 400 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 400. Some of these processes may be performed inAttorney Docket No.: AUR6374WOPCT3 Electronically Filed “real-time”, during a surgical procedure in which an operator is controlling a robotic arm to perform one or more surgical tasks.

[0100] Turning to Fig.6A, this figure illustrates a flowchart of one embodiment of the process 600 for performing multi-objective IK for planning joint motion of the HID 140. The process 600 begins with the system 100 determining a tool pose of the surgical tool of the robotic arm (at block 605). As described herein, the HID / tool pose estimator 450 may be configured to determine the pose of the surgical tool based on position data from one or more sensors of the robotic arm. The system may determine a target HID pose as the determined tool pose, which may have moved with respect to a previously determined tool pose. For instance, the tool pose may be received (e.g., from the estimator 450) responsive to the surgical tool having been manually moved from a previous tool pose. As described herein, this movement may be due to user input at the robotic arm or the surgical tool during a surgical procedure, such as a bedside operator 180 moving the tool into a position for the remote operator 190 to perform a surgical task. In another embodiment, the tool pose may be determined based on other sensor data, such as image data captured by one or more cameras of the surgical system 100. The system 100 receives position data of one or more sensors of the HID (at block 610). In one embodiment, this position data may be received responsive to the system 100 determining the tool pose of the surgical tool. The system determines a HID pose of the HID based on the position data (at block 615). For instance, the system 100 may perform a forward kinematics function to determine the pose of the (e.g., end user control of the) HID in Cartesian space. The system 100 determines a pose error based on a difference between the tool pose and the current HID pose (at block 620). As described herein, the pose error may represent a path along which the end user control 220 of the HID 140 is to move within space from its current HID pose to a target HID pose based on the determined tool pose.

[0101] The system 100 determines whether the pose error is less than a threshold (at decision block 625). In particular, the system 100 may determine whether the current HID pose converges with the target HID pose based on whether both poses are within a threshold of each other. If not, the system determines an intermediate HID pose based on the pose error (at block 635). The system may determine the intermediate pose by performing a gain scheduling function upon the pose error to scaleAttorney Docket No.: AUR6374WOPCT3 Electronically Filed the pose based on the magnitude of the pose error, for example. As a result, the system may determine an intermediate (or reduced) pose error based on the difference between the current HID pose and the intermediate HID pose.

[0102] Moving 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 cause the current HID pose to reduce the (e.g., intermediate) pose error such that the HID converges with the intermediate pose. This process may be an iterative process performed by the local minimum handler to identify the global minimum from derived local minimums. For instance, the process 600 may iteratively perform blocks 640-665 to solve an optimization problem in which the system 100 may determines (e.g., within reasonable certainty) a global minimum of pose error from one or more determined local minimums. As described herein, the IK 460 may perform an IK process to determine joint commands for the joints to converge the pose of the HID into the target pose of the HID, while satisfying at least one null space motion constraint associated with one or more of the joints. Turning to the optimization process, the system 100 determines an initial guess of joint positions based on the intermediate pose of the HID (at block 640). 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 based on this target HID pose. 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.

[0103] The system 100 determines a joint motion for satisfying the primary task of moving the HID based on the target tool pose (at block 645). In particular, the system may determine joint motions (e.g., joint velocities) for one or more joints of the HID associated with minimizing the pose error. Primary task joint motion may be a function of a Jacobian of the robotic arm, as ^^^^^^^^^^^^^^^^^ ൌ ^^ା^^

[0104] where J+may be a pseudoinverse of the Jacobian, J, of the HID, and e may be the current pose error, which may be based on the difference between a currentAttorney Docket No.: AUR6374WOPCT3 Electronically Filed pose of the HID to the intermediate pose. In one embodiment, e may include a cost function that represents the error, which may be minimized using an optimization function, such as damped least squares. This cost function may be minimized by taking steps into the opposite direction of the gradient, where the optimization starts at the initial guess in order to find a joint motion solution for a local minimum pose error. The system 100 determines a nullspace motion for satisfying one or more secondary tasks (at block 650). This motion may include one or more joint motions (e.g., joint velocities) for one or more of the joints, such as joint 215d), which may be based on the joint motion determined for the primary task and based on secondary task joint motion constraints. In particular, the nullspace motion may be based on the primary task joint motion for moving one or more joints of the HID into a desired joint configuration resulting from the joint motion using one or more secondary task joint motion constraints. Specifically, the 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 ^^^ ^^^^^^^^^^^^^^^^^^ ൌ ^^^ െ ^^ା^^^^^௧^^^

[0105] where I is the identify matrix, (I – J+J) includes a nullspace projection for mapping secondary task joint motion within the nullspace to ensure that any mapped secondary joint motion does not result in end user control motions, and Stask(or ^^^^ ) includes the secondary task that is to be achieved for the nullspace motion. Inone embodiment, Staskmay include or be based on one or more 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 generator 470 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 between the HID pose and the new HID pose, 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 one embodiment, Stask may include an objective function based on the current joint positions for optimizing nullspace joint motion to ensureAttorney Docket No.: AUR6374WOPCT3 Electronically Filed object avoidance and / or to maximize ergonomics while satisfying the one or more motion constraints. In another embodiment, the Staskmay include an optimization function for minimizing the joint’s energy in the nullspace. For instance, the optimization function may take into account the joint energy used to complete the primary task, and may ensure that any additional joint energy in the nullspace is reduced to a minimum. In one embodiment, the system may produce one or more nullspace motions that indicate one or more joint configurations, and may be configured to select a nullspace motion (e.g., one or more nullspace joint motions) that satisfies at least one of the secondary task joint motion constraints. 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. From these solutions, the system 100 may select the solution that satisfies at least one of the motion constraints. The system 100 determines the resulting joint motion based on a combination of the joint motion and the nullspace motion (at block 655). In particular, the system 100 may optimize the primary task joint motion and / or the nullspace joint motion in parallel until a local minimum is achieved. Once achieved, the system may combine the optimized primary joint motion and the nullspace joint motion into a resulting joint motion, ^^^ ^^^^^^^^^^^^^^^^^^, such as: ^^^ ^^^^^^^^^^^^^^^^^^ ൌ ^^^^^^^^^^^^^^^^^ ^ ^^^^^^^^^^^^^^^^^^^^^

[0106] As a result, the system determines joint motions for the primary task solution and nullspace joint motions using a damped least squares algorithm to minimize the scaled pose error starting at the initial guess of joint positions. The system 100 determines a new HID pose based on the resulting joint motion (at block 660). For instance, the resulting joint motion may indicate the resulting joint positions based on the primary and secondary tasks, and the system may perform a forward kinematics operations to determine a new (resulting) pose of the HID based on that motion. The system 100 determines whether the new HID pose converges with the intermediate pose within a threshold (at decision block 665). As described herein, the local minimum handler 461 may be configured to determine a solution for a local minimum of pose error. To get the most optimized solution, however, the system require the global minimum, since this would be the solution with the lowest pose error. As a result, the IK 460 may be configured to determine whether the solution is a localAttorney Docket No.: AUR6374WOPCT3 Electronically Filed minimum or the global minimum. To do this, the IK may determine whether the resulting pose error, between the new HID pose and the intermediate pose is less than a threshold. If not, the IK 460 may restart the optimization process, by returning to block 640 to select a new initial guess. In one embodiment, the determination of whether the solution is the global minimum may be based on other criteria. For example, the IK 460 may be configured to repeat these operations for a predefined number of times (e.g., 100 times), after which, the IK 460 may be configured to select the solution with the most minimum pose error between the resulting HID pose and the intermediate pose as the global minimum.

[0107] If, however, the new HID pose is associated with a solution of the global minimum, the system 100 updates the joint position data based on the new HID pose (at block 670), and returns to block 620 to determine a new pose error based on a difference between the tool pose and a new current HID pose based on the updated position data. The system 100 returns to decision block 625, and if the pose error is less than the threshold, meaning that the final HID pose associated with the most recent updated position data converges with the tool pose, the system 100 provides joint commands based on the updated position data to joints of the HID to cause the HID to move into the updated HID pose (at block 630). As a result, the pose of the end user control may converge with the pose of the tool pose responsive to the output of the joint commands, while the user’s hand is holding the end user control.

[0108] The process 600 of Figs.6A and 6B may provide joint motion for the HID to converge with the surgical tool, while also providing desired joint configurations that reduce the likelihood of the HID colliding with the user’s hands. Moreover, the IK 460 provides a numerically stable method for kinematically redundant robots that is able to achieve high success rate for providing solutions for primary and secondary tasks with a very tight convergence threshold, which may be required for highly constrained environments, such as surgical environments.

[0109] Fig.7 is a flowchart of another embodiment of the process 700 for performing IK for joint movement of the HID. The process 700 begins with the system 100 determining a tool pose of a surgical tool coupled to a robotic arm (at block 710). The system determines that the HID pose of the HID is different than the tool pose,Attorney Docket No.: AUR6374WOPCT3 Electronically Filed where the HID includes several joints (at block 720). For instance, the system 100 may convert the tool pose into a target HID pose, and may determine that both poses are different based on a pose error based on a difference between the HID pose being less than a threshold. If so, the system 100 may determine that the HID pose is different. The system 100 determines one or more joint commands for one or more joints of the HID’s joints to move the HID pose into a new HID pose that converges within a threshold of the tool pose while satisfying one or more secondary task joint motion constraints (at block 730). As described herein, the system 100 may perform a numerical iterative process to converge the HID pose with the tool pose, while satisfying one or more motion constraints that may be based on desired joint configurations derived from a hand posture model in order to maximize user comfort and object avoidance. The system provides the joint commands to the joints to actuate movement of the joints of the HID (at block 740). Thus, the joint commands may be provided responsive to the original pose error being less than the threshold in order to move the joints so that the HID pose converges with the tool pose.

[0110] Some embodiments 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.

[0111] 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 HID IK operations to produce joint motion for converging the HID pose with a tool pose while satisfying motion constraints based on secondary tasks, 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.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed

[0112] 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 they 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.

[0113] 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.

[0114] 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.: AUR6374WOPCT3 Electronically Filed CLAIMS What is claimed is:

1. A method performed by at least one programmed processor of a surgical robotic system that comprises a human interface device (HID) configured to control a robotic arm, the method comprising: determining a tool pose of a surgical tool coupled to the robotic arm; determining that a HID pose of the HID is different than the tool pose, wherein the HID comprises a plurality of joints; determining one or more joint commands for one or more joints of the plurality of joints to move the HID pose into a new HID pose that converges within a threshold of the tool pose while satisfying one or more joint motion constraints; and providing the one or more joint commands to the one or more joints to actuate movement of the one or more joints the HID.

2. The method of claim 1, wherein determining the tool pose of the surgical tool comprises receiving the tool pose of the surgical tool responsive to the surgical tool having been moved from a previous tool pose.

3. The method of claim 2, wherein the surgical tool is moved responsive to user input at the robotic arm or the surgical tool during a surgical procedure.

4. The method of claim 1, wherein determining that the HID pose is different comprises determining a pose error based on a difference between the HID pose and the tool pose, wherein the one or more joint commands are provided responsive to the pose error being less than the threshold.

5. The method of claim 4 further comprising: determining joint motions for the plurality of joints associated with minimizing the pose error; and determining nullspace joint motion for at least one joint of the plurality of joints based on the determined joint motion and based on the one or more joint motion constraints,Attorney Docket No.: AUR6374WOPCT3 Electronically Filed wherein the one or more joint commands comprises the joint motions and the nullspace joint motion.

6. The method of claim 5, wherein the plurality of joints comprises a redundant joint, a first joint, and a second joint, wherein determining the nullspace joint motion comprises determining a desired joint configuration of the plurality of joints resulting from the joint motion using the one or more joint motion constraints that comprises: the redundant joint is to remain stationary between the HID pose and the new HID pose, the first joint is at a rotational joint position; or the second joint is less than the rotational joint position.

7. The method of claim 6 further comprising determining the one or more joint motion constraints as output of a hand posture model responsive to input based on the pose of the HID.

8. The method of claim 6, wherein the one or more joint motion constraints further comprises a joint motion constraint indicating that the second joint has a range of motion that includes joint positions that are greater than a negative rotational joint position and includes joint positions that are less than a positive rotational joint position.

9. The method of claim 6, wherein the plurality of joints comprises a first set of joints and a second set of joints, wherein HID comprises a gimbal having an end user control and a first set of joints that includes the redundant joint, the first joint, and the second joint, and a support arm having a second set of joints, wherein the arm support is coupled to the gimbal via the redundant joint.

10. The method of claim 9, wherein the pose of the HID comprises a pose of the end user control, wherein the pose of the end user control converges with the pose of the tool pose responsive to the provided of the joint commands and while a hand of a user is holding the end user control.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed 11. A surgical system comprising: a surgical tool; a human interface device (HID) comprising a plurality of joints, the HID configured to control the surgical tool; at least one processor; and memory having instructions which when executed by the at least one processor causes the surgical system to: determine a target HID pose; determine one or more joint commands for one or more joints of the plurality of joints to perform a primary task of converging a current HID pose into the target HID pose within a threshold while satisfying one or more secondary task joint motion constraints; and providing the one or more joint commands to the one or more joints to actuate movement of the one or more joints the HID.

12. The surgical system of claim 11, wherein the memory comprises further instructions to: determine a tool pose of the surgical tool based on a first set of position data received from one or more sensors of the surgical tool; and determine the current HID pose based on a second set of position data received from one to more sensors of the HID, wherein the target HID pose is based on the tool pose.

13. The surgical system of claim 12, wherein the memory comprises further instructions to receive the first set of position data responsive to the surgical tool having moved from a previous tool poses due to an applied external force.

14. The surgical system of claim 12, wherein the memory comprises further instructions to: determine a pose error based on a difference between the target HID pose and the current HID pose; and determine, for the primary task, joint motion for the one or more joints as a solution to an optimization problem that includes minimizing the pose error,Attorney Docket No.: AUR6374WOPCT3 Electronically Filed wherein the joint commands are based on the joint motion.

15. The surgical system of claim 14, wherein the memory comprises further instructions to determine nullspace joint motion for at least one joint based on the determined joint motion and based on the one or more secondary task joint motion constraints, wherein the joint commands are based on a combination of the determined joint motion and the determined nullspace joint motion.

16. The surgical system of claim 15, wherein the plurality of joints comprises a redundant joint, a first joint, and a second joint, wherein determining the nullspace joint motion comprises determining a desired joint configuration of the plurality of joints resulting from the joint motion using the one or more joint motion constraints that comprises: the redundant joint is to remain stationary between the HID pose and the target HID pose, the first joint is at a rotational joint position; or the second joint is less than the rotational joint position.

17. The surgical system of claim 16, wherein the memory comprises further instructions to determine the one or more secondary task joint motion constraints as output of a hand posture model responsive to input based on the pose of the HID.

18. The surgical system of claim 16, wherein the one or more joint motion constraints further comprises a joint motion constraint indicating that the second joint has a range of motion that includes joint positions that are greater than a negative rotational joint position and includes joint positions that are less than a positive rotational joint position.

19. The surgical system of claim 16, wherein the plurality of joints comprises a first set of joints and a second set of joints, wherein HID comprises a gimbal having an end user control and a first set of joints that includes the redundant joint, the first joint, and the second joint, and a support arm having a second set of joints, wherein the arm support is coupled to the gimbal via the redundant joint.Attorney Docket No.: AUR6374WOPCT3 Electronically Filed 20. The surgical system of claim 19, wherein the pose of the HID comprises a pose of the end user control, wherein the pose of the end user control converges with the pose of the tool pose responsive to the provided joint commands and while a hand of a user is holding the end user control.

21. A method comprising: determining a pose of a human interface device (HID) arranged to control a robotic arm, wherein the HID comprises a plurality of joints; determining a target pose of HID; performing an inverse kinematics (IK) process to determine joint commands for the plurality of joints to converge the pose of the HID into the target pose of the HID within a threshold while satisfying at least one nullspace motion constraint associated with one or more joints of the plurality of joints; and providing the joint commands to the plurality of joints.

22. The method of claim 21, wherein performing the IK process comprises: determining a pose error based on a difference between the target pose and the pose; and determining the joint commands as joint motions for the plurality of joints based on the pose error to cause the HID to move into the target pose.

23. Thet method of claim 22, wherein performing the IK process further comprises: scaling the pose error based on a magnitude of the pose error; determining an intermediate HID pose that is between the pose of the HID and the target pose of the HID based on the scaled pose error; and determining the joint motions for the plurality of joints as a primary task solution to an optimization problem to minimize the scaled pose error within the threshold and to determine a desired joint configuration of at least one of the joints that satisfies the at least one nullspace motion constraints.

24. The method of claim 23, wherein the optimization problem comprises a damped least squares problem, wherein determining the joint motions comprises:Attorney Docket No.: AUR6374WOPCT3 Electronically Filed iteratively solving the optimization problem until a new HID pose converges with the intermediate HID pose within at least one threshold by: determining an initial guess of joint positions of the plurality of joints based on the intermediate pose of the HID; determining a set of joint motions for the primary task solution and a set of nullspace joint motions using a damped least squares algorithm to minimize the scaled pose error starting at the initial guess of joint positions; and determining the new HID pose based on the set of joint motions and the set of nullspace joint motions.

25. The method of claim 22, wherein the HID comprises an end user control arranged to be held and manipulated by the user for controlling the robotic arm.

26. The method of claim 25, wherein the plurality of joints comprises three joints that are orthogonal with respect to each other that are coupled in series with the end user control to allow the control to move within three-dimensional (3D) space.

27. The method of claim 26, wherein the at least one nullspace motion constraint comprises: a first joint motion constraint of a first joint of the three joints being less than a rotational threshold; and a second joint motion constraint of a second joint of the three joints being at the rotational threshold.

28. The method of claim 27, wherein the plurality of joints comprises a redundant joint that is arranged to rotate about a same axis of the first joint, wherein the at least one nullspace motion constraint further comprises a third joint motion constraint of the redundant joint in which the redundant joint is to remain within a threshold from a previous joint position as the pose of the HID converges with the target pose of the HID.

Images