rby1_sdk.CartesianCommandBuilder¶
- class CartesianCommandBuilder¶
Bases:
pybind11_objectCartesian command builder.
Builds end-effector Cartesian pose targets and optional per-joint auxiliary targets. Each Cartesian target specifies a desired homogeneous transform \({}^{ref}\!T^{link}_d \in SE(3)\) together with velocity/acceleration constraints used to time the motion.
Notes
Cartesian error is computed in the reference frame and tracked subject to velocity/acceleration limits provided per target.
Joint targets added via
add_joint_position_targetare auxiliary hints that can help disambiguate IK or bias the posture during Cartesian tracking.
Examples
>>> import numpy as np, rby1_sdk as rby >>> T = np.eye(4); T[0,3] = 0.40; T[2,3] = 0.60 # 40 cm forward, 60 cm up >>> cart = ( ... rby.CartesianCommandBuilder() ... .add_target("base", "link_torso_5", T, ... linear_velocity_limit=0.4, # [m/s] ... angular_velocity_limit=1.0, # [rad/s] ... acceleration_limit_scaling=0.6) ... .add_joint_position_target("right_arm_3", 0.3, velocity_limit=1.2) ... .set_minimum_time(2.0) ... )
- __init__(self: rby1_sdk.CartesianCommandBuilder) None¶
Construct a CartesianCommandBuilder instance.
Methods
__init__(self)Construct a CartesianCommandBuilder instance.
add_joint_position_target(self, joint_name, ...)Add an auxiliary joint position target.
add_target(self, ref_link_name, link_name, ...)Add a Cartesian pose target for a link.
set_command_header(self, command_header_builder)Attach a command header.
set_minimum_time(self, minimum_time)Set the minimum execution time for the motion.
set_stop_joint_position_tracking_error(self, ...)Set the stop threshold on joint-space position tracking error.
set_stop_orientation_tracking_error(self, ...)Set the stop threshold on rotational tracking error.
set_stop_position_tracking_error(self, ...)Set the stop threshold on translational tracking error.