adam.casadi.inverse_kinematics ============================== .. py:module:: adam.casadi.inverse_kinematics Classes ------- .. autoapisummary:: adam.casadi.inverse_kinematics.TargetType adam.casadi.inverse_kinematics.FramesConstraint adam.casadi.inverse_kinematics.Target adam.casadi.inverse_kinematics.InverseKinematics Module Contents --------------- .. py:class:: TargetType(*args, **kwds) Bases: :py:obj:`enum.Enum` Type of IK target supported by the solver. .. py:attribute:: POSITION .. py:attribute:: ROTATION .. py:attribute:: POSE .. py:class:: FramesConstraint(*args, **kwds) Bases: :py:obj:`enum.Enum` Type of constraint for the frames in the IK problem. .. py:attribute:: BALL .. py:attribute:: FIXED .. py:class:: Target Dataclass to store target information. .. py:attribute:: target_type :type: TargetType .. py:attribute:: frame :type: str .. py:attribute:: weight :type: float :value: 1.0 .. py:attribute:: as_soft_constraint :type: bool :value: True .. py:attribute:: forward_kinematics_function :type: casadi.Function :value: None .. py:attribute:: param_pos :type: casadi.SX :value: None .. py:attribute:: param_rot :type: casadi.SX :value: None .. py:class:: InverseKinematics(urdf_path: str, joints_list: list[str], joint_limits_active: bool = True, solver_settings: dict[str, Any] = None) .. py:attribute:: kd .. py:attribute:: ndof .. py:attribute:: joints_list .. py:attribute:: opti .. py:attribute:: base_pos .. py:attribute:: base_quat .. py:attribute:: joint_pos .. py:attribute:: base_homogeneous .. py:attribute:: targets :type: dict[str, Target] .. py:attribute:: cost_terms :type: list[casadi.MX] :value: [] .. py:attribute:: _problem_built :value: False .. py:attribute:: _cached_sol :value: None .. py:attribute:: joint_limits_active :value: True .. py:method:: get_opti_variables() -> dict[str, casadi.SX] Get the optimization variables of the IK solver. :returns: Dictionary containing the optimization variables. - "base_pos": Position of the base (3D vector). - "base_quat": Quaternion representing the base orientation (4D vector with x, y, z, w). - "joint_pos": Joint variables (Vector with length equal to the number of non-fixed joints). - "base_homogeneous": Homogeneous transformation matrix of the base (4x4 matrix, from position and quaternion). :rtype: dict[str, cs.SX] .. py:method:: set_solver(solver_name: str, solver_settings: dict[str, Any]) Set the solver for the optimization problem. :param solver_name: The name of the solver to use. :type solver_name: str :param solver_settings: The settings for the solver. :type solver_settings: dict[str, Any] .. py:method:: get_opti() -> casadi.Opti Get the CasADi Opti object for this IK solver. This can be used to add custom constraints or objectives. :returns: The CasADi Opti object. :rtype: cs.Opti .. py:method:: get_kin_dyn_computations() -> adam.casadi.KinDynComputations Get the KinDynComputations object used by this IK solver. :returns: The KinDynComputations object. :rtype: KinDynComputations .. py:method:: set_default_joint_limit_constraints() Set joint limit constraints if active. .. py:method:: set_joint_limits(lower_bounds: numpy.ndarray, upper_bounds: numpy.ndarray) Set custom joint limits for the optimization problem. :param lower_bounds: Lower bounds for each joint. :type lower_bounds: np.ndarray :param upper_bounds: Upper bounds for each joint. :type upper_bounds: np.ndarray .. py:method:: set_joint_limit(joint_name: str, lower_limit: float, upper_limit: float) Set custom joint limits for a specific joint for the optimization problem. :param joint_name: The name of the joint to set limits for. :type joint_name: str :param lower_limit: The lower limit for the joint. :type lower_limit: float :param upper_limit: The upper limit for the joint. :type upper_limit: float .. py:method:: add_target_position(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0) Add a target position for the IK solver. :param frame: The name of the frame to target. :type frame: str :param as_soft_constraint: If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: The weight of the target, ignored if `as_soft_constraint` is `False`. :type weight: float :raises ValueError: If the target frame already exists. .. py:method:: add_frames_constraint(parent_frame: str, child_frame: str, constraint_type: FramesConstraint, as_soft_constraint: bool = True, weight: float = 100000.0) Add a constraint between two frames. :param parent_frame: The name of the parent frame. :type parent_frame: str :param child_frame: The name of the child frame. :type child_frame: str :param constraint_type: Type of constraint to apply. :type constraint_type: FramesConstraint :param as_soft_constraint: If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the constraint in the cost function, ignored if `as_soft_constraint` is `False`. :type weight: float .. py:method:: add_ball_constraint(parent_frame: str, child_frame: str, as_soft_constraint: bool = True, weight: float = 100000.0) Add a ball constraint between two frames. :param parent_frame: The name of the parent frame. :type parent_frame: str :param child_frame: The name of the child frame. :type child_frame: str :param as_soft_constraint: If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the constraint in the cost function, ignored if `as_soft_constraint` is `False` :type weight: float .. py:method:: add_fixed_constraint(parent_frame: str, child_frame: str, as_soft_constraint: bool = True, weight: float = 100000.0) Add a fixed constraint between two frames. :param parent_frame: The name of the parent frame. :type parent_frame: str :param child_frame: The name of the child frame. :type child_frame: str :param as_soft_constraint: If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the constraint in the cost function, ignored if `as_soft_constraint` is `False` :type weight: float .. py:method:: add_min_distance_constraint(frames_list: list[str], distance: float) Add a distance constraint between frames. :param frames_list: List of frame names to apply the distance constraint. :type frames_list: list[str] :param distance: Minimum distance between consecutive frames. :type distance: float .. py:method:: add_target_orientation(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0) Add an orientation target for a frame. :param frame: The name of the frame to target. :type frame: str :param as_soft_constraint: If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the target in the cost function, ignored if `as_soft_constraint` is `False`. :type weight: float :raises ValueError: If the target frame already exists. .. py:method:: add_target_pose(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0) Add a pose target for a frame. :param frame: The name of the frame to target. :type frame: str :param as_soft_constraint: If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the target in the cost function, ignored if `as_soft_constraint` is `False`. :type weight: float :raises ValueError: If the target frame already exists. .. py:method:: add_target(frame: str, target_type: TargetType, *, as_soft_constraint: bool = True, weight: float = 1.0) Add a target for a frame. :param frame: The name of the frame to target. :type frame: str :param target_type: The type of target (position, rotation, or pose). :type target_type: TargetType :param as_soft_constraint: If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term). :type as_soft_constraint: bool :param weight: Weight for the target in the cost function, ignored if `as_soft_constraint` is `False`. :type weight: float .. py:method:: update_target_position(frame: str, position: numpy.ndarray) Update the target position for a frame. :param frame: The name of the frame to update. :type frame: str :param position: The new target position. :type position: np.ndarray .. py:method:: update_target_orientation(frame: str, rotation: numpy.ndarray) Update the target orientation for a frame. :param frame: The name of the frame to update. :type frame: str :param rotation: The new target rotation. :type rotation: np.ndarray .. py:method:: update_target_pose(frame: str, position: numpy.ndarray, rotation: numpy.ndarray) Update the target pose for a frame. :param frame: The name of the frame to update. :type frame: str :param position: The new target position. :type position: np.ndarray :param rotation: The new target rotation. :type rotation: np.ndarray .. py:method:: update_target(frame: str, target: numpy.ndarray | tuple[numpy.ndarray, numpy.ndarray]) Update the target for a frame. :param frame: The name of the frame to update. :type frame: str :param target: The new target position, rotation, or pose. :type target: Union[np.ndarray, tuple[np.ndarray, np.ndarray]] :raises ValueError: If the target is of an invalid type. :raises ValueError: If the frame is not a pose target. :raises RuntimeError: If the optimization problem has already been built. .. py:method:: set_initial_guess(base_transform: numpy.ndarray, joint_values: numpy.ndarray) Set the initial guess for the optimization problem. :param base_transform: 4x4 transformation matrix for the floating base. :type base_transform: np.ndarray :param joint_values: Initial joint values for the robot. :type joint_values: np.ndarray .. py:method:: solve() Solve the optimization problem. .. py:method:: get_solution() Get the solution of the optimization problem. :raises RuntimeError: If the optimization problem has not been solved yet. :returns: The 4x4 transformation matrix and joint values. :rtype: Tuple[np.ndarray, np.ndarray] .. py:method:: base_transform() Symbolic 4x4 transform of the floating base (SX). .. py:method:: _ensure_graph_modifiable() .. py:method:: _finalize_problem() Finalize the optimization problem. :raises RuntimeError: If the optimization problem has already been built. .. py:method:: _check_target_type(frame: str, expected: TargetType) Check the target type for a given frame. :param frame: The name of the frame to check. :type frame: str :param expected: The expected target type. :type expected: TargetType :raises ValueError: If the target type does not match the expected type. .. py:method:: add_cost(cost: casadi.MX) Add a custom cost term to the optimization problem. This method appends the provided cost term to the `cost_terms` list. During the `_finalize_problem` step, all cost terms in this list are combined using `cs.sum(cs.vertcat(*self.cost_terms))` and minimized as part of the objective function. Note: This method ensures that the optimization graph is still modifiable before adding the cost term. Once the problem is finalized (after the first call to `solve()`), no new cost terms can be added. :param cost: The cost term to add. :type cost: cs.MX