rby1_sdk.OptimalControlCommandBuilder#

class OptimalControlCommandBuilder#

Bases: pybind11_object

Optimal control command builder.

Constructs an optimal control objective composed of Cartesian pose costs, center-of-mass (COM) costs, and joint position costs, with global error and limit scalings.

Notes

  • The overall objective can be viewed as a weighted least-squares form:

    \[J \;=\; \sum_{i} w_t^{(i)} \,\|\, p^{(i)}_d - p^{(i)} \,\|^2 \;+\; \sum_{i} w_r^{(i)} \,\|\, r^{(i)}_d \ominus r^{(i)} \,\|^2 \;+\; w_{\mathrm{com}} \,\|\, c_d - c \,\|^2 \;+\; \sum_j w_q^{(j)} \,(q^{(j)}_d - q^{(j)})^2\]

    where \(p, r\) denote translation and rotation of a link in a reference frame, \(c\) is the COM position, and \(q\) are joint positions.

  • Velocity/acceleration limit scalings multiply internal limits used by the optimizer; they do not redefine hardware limits.

Examples

Set a right-wrist Cartesian target and a joint hint, with moderate limit scalings:

>>> import numpy as np, rby1_sdk as rby
>>> T = np.eye(4); T[0,3] = 0.35; T[2,3] = 0.55
>>> oc = (
...   rby.OptimalControlCommandBuilder()
...   .add_cartesian_target("base_link", "ee_right", T, translation_weight=1.0, rotation_weight=0.5)
...   .add_joint_position_target("right_arm_4", 0.4, weight=0.2)
...   .set_velocity_limit_scaling(0.8)
...   .set_acceleration_limit_scaling(0.6)
...   .set_error_scaling(1.0)
... )
__init__(self: rby1_sdk.OptimalControlCommandBuilder) → None#

Methods

__init__(self)

add_cartesian_target(self, ref_link_name, ...)

Add a Cartesian pose target for a link.

add_joint_position_target(self, joint_name, ...)

Add a joint position target.

set_acceleration_limit_scaling(self, ...)

Scale internal joint acceleration limits used by the optimizer.

set_center_of_mass_target(self, ...)

Set a center-of-mass (COM) position target.

set_command_header(self, command_header_builder)

Attach a command header to this builder.

set_error_scaling(self, error_scaling)

Set a global error scaling applied to cost terms.

set_min_delta_cost(self, min_delta_cost)

Set the minimum required improvement in cost between iterations.

set_patience(self, patience)

Set the iteration patience before stopping on small improvements.

set_stop_cost(self, stop_cost)

Set the absolute objective threshold to consider the problem "solved".

set_velocity_limit_scaling(self, ...)

Scale internal joint velocity limits used by the optimizer.
add_cartesian_target(self: rby1_sdk.OptimalControlCommandBuilder, ref_link_name: str, link_name: str, T: numpy.ndarray[numpy.float64[4, 4]], translation_weight: float, rotation_weight: float) → rby1_sdk.OptimalControlCommandBuilder#

Add a Cartesian pose target for a link.

Parameters:

  • ref_link_name (str) – Reference frame (link) name in which the target pose is expressed.

  • link_name (str) – Controlled link name.

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

  • translation_weight (float) – Weight \(w_t\) for translational error.

  • rotation_weight (float) – Weight \(w_r\) for rotational error.

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • The cost terms are typically of the form

    \[J_t = w_t \, \|\, p_d - p \,\|^2, \qquad J_r = w_r \, \|\, r_d \ominus r \,\|^2,\]

    where \(p\) is position and \(r\) is orientation (e.g., axis-angle error) of link_name measured in ref_link_name.

add_joint_position_target(self: rby1_sdk.OptimalControlCommandBuilder, joint_name: str, target_position: float, weight: float) → rby1_sdk.OptimalControlCommandBuilder#

Add a joint position target.

