sas
Modularised monitoring, logging, and control of robots.
Loading...
Searching...
No Matches
core.papers.icra2019.controller.Controller Class Reference

Public Member Functions

None __init__ (self, DQ_SerialManipulator kinematics, float gain, float damping, float alpha, Sequence[tuple[DQ, float, int]] rcm_constraints, float vfi_gain=2.0, **kwargs)
DQ get_last_robot_pose (self)
np.ndarray|None get_last_error (self)
np.ndarray compute_setpoint_control_signal (self, np.ndarray q, DQ xd)

Static Public Member Functions

tuple[np.ndarray, np.ndarray] get_rcm_constraint (np.ndarray Jx, DQ x, DQ primitive, DQ p, float d_safe, float eta_d)

Public Attributes

 qp_solver = Solver()
DQ_SerialManipulator kinematics = kinematics
float gain = gain
float damping = damping
float alpha = alpha
Sequence[tuple[DQ, float, int]] rcm_constraints = rcm_constraints
float vfi_gain = vfi_gain
np.ndarray er_accumulated = np.zeros((4,))
np.ndarray et_accumulated = np.zeros((4,))
float integral_gain = 0.0
 verbose = normalize_verbose(kwargs.get("verbose", False))
DQ|None last_x = None
np.ndarray|None last_Jx = None
np.ndarray|None last_error = None

Protected Member Functions

tuple[np.ndarray, np.ndarray, np.ndarray|None, np.ndarray|None] _get_optimization_parameters (self, np.ndarray q, DQ xd)

Static Protected Member Functions

np.ndarray _get_rotation_error (DQ x, DQ xd)

Detailed Description

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

Constructor & Destructor Documentation

◆ __init__()

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}``).

Member Function Documentation

◆ _get_optimization_parameters()

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``).

◆ _get_rotation_error()

np.ndarray core.papers.icra2019.controller.Controller._get_rotation_error ( DQ x,
DQ xd )
staticprotected
Return the 4-component rotation error between two poses.

Uses the dual-quaternion invariant ``conj(r_x) * r_xd``: the error
that is closer to the identity (i.e. whose dual part is smaller in
norm) is selected, avoiding the 180° ambiguity.

Args:
    x: Current end-effector pose.
    xd: Desired end-effector pose.

Returns:
    The 4-component rotation error vector.

◆ compute_setpoint_control_signal()

np.ndarray core.papers.icra2019.controller.Controller.compute_setpoint_control_signal ( self,
np.ndarray q,
DQ xd )
Compute the control signal for the next step.

Solves the constrained quadratic program built by
:meth:`_get_optimization_parameters` for the joint velocity that
drives the end effector toward the desired pose.

Args:
    q: The current joint positions (vector of size ``DOF``).
    xd: The desired end-effector pose (a unit dual quaternion).

Returns:
    The joint-velocity vector ``u`` to be applied for this step.

Raises:
    Exception: If ``xd`` is not a unit dual quaternion.

◆ get_last_error()

np.ndarray | None core.papers.icra2019.controller.Controller.get_last_error ( self)
Return the task error of the last control step, if any.

:return: The stacked translation/rotation error vector of the last
    control step, or ``None`` before the first step.

◆ get_last_robot_pose()

DQ core.papers.icra2019.controller.Controller.get_last_robot_pose ( self)
Retrieves the last recorded position of the robot.

This method returns the most recently recorded pose of the end-effector of the robot.

:return: The last recorded x-axis position of the robot.
:rtype: DQ

◆ get_rcm_constraint()

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.

The documentation for this class was generated from the following file:
  • sas_py/marinholab/sas/core/papers/icra2019/controller.py