rby1_sdk.ImpedanceControlCommandBuilder

class ImpedanceControlCommandBuilder

Bases: pybind11_object

Cartesian impedance control command builder.

Builds a Cartesian impedance command for a given link in a reference frame. The control is formulated in the se(3) space with a 6D error \(\xi = [\,\Delta x;\,\Delta\phi\,] \in \mathbb{R}^6\) (translation and rotation in the Lie algebra), yielding the wrench:

\[w = K\,\xi \;+\; D\,\dot{\xi}\]

where \(K \in \mathbb{R}^{6\times 6}\) is the stiffness matrix and \(D \in \mathbb{R}^{6\times 6}\) is the damping matrix.

Notes

  • The stiffness matrix \(K\) is assumed block-diagonal:

    \[K \;=\; \mathrm{diag}(K_t,\; K_r), \qquad K_t, K_r \in \mathbb{R}^{3\times 3}\ \text{(diagonal)}\]

    set_translation_weight([k_x,k_y,k_z]) fills \(K_t=\mathrm{diag}(k_x,k_y,k_z)\), and set_rotation_weight([k_{rx},k_{ry},k_{rz}]) fills \(K_r=\mathrm{diag}(k_{rx},k_{ry},k_{rz})\).

  • Damping is derived from a dimensionless ratio \(\zeta\):

    \[D \;=\; \zeta \left(\sqrt{M}\sqrt{K} \;+\; \sqrt{K}\sqrt{M}\right),\]

    with \(M \in \mathbb{R}^{6\times 6}\) the Cartesian inertia matrix.

  • set_reference_link_name and set_link_name must be set before applying the target pose (set_transformation).

Examples

>>> import numpy as np, rby1_sdk as rby
>>> T = np.eye(4); T[0,3] = 0.30  # desired pose, 30 cm forward
>>> cmd = (
...   rby.ImpedanceControlCommandBuilder()
...   .set_reference_link_name("base_link")
...   .set_link_name("right_wrist")
...   .set_transformation(T)
...   .set_translation_weight(np.array([1200.0, 1200.0, 1600.0]))  # N/m
...   .set_rotation_weight(np.array([60.0, 60.0, 60.0]))           # Nm/rad
...   .set_damping_ratio(0.7)
... )
__init__(self: rby1_sdk.ImpedanceControlCommandBuilder) → None

Methods

__init__(self)

set_command_header(self, command_header_builder)

Attach a command header.

set_damping_ratio(self, damping_ratio)

Set the (dimensionless) damping ratio used to build the 6×6 damping matrix.

set_link_name(self, link_name)

Set the controlled link name.

set_reference_link_name(self, ...)

Set the reference frame (link) name in which the pose is expressed.

set_rotation_weight(self, weight)

Set rotational stiffness (fills the rotational block of K).

set_transformation(self, T)

Set the desired Cartesian pose (homogeneous transform).

set_translation_weight(self, weight)

Set translational stiffness (fills the translational block of K).