roboplan.roboplan_ext.optimal_ik
================================

.. py:module:: roboplan.roboplan_ext.optimal_ik


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.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)

   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:class:: FrameTask(target_pose, num_variables, 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: std::optional<unsigned long>


      Index of the frame in the scene's Pinocchio model.


   .. py:property:: target_pose
      :type: roboplan_ext.core.CartesianConfiguration


      Target pose for the frame.


   .. py:method:: setTargetFrameTransform(arg, /)

      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(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(num_variables, 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(num_variables, 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:: Oink(num_variables)

   Optimal Inverse Kinematics solver.


   .. py:method:: solveIk(tasks, constraints, scene, delta_q, regularization = 1e-12)

      Solve inverse kinematics for given tasks and constraints.

      Solves a QP optimization problem to compute the joint velocity that minimizes
      weighted task errors while satisfying all constraints. The result is written
      directly into the provided delta_q buffer.

      :param tasks: List of weighted tasks to optimize for.
      :param constraints: List of constraints to satisfy.
      :param scene: Scene containing robot model and state.
      :param delta_q: Pre-allocated numpy array for output (size = num_variables).
                      Must be a contiguous float64 array. Modified in-place.
      :param regularization: Tikhonov regularization weight added to the Hessian diagonal.
                             Provides numerical stability. Higher values increase regularization
                             but may reduce task tracking accuracy. Default is 1e-12.

      :raises RuntimeError: If the QP solver fails to find a solution.

      .. rubric:: Example

      delta_q = np.zeros(oink.num_variables)
      oink.solveIk(tasks, constraints, scene, delta_q)
      # Or with custom regularization:
      oink.solveIk(tasks, constraints, scene, delta_q, regularization=1e-6)