Articulation joint hardstop handling for surgical tool
The disclosed embodiments relate to systems and methods for a surgical tool or a surgical robotic system. One example system for handling hardstops includes one or more processors configured to calculate an articulation joint position for the articulation drive disk or the one or more corresponding rotary motors corresponding rotary motors, calculate an articulation joint torque for the articulation drive disk or the one or more corresponding rotary motors, determine a torque ratio based on the articulation joint position and the articulation joint torque, and adjust a commanded articulation joint position received from the user based on the torque ratio to compensate for collision involving the end effector.
1. An apparatus to compensate for collision between an end effector of a surgical tool and a hardstop, the apparatus comprising:
a tool driver connected by a shaft to the end effector, the tool driver having at least an articulation drive disk driven by one or more corresponding rotary motors, the articulation drive disk corresponding to an articulation motion of the end effector in a plane corresponding to a longitudinal axis of the shaft connecting the tool driver to the end effector; and
one or more processors configured to:
calculate an articulation joint position for the articulation drive disk or the one or more corresponding rotary motors;
calculate an articulation joint torque for the articulation drive disk or the one or more corresponding rotary motors;
determine a torque ratio based on the articulation joint position and the articulation joint torque; and
calculate an articulation joint adjustment, for the articulation drive disk or the one or more corresponding rotary motors, in response to the torque ratio to compensate for a hardstop.
2. The apparatus of claim 1 , the one or more processors configured to:
adjust a user command using the articulation joint adjustment; and
provide the adjusted user command to the tool driver.
3. The apparatus of claim 1 , wherein the articulation joint position is calculated based on a relationship between at least one measured motor position for the one or more corresponding rotary motors and at least one property constant for the surgical tool.
4. The apparatus of claim 3 , wherein the relationship for the joint position (θ j ) is provided by:
θ
j
=
sin
-
1
a
+
1
2
(
θ
m
1
+
θ
m
2
)
b
,
wherein the at least one measured motor position includes a first motor position (θ m1 ) for a first motor of the one or more corresponding rotary motors and a second motor position (θ m2 ) for a second motor of the one or more corresponding rotary motors, and the at least one property constant includes first constant a and second constant b.
5. The apparatus of claim 1 , the articulation joint torque is calculated based on a relationship at least one measured torque for the one or more corresponding rotary motors and at least one property constant for the surgical tool.
6. The apparatus of claim 5 , wherein the relationship for the joint torque (τ j ) is provided by:
τ
j
=
b
a
(
τ
m
1
+
τ
m
2
)
*
1
-
a
b
(
τ
m
1
+
τ
m
2
)
2
,
wherein the at least one measured torque includes a first torque (τ m1 ) for a first motor of the one or more corresponding rotary motors and a second torque (τ m2 ) for a second motor of the one or more corresponding rotary motors, and the at least one property constant includes first constant a and second constant b.
7. The apparatus of claim 1 , wherein the relationship for the joint torque (τ j ) is based on static force analysis.
8. The apparatus of claim 1 , wherein the torque ratio is determined based on at least one comparison between the joint position and a position threshold or at least one comparison between the joint torque and a torque threshold.
9. The apparatus of claim 1 , the one or more processors configured to:
calculate a joint position percentage (Δθ) when the joint position is within a compensation range bounded by a minimum value (θ jmin ) and a maximum value (θ jmax ) and outside of an out of bounds range bounded by a minimum limit value (θ jmin,limit ) and a maximum limit value (θ jmax,limit ).
10. The apparatus of claim 9 , wherein the joint position percentage (Δθ) is:
θ
jmin
-
θ
j
(
θ
jmin
-
θ
jmin
,
limit
when
θ
j
<
θ
jmin
or
θ
j
-
θ
jmax
(
θ
jmax
,
limit
-
θ
jmax
when
θ
j
>
θ
jmax
.
11. The apparatus of claim 10 , wherein the torque ratio (r τ ) is determined based on
r
τ
=
sign
(
τ
j
)
*
1
-
(
c
*
Δθ
-
1
)
2
*
1
-
(
❘
"\[LeftBracketingBar]"
τ
joint
❘
"\[RightBracketingBar]"
-
τ
jnormal
❘
"\[LeftBracketingBar]"
τ
joint
❘
"\[RightBracketingBar]"
-
τ
jnormal
-
1
)
2
,
wherein a constant c is a predetermined value.
12. The apparatus of claim 11 , wherein c is set to zero when the joint position percentage (Δθ) is below a percentage threshold.
13. A method to compensate for collision between an end effector of a surgical tool and a hardstop, the method comprising:
calculating an articulation joint position for an articulation drive disk or one or more corresponding rotary motors corresponding rotary motors for an articulation motion of the end effector in a plane corresponding to a longitudinal axis of a shaft connecting the end effector;
calculating an articulation joint torque for the articulation drive disk or the one or more corresponding rotary motors corresponding rotary motors;
determining a torque ratio based on the articulation joint position and the articulation joint torque; and
calculating an articulation joint adjustment in response to the torque ratio to compensate for a hardstop.
14. The method of claim 13 , further comprising:
adjusting a user command using the articulation joint adjustment; and
providing the adjusted user command to the articulation drive disk.
15. The method of claim 13 , wherein the articulation joint position is calculated based on a relationship between at least one measured motor position for the one or more corresponding rotary motors and at least one property constant for the surgical tool.
16. The method of claim 13 , the articulation joint torque is calculated based on a relationship at least one measured torque for the one or more corresponding rotary motors and at least one property constant for the surgical tool.
17. The method of claim 13 , further comprising:
calculating a joint position percentage (Δθ) when the joint position is inside a compensation range bounded by a minimum value (θ jmin ) and a maximum value (θ jmax ) and outside of an out of bounds range bounded by a minimum limit value (θ jmin,limit ) and a maximum limit value (θ jmax,limit ).
18. The method of claim 17 , wherein the joint position percentage (Δθ) is:
θ
jmin
-
θ
j
(
θ
jmin
-
θ
jmin
,
limit
when
θ
j
<
θ
jmin
or
θ
j
-
θ
jmax
(
θ
jmax
,
limit
-
θ
jmax
when
θ
j
>
θ
jmax
.
19. The method of claim 18 , wherein the torque ratio (r τ ) is determined based on
r
τ
=
sign
(
τ
j
)
*
1
-
(
c
*
Δθ
-
1
)
2
*
1
-
(
❘
"\[LeftBracketingBar]"
τ
joint
❘
"\[RightBracketingBar]"
-
τ
jnormal
❘
"\[LeftBracketingBar]"
τ
joint
❘
"\[RightBracketingBar]"
-
τ
jnormal
-
1
)
2
,
wherein a constant c is a predetermined value.
20. An apparatus to compensate for collision between an end effector of a surgical tool and a hardstop, the apparatus comprising:
a tool driver connected by a shaft to the end effector, the tool driver having a first articulation drive disk driven by a first motor and a second articulation disk driven by a second motor, wherein the first articulation drive disk and the second articulation disk cooperate to move the end effector in a plane corresponding to a longitudinal axis of the shaft connecting the tool driver to the end effector; and
one or more processors configured to:
calculate an articulation joint position for the first motor or the second motor;
calculate an articulation joint torque for the first motor or the second motor;
determine a torque ratio based on the articulation joint position and the articulation joint torque; and
adjust a commanded articulation joint position received from a user based on the torque ratio to compensate for collision involving the end effector.