optimal_ik ========== .. py:module:: optimal_ik Submodules ---------- .. toctree:: :maxdepth: 1 /autoapi/optimal_ik/_optimal_ik_ext/index Classes ------- .. autoapisummary:: optimal_ik.Task optimal_ik.FrameTaskOptions optimal_ik.FrameTask optimal_ik.ConfigurationTaskOptions optimal_ik.ConfigurationTask optimal_ik.Constraints optimal_ik.PositionLimit optimal_ik.VelocityLimit optimal_ik.Barrier optimal_ik.ConstraintAxisSelection optimal_ik.PositionBarrier optimal_ik.SelfCollisionBarrier optimal_ik.Oink Package Contents ---------------- .. py:class:: Task Abstract base class for IK tasks. .. py:property:: gain :type: float Task gain for low-pass filtering. .. py:property:: weight :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, None), order='F')] Weight matrix for cost normalization. .. py:property:: lm_damping :type: float Levenberg-Marquardt damping. .. py:property:: priority :type: int Priority level (1 = highest; lower priorities are projected into the nullspace of higher priorities). .. py:property:: num_variables :type: int Number of optimization variables. .. py:class:: FrameTaskOptions(position_cost = 1.0, orientation_cost = 1.0, task_gain = 1.0, lm_damping = 0.0, max_position_error = float('inf'), max_rotation_error = float('inf'), priority = 1) Parameters for FrameTask. .. py:property:: position_cost :type: float Position cost weight. .. py:property:: orientation_cost :type: float Orientation cost weight. .. py:property:: task_gain :type: float Task gain for low-pass filtering. .. py:property:: lm_damping :type: float Levenberg-Marquardt damping. .. py:property:: max_position_error :type: float Maximum position error magnitude (meters). Infinite = no limit. .. py:property:: max_rotation_error :type: float Maximum rotation error magnitude (radians). Infinite = no limit. .. py:property:: priority :type: int Priority level (1 = highest). Tasks at higher priority numbers are projected into the nullspace of lower priority numbers. .. py:class:: FrameTask(oink, scene, target_pose, options = ...) Bases: :py:obj:`Task` Task to reach a target pose for a specified frame. .. py:property:: frame_name :type: str Name of the frame to control. .. py:property:: frame_id :type: int Index of the frame in the scene's Pinocchio model. .. py:property:: v_indices :type: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order='C')] Velocity vector indices for the joint group. .. py:property:: target_pose :type: roboplan.core._core_ext.CartesianConfiguration Target pose for the frame. .. py:property:: max_position_error :type: float Maximum position error magnitude (meters). .. py:property:: max_rotation_error :type: float Maximum rotation error magnitude (radians). .. py:method:: setTargetFrameTransform(tform) Sets the target transform for this frame task. .. py:class:: ConfigurationTaskOptions(task_gain = 1.0, lm_damping = 0.0, priority = 1) Parameters for ConfigurationTask. .. py:property:: task_gain :type: float Task gain for low-pass filtering. .. py:property:: lm_damping :type: float Levenberg-Marquardt damping. .. py:property:: priority :type: int Priority level (1 = highest). Tasks at higher priority numbers are projected into the nullspace of lower priority numbers. .. py:class:: ConfigurationTask(oink, target_q, joint_weights, options = ...) Bases: :py:obj:`Task` Task to reach a target joint configuration. .. py:property:: target_q :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')] Target joint configuration. .. py:property:: joint_weights :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')] Weights for each joint in the configuration task. .. py:class:: Constraints Abstract base class for IK constraints. .. py:class:: PositionLimit(oink, gain = 1.0) Bases: :py:obj:`Constraints` Constraint to enforce joint position limits. .. py:property:: config_limit_gain :type: float Gain for position limit enforcement. .. py:class:: VelocityLimit(oink, dt, v_max) Bases: :py:obj:`Constraints` Constraint to enforce joint velocity limits. .. py:property:: dt :type: float Time step for velocity calculation. .. py:property:: v_max :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ), order='C')] Maximum joint velocities. .. py:class:: Barrier Abstract base class for Control Barrier Functions. .. py:method:: get_num_barriers(scene) Get the number of barrier constraints. .. py:property:: gain :type: float Barrier gain (gamma). .. py:property:: dt :type: float Timestep. .. py:property:: safe_displacement_gain :type: float Gain for safe displacement regularization. .. py:property:: safety_margin :type: float Conservative margin for hard constraints. .. py:class:: ConstraintAxisSelection(x = True, y = True, z = True) Axis selection for position barrier constraints. .. py:property:: x :type: bool Constrain X axis. .. py:property:: y :type: bool Constrain Y axis. .. py:property:: z :type: bool Constrain Z axis. .. py:class:: PositionBarrier(oink, scene, frame_name, p_min, p_max, dt, axis_selection = ..., gain = 1.0, safe_displacement_gain = 1.0, safety_margin = 0.0) Bases: :py:obj:`Barrier` Position barrier constraint that keeps a frame within an axis-aligned bounding box. .. py:method:: get_frame_position(scene) Get the current frame position in world coordinates. .. py:property:: frame_name :type: str Name of the constrained frame. .. py:property:: axis_selection :type: ConstraintAxisSelection Axis selection for constraints. .. py:property:: p_min :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')] Minimum position bounds. .. py:property:: p_max :type: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=3, order='C')] Maximum position bounds. .. py:class:: SelfCollisionBarrier(oink, scene, n_collision_pairs, dt, gain = 1.0, safe_displacement_gain = 1.0, d_min = 0.02, safety_margin = 0.0) Bases: :py:obj:`Barrier` Self-collision avoidance barrier based on hpp-fcl / coal collision pair distances. Constrains the closest `n_collision_pairs` collision pairs in the scene to remain at least `d_min` apart. Inspired by pink.barriers.SelfCollisionBarrier. .. py:property:: n_collision_pairs :type: int Number of closest collision pairs to constrain. .. py:property:: d_min :type: float Minimum allowed distance between any pair of bodies. .. py:class:: Oink(scene: roboplan.core._core_ext.Scene, group_name: str) Oink(scene) Optimal Inverse Kinematics solver. .. py:property:: num_variables :type: int Number of optimization variables. .. py:property:: q_indices :type: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order='C')] Position indices of the joint group. .. py:property:: v_indices :type: Annotated[numpy.typing.NDArray[numpy.int32], dict(shape=(None, ), order='C')] Velocity indices of the joint group. .. py:method:: solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], constraints: collections.abc.Sequence[Constraints], barriers: collections.abc.Sequence[Barrier], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ))], regularization: float = 1e-12) -> None solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ))], regularization: float = 1e-12) -> None solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], constraints: collections.abc.Sequence[Constraints], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ))], regularization: float = 1e-12) -> None solveIk(scene: roboplan.core._core_ext.Scene, tasks: collections.abc.Sequence[Task], barriers: collections.abc.Sequence[Barrier], delta_q: Annotated[numpy.typing.NDArray[numpy.float64], dict(shape=(None, ))], regularization: float = 1e-12) -> None Solve inverse kinematics for tasks with barriers (no constraints). :param tasks: List of weighted tasks to optimize for. :param barriers: List of barrier functions for safety constraints. :param delta_q: Pre-allocated numpy array for output (size = num_variables). :param regularization: Tikhonov regularization weight (default: 1e-12). .. rubric:: Example delta_q = np.zeros(oink.num_variables) oink.solveIk(scene, tasks, barriers, delta_q) .. py:method:: enforceBarriers(scene, barriers, delta_q, tolerance = 0.0) Validate delta_q against barriers using forward kinematics. This method provides a post-solve safety check by evaluating the actual barrier values at the candidate configuration (q + delta_q). If any barrier would be violated, delta_q is set to zero to prevent unsafe motion. This is a backup safety mechanism for cases where the linearized CBF constraint in the QP has significant error (e.g., large jumps, near-boundary configurations). :param barriers: List of barrier functions to check. :param delta_q: Configuration displacement to validate. Modified in place: set to zero if barrier violation is detected. :param tolerance: Tolerance for barrier violation detection. A barrier is considered violated if h(q + delta_q) < -tolerance. Default is 0.0. :raises RuntimeError: If barrier evaluation fails (e.g., frame not found). .. rubric:: Example delta_q = np.zeros(oink.num_variables) oink.solveIk(scene, tasks, constraints, barriers, delta_q) oink.enforceBarriers(scene, barriers, delta_q)