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.
add_joint_limit(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, joint_name: str, lower: float, upper: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Add a joint position limit.

Parameters:

  • joint_name (str) – Joint name.

  • lower (float) – Lower limit [rad].

  • upper (float) – Upper limit [rad].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

add_joint_position_target(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, joint_name: str, target_position: float, velocity_limit: float | None = None, acceleration_limit: float | None = None) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Add an auxiliary joint position target.

Parameters:

  • joint_name (str) – Joint name.

  • target_position (float) – Desired position [rad].

  • velocity_limit (Optional[float]) – Max velocity [rad/s].

  • acceleration_limit (Optional[float]) – Max acceleration [rad/s²].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

add_target(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, ref_link_name: str, link_name: str, T: numpy.ndarray[numpy.float64[4, 4]], linear_velocity_limit: float | None = None, angular_velocity_limit: float | None = None, linear_acceleration_limit: float | None = None, angular_acceleration_limit: float | None = None) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Add a Cartesian impedance target.

Parameters:

  • ref_link_name (str) – Reference frame (link) name.

  • link_name (str) – Controlled link name (end-effector).

  • T (numpy.ndarray, shape (4,4), dtype=float64) – Desired homogeneous transform \({}^{ref}\!T^{link}_d\).

  • linear_velocity_limit (Optional[float]) – Max translational velocity [m/s].

  • angular_velocity_limit (Optional[float]) – Max angular velocity [rad/s].

  • linear_acceleration_limit (Optional[float]) – Max translational acceleration [m/s²].

  • angular_acceleration_limit (Optional[float]) – Max angular acceleration [rad/s²].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_command_header(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, command_header_builder: rby1_sdk.CommandHeaderBuilder) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Attach a command header.

Parameters:

command_header_builder (CommandHeaderBuilder) – Command header configuration (e.g., control hold time).

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_joint_damping_ratio(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, damping_ratio: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set joint damping ratio.

Parameters:

damping_ratio (float) – Dimensionless damping ratio ζ.

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_joint_stiffness(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, stiffness: numpy.ndarray[numpy.float64[m, 1]]) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set per-joint stiffness.

Parameters:

stiffness (numpy.ndarray, shape (N,), dtype=float64) – Joint stiffness values (Nm/rad).

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_joint_torque_limit(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, torque_limit: numpy.ndarray[numpy.float64[m, 1]]) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set per-joint torque limits.

Parameters:

torque_limit (numpy.ndarray, shape (N,), dtype=float64) – Max torques [Nm].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_minimum_time(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, minimum_time: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set minimum execution time.

Parameters:

minimum_time (float) – Minimum time [s] to execute trajectory.

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_nullspace_joint_target(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, target_position: numpy.ndarray[numpy.float64[m, 1]], weight: numpy.ndarray[numpy.float64[m, 1]], k_p: float | None = 0.2, k_d: float | None = 0.2, cost_weight: float | None = 0.001) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set nullspace joint target.

Parameters:

  • target_position (numpy.ndarray, shape (N,), dtype=float64) – Desired position [rad].

  • weight (numpy.ndarray, shape (N,), dtype=float64) – Weight for the nullspace term.

  • k_p (float, optional, default=0.2) – Proportional gain for the nullspace term.

  • k_d (float, optional, default=0.2) – Derivative gain for the nullspace term.

  • cost_weight (float, optional, default=1e-3) – Cost weight for the nullspace term.

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_reset_reference(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, reset_reference: bool) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set whether to reset the reference pose.

Parameters:

reset_reference (bool) – If True, resets the reference configuration before applying new targets.

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_stop_joint_position_tracking_error(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, stop_joint_position_tracking_error: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set stop threshold on joint position error.

Parameters:

stop_joint_position_tracking_error (float) – Threshold [rad].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_stop_orientation_tracking_error(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, stop_orientation_tracking_error: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set stop threshold on orientation error.

Parameters:

stop_orientation_tracking_error (float) – Threshold [rad].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder

set_stop_position_tracking_error(self: rby1_sdk.CartesianImpedanceControlCommandBuilder, stop_position_tracking_error: float) → rby1_sdk.CartesianImpedanceControlCommandBuilder#

Set stop threshold on translational error.

Parameters:

stop_position_tracking_error (float) – Threshold [m].

Returns:

Self reference for method chaining.

Return type:

CartesianImpedanceControlCommandBuilder