rby1_sdk.CartesianImpedanceControlCommandBuilder

class CartesianImpedanceControlCommandBuilder

Bases: pybind11_object

Cartesian impedance control command builder.

Takes Cartesian pose targets (in SE(3)) but executes control in joint space: first, an optimal control / inverse-kinematics problem finds joint targets \(q^\*\) that best realize the Cartesian objectives; then a joint-space impedance controller tracks \(q^\*\) using the stiffness/damping/torque limits you set.

Pipeline

  1. Cartesian objective (se(3) error): for each target transform \({}^{ref}\!T^{link}_d\), an error \(\xi = [\,\Delta x;\,\Delta\phi\,] \in \mathbb{R}^6\) is formed (translation/orientation in se(3)) and contributes to an optimal-control cost.

  2. Optimal control solve: solves for joint configuration \(q^\*\) (and, if needed, velocities/accelerations) that minimize a weighted sum of Cartesian errors and optional joint terms while respecting internal (scaled) velocity/acceleration limits and joint bounds.

  3. Joint-space impedance tracking: applies

    \[\tau \;=\; K \,(q^\* - q) \;+\; D \,(\dot{q}^\* - \dot{q}) \;+\; \tau_{ff},\]

    where \(K=\mathrm{diag}(k_1,\ldots,k_N)\) is set via set_joint_stiffness(...) (diagonal terms only), \(D\) is built from a damping ratio \(\zeta\) as

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

    with \(M\) the joint-space inertia, and \(\tau_{ff}\) is the gravity feed-forward torque. The final torque is saturated per-joint by set_joint_torque_limit(...):

    \[\tau_{\text{applied}} \;=\; \mathrm{clip}(\tau,\,-\tau_{\max},\,\tau_{\max}).\]

What each setting affects

  • add_target(...): defines Cartesian pose costs used only in the optimal-control solve that produces \(q^\*\).

  • set_joint_stiffness(...), set_joint_damping_ratio(...), set_joint_torque_limit(...): shape the joint-space impedance that tracks \(q^\*\) (they do not change the Cartesian cost; they change how \(q^\*\) is followed).

  • add_joint_position_target(...) and add_joint_limit(...): add joint-space costs/constraints to the optimal-control stage.

  • Velocity/acceleration limits passed to add_target(...) are used as internal bounds/scalings for the solver, not hardware limits.

Examples

>>> import numpy as np, rby1_sdk as rby
>>> T = np.eye(4); T[0,3] = 0.45; T[2,3] = 0.60   # desired wrist pose in base_link
>>> cmd = (
...   rby.CartesianImpedanceControlCommandBuilder()
...   .add_target("base_link", "right_wrist", T,
...               linear_velocity_limit=0.3,    # solver internal bounds
...               angular_velocity_limit=0.8)
...   .add_joint_position_target("right_arm_3", 0.5)  # joint hint in the OC stage
...   .set_joint_stiffness(np.full(7, 40.0))          # K diag for joint impedance
...   .set_joint_damping_ratio(0.7)                   # ζ → D
...   .set_joint_torque_limit(np.full(7, 35.0))       # τ saturation
...   .set_minimum_time(2.0)
... )
__init__(self: rby1_sdk.CartesianImpedanceControlCommandBuilder) → None

Construct a CartesianImpedanceControlCommandBuilder instance.

Methods

__init__(self)

Construct a CartesianImpedanceControlCommandBuilder instance.

add_joint_limit(self, joint_name, lower, upper)

Add a joint position limit.

add_joint_position_target(self, joint_name, ...)

Add an auxiliary joint position target.

add_target(self, ref_link_name, link_name, T)

Add a Cartesian impedance target.

set_command_header(self, command_header_builder)

Attach a command header.

set_joint_damping_ratio(self, damping_ratio)

Set joint damping ratio.

set_joint_stiffness(self, stiffness)

Set per-joint stiffness.

set_joint_torque_limit(self, torque_limit)

Set per-joint torque limits.

set_minimum_time(self, minimum_time)

Set minimum execution time.

set_nullspace_joint_target(self, ...[, k_p, ...])

Set nullspace joint target.

set_reset_reference(self, reset_reference)

Set whether to reset the reference pose.

set_stop_joint_position_tracking_error(self, ...)

Set stop threshold on joint position error.

set_stop_orientation_tracking_error(self, ...)

Set stop threshold on orientation error.

set_stop_position_tracking_error(self, ...)

Set stop threshold on translational error.