Parameters:

  • joint_name (str) – Joint name.

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

  • weight (float) – Weight \(w_q\) for this joint’s position error.

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • The joint cost term is:

    \[J_q \;=\; w_q \,(q_d - q)^2 .\]
set_acceleration_limit_scaling(self: rby1_sdk.OptimalControlCommandBuilder, acceleration_limit_scaling: float) → rby1_sdk.OptimalControlCommandBuilder#

Scale internal joint acceleration limits used by the optimizer.

Parameters:

acceleration_limit_scaling (float) – Scaling factor \(\alpha_a > 0\). The internal acceleration limits become \(\alpha_a \cdot a_{\max}\).

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • Like velocity scaling, this only affects internal optimization constraints.

set_center_of_mass_target(self: rby1_sdk.OptimalControlCommandBuilder, ref_link_name: str, pose: numpy.ndarray[numpy.float64[3, 1]], weight: float) → rby1_sdk.OptimalControlCommandBuilder#

Set a center-of-mass (COM) position target.

Parameters:

  • ref_link_name (str) – Reference frame (link) name for the COM target.

  • pose (numpy.ndarray, shape (3,), dtype=float64) – Desired COM position \(c_d = [x,y,z]^T\) in meters, expressed in ref_link_name.

  • weight (float) – Weight \(w_{\mathrm{com}}\) for COM position error.

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • The COM cost is:

    \[J_{\mathrm{com}} \;=\; w_{\mathrm{com}} \,\|\, c_d - c \,\|^2 .\]
set_command_header(self: rby1_sdk.OptimalControlCommandBuilder, command_header_builder: rby1_sdk.CommandHeaderBuilder) → rby1_sdk.OptimalControlCommandBuilder#

Attach a command header to this builder.

Parameters:

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

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

set_error_scaling(self: rby1_sdk.OptimalControlCommandBuilder, error_scaling: float) → rby1_sdk.OptimalControlCommandBuilder#

Set a global error scaling applied to cost terms.

Parameters:

error_scaling (float) – A positive scalar multiplying error magnitudes internally (e.g., to balance different unit scales).

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • Conceptually, this scales error vectors before squaring, e.g., \(e \leftarrow \alpha e\), which yields \(\alpha^2\) scaling on quadratic costs.

set_min_delta_cost(self: rby1_sdk.OptimalControlCommandBuilder, min_delta_cost: float) → rby1_sdk.OptimalControlCommandBuilder#

Set the minimum required improvement in cost between iterations.

Parameters:

min_delta_cost (float) – If the reduction \(\Delta J\) between iterations drops below this value, the optimizer may stop (convergence).

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

set_patience(self: rby1_sdk.OptimalControlCommandBuilder, patience: int) → rby1_sdk.OptimalControlCommandBuilder#

Set the iteration patience before stopping on small improvements.

Parameters:

patience (int) – Number of iterations to tolerate without sufficient cost improvement (in combination with set_min_delta_cost).

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

set_stop_cost(self: rby1_sdk.OptimalControlCommandBuilder, stop_cost: float) → rby1_sdk.OptimalControlCommandBuilder#

Set the absolute objective threshold to consider the problem “solved”.

Parameters:

stop_cost (float) – If the total cost \(J\) falls below this value, the optimizer may terminate early.

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

set_velocity_limit_scaling(self: rby1_sdk.OptimalControlCommandBuilder, velocity_limit_scaling: float) → rby1_sdk.OptimalControlCommandBuilder#

Scale internal joint velocity limits used by the optimizer.

Parameters:

velocity_limit_scaling (float) – Scaling factor \(\alpha_v > 0\). The internal velocity limits become \(\alpha_v \cdot v_{\max}\).

Returns:

Self reference for method chaining.

Return type:

OptimalControlCommandBuilder

Notes

  • This does not change hardware limits; it affects only the optimization’s internal constraints and trajectory shaping.