roboplan.roboplan_ext.optimal_ik ================================ .. py:module:: roboplan.roboplan_ext.optimal_ik .. autoapi-nested-parse:: Optimal IK solver module Classes ------- .. autoapisummary:: roboplan.roboplan_ext.optimal_ik.Task roboplan.roboplan_ext.optimal_ik.FrameTaskOptions roboplan.roboplan_ext.optimal_ik.FrameTask roboplan.roboplan_ext.optimal_ik.ConfigurationTaskOptions roboplan.roboplan_ext.optimal_ik.ConfigurationTask roboplan.roboplan_ext.optimal_ik.Constraints roboplan.roboplan_ext.optimal_ik.PositionLimit roboplan.roboplan_ext.optimal_ik.VelocityLimit roboplan.roboplan_ext.optimal_ik.Barrier roboplan.roboplan_ext.optimal_ik.ConstraintAxisSelection roboplan.roboplan_ext.optimal_ik.PositionBarrier roboplan.roboplan_ext.optimal_ik.Oink Module 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:: 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')) 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: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_ext.core.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) 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: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:: Oink(scene: roboplan_ext.core.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_ext.core.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_ext.core.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_ext.core.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_ext.core.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)