An implementation of the task-space controller described in:
"A Unified Framework for the Teleoperation of Surgical Robots in Constrained Workspaces".
Marinho, M. M; et al.
In 2019 IEEE International Conference on Robotics and Automation (ICRA), pages 2721–2727, May 2019. IEEE
http://doi.org/10.1109/ICRA.2019.8794363
| None core.papers.icra2019.controller.Controller.__init__ |
( |
| self, |
|
|
DQ_SerialManipulator | kinematics, |
|
|
float | gain, |
|
|
float | damping, |
|
|
float | alpha, |
|
|
Sequence[tuple[DQ, float, int]] | rcm_constraints, |
|
|
float | vfi_gain = 2.0, |
|
|
** | kwargs ) |
Initialize the controller.
:param kinematics: A suitable DQ_SerialManipulator object.
:param gain: A positive float. Controller proportional gain.
:param damping: A positive float. Damping factor.
:param alpha: A float between 0 and 1. Soft priority between translation and rotation.
:param rcm_constraints: A list of tuples (p, r, ith), where p is the position of the constraint as a pure quaternion
r is the radius of the constraint, and ith is the index of the joint this constraint relates to.
:param vfi_gain: Violation Field Indicator gain applied to the RCM constraints.
:param kwargs: Optional keyword arguments. Recognized keys: ``verbose``
(bool or dict) — per-category debug output; ``True`` prints
every category, ``False`` prints nothing, and a dict selects
categories by name (e.g. ``{"rcm": True}``).
| tuple[np.ndarray, np.ndarray, np.ndarray | None, np.ndarray | None] core.papers.icra2019.controller.Controller._get_optimization_parameters |
( |
| self, |
|
|
np.ndarray | q, |
|
|
DQ | xd ) |
|
protected |
Assemble the QP cost (``H``, ``f``) and inequality constraints (``W``, ``w``).
Computes the soft-priority blend of the translation and rotation
task-space costs plus the damping term, and stacks the joint-limit
and (if any) RCM inequality constraints. Also records the end
effector pose, its pose Jacobian, and the task error for later use.
Args:
q: Current joint configuration vector.
xd: Desired end-effector pose (a unit dual quaternion).
Returns:
A 4-tuple ``(H, f, W, w)`` where ``H`` and ``f`` are the QP cost
matrix and linear term, and ``W``/``w`` are the stacked
inequality matrix and right-hand side (``None`` when only the
always-present joint-limit rows exist, i.e. ``W == W_jl``).
| tuple[np.ndarray, np.ndarray] core.papers.icra2019.controller.Controller.get_rcm_constraint |
( |
np.ndarray | Jx, |
|
|
DQ | x, |
|
|
DQ | primitive, |
|
|
DQ | p, |
|
|
float | d_safe, |
|
|
float | eta_d ) |
|
static |
This static method computes the Remote Centre of Motion (RCM) constraint
for the end-effector represented by x and its Jacobian Jx. It calculates the
inequality matrix and vector that ensure the minimum safe squared distance (d_safe) between a line
and a point is maintained during motion.
:param Jx: The pose Jacobian of the robot.
:param x: The current pose of the end-effector, compatible with Jx.
:param primitive: The primitive in the end-effector in which the line is spanned. For instance i_, j_, or k_.
:param p: The centre of the RCM constraint, represented as a pure quaternion.
:param d_safe: The safe distance (float) to maintain between the line and the
point, squared internally in the calculation.
:param eta_d: VFI gain.
:return: A tuple containing:
- W (np.array): The inequality constraint matrix derived from the line-to-point
distance Jacobian.
- w (np.array): The inequality constraint vector determined by the distance
error and safety distance.