adam.casadi.inverse_kinematics

adam.casadi.inverse_kinematics#

Classes#

`TargetType`	Type of IK target supported by the solver.
`FramesConstraint`	Type of constraint for the frames in the IK problem.
`Target`	Dataclass to store target information.
`InverseKinematics`

Module Contents#

class adam.casadi.inverse_kinematics.TargetType(*args, **kwds)[source]#

Bases: enum.Enum

Type of IK target supported by the solver.

POSITION[source]#

ROTATION[source]#

POSE[source]#

class adam.casadi.inverse_kinematics.FramesConstraint(*args, **kwds)[source]#

Bases: enum.Enum

Type of constraint for the frames in the IK problem.

BALL[source]#

FIXED[source]#

class adam.casadi.inverse_kinematics.Target[source]#

Dataclass to store target information.

target_type: TargetType[source]#

frame: str[source]#

weight: float = 1.0[source]#

as_soft_constraint: bool = True[source]#

forward_kinematics_function: casadi.Function = None[source]#

param_pos: casadi.SX = None[source]#

param_rot: casadi.SX = None[source]#

class adam.casadi.inverse_kinematics.InverseKinematics(urdf_path: str, joints_list: list[str], joint_limits_active: bool = True, solver_settings: dict[str, Any] = None)[source]#

kd[source]#

ndof[source]#

joints_list[source]#

opti[source]#

base_pos[source]#

base_quat[source]#

joint_pos[source]#

base_homogeneous[source]#

targets: dict[str, Target][source]#

cost_terms: list[casadi.MX] = [][source]#

_problem_built = False[source]#

_cached_sol = None[source]#

joint_limits_active = True[source]#

get_opti_variables() → dict[str, casadi.SX][source]#

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

Return type:

dict[str, cs.SX]

set_solver(solver_name: str, solver_settings: dict[str, Any])[source]#

Set the solver for the optimization problem.

Parameters:

solver_name (str) – The name of the solver to use.
solver_settings (dict[str, Any]) – The settings for the solver.

get_opti() → casadi.Opti[source]#

Get the CasADi Opti object for this IK solver. This can be used to add custom constraints or objectives.

Returns:: The CasADi Opti object.
Return type:: cs.Opti

get_kin_dyn_computations() → adam.casadi.KinDynComputations[source]#

Get the KinDynComputations object used by this IK solver.

Returns:: The KinDynComputations object.
Return type:: KinDynComputations

set_default_joint_limit_constraints()[source]#: Set joint limit constraints if active.

set_joint_limits(lower_bounds: numpy.ndarray, upper_bounds: numpy.ndarray)[source]#

Set custom joint limits for the optimization problem.

Parameters:

lower_bounds (np.ndarray) – Lower bounds for each joint.
upper_bounds (np.ndarray) – Upper bounds for each joint.

add_target_position(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0)[source]#

Add a target position for the IK solver.

Parameters:

frame (str) – The name of the frame to target.
as_soft_constraint (bool) – If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – The weight of the target, ignored if as_soft_constraint is False.

Raises:

ValueError – If the target frame already exists.

add_frames_constraint(parent_frame: str, child_frame: str, constraint_type: FramesConstraint, as_soft_constraint: bool = True, weight: float = 100000.0)[source]#

Add a constraint between two frames.

Parameters:

parent_frame (str) – The name of the parent frame.
child_frame (str) – The name of the child frame.
constraint_type (FramesConstraint) – Type of constraint to apply.
as_soft_constraint (bool) – If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the constraint in the cost function, ignored if as_soft_constraint is False.

add_ball_constraint(parent_frame: str, child_frame: str, as_soft_constraint: bool = True, weight: float = 100000.0)[source]#

Add a ball constraint between two frames.

Parameters:

parent_frame (str) – The name of the parent frame.
child_frame (str) – The name of the child frame.
as_soft_constraint (bool) – If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the constraint in the cost function, ignored if as_soft_constraint is False

add_fixed_constraint(parent_frame: str, child_frame: str, as_soft_constraint: bool = True, weight: float = 100000.0)[source]#

Add a fixed constraint between two frames.

Parameters:

parent_frame (str) – The name of the parent frame.
child_frame (str) – The name of the child frame.
as_soft_constraint (bool) – If False, adds the constraint as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the constraint in the cost function, ignored if as_soft_constraint is False

add_min_distance_constraint(frames_list: list[str], distance: float)[source]#

Add a distance constraint between frames.

Parameters:

frames_list (list[str]) – List of frame names to apply the distance constraint.
distance (float) – Minimum distance between consecutive frames.

add_target_orientation(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0)[source]#

Add an orientation target for a frame.

Parameters:

frame (str) – The name of the frame to target.
as_soft_constraint (bool) – If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the target in the cost function, ignored if as_soft_constraint is False.

Raises:

ValueError – If the target frame already exists.

add_target_pose(frame: str, *, as_soft_constraint: bool = True, weight: float = 1.0)[source]#

Add a pose target for a frame.

Parameters:

frame (str) – The name of the frame to target.
as_soft_constraint (bool) – If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the target in the cost function, ignored if as_soft_constraint is False.

Raises:

ValueError – If the target frame already exists.

add_target(frame: str, target_type: TargetType, *, as_soft_constraint: bool = True, weight: float = 1.0)[source]#

Add a target for a frame.

Parameters:

frame (str) – The name of the frame to target.
target_type (TargetType) – The type of target (position, rotation, or pose).
as_soft_constraint (bool) – If False, adds the target as a hard constraint. If True, adds it as a soft constraint (cost term).
weight (float) – Weight for the target in the cost function, ignored if as_soft_constraint is False.

update_target_position(frame: str, position: numpy.ndarray)[source]#

Update the target position for a frame.

Parameters:

frame (str) – The name of the frame to update.
position (np.ndarray) – The new target position.

update_target_orientation(frame: str, rotation: numpy.ndarray)[source]#

Update the target orientation for a frame.

Parameters:

frame (str) – The name of the frame to update.
rotation (np.ndarray) – The new target rotation.

update_target_pose(frame: str, position: numpy.ndarray, rotation: numpy.ndarray)[source]#

Update the target pose for a frame.

Parameters:

frame (str) – The name of the frame to update.
position (np.ndarray) – The new target position.
rotation (np.ndarray) – The new target rotation.

update_target(frame: str, target: numpy.ndarray | tuple[numpy.ndarray, numpy.ndarray])[source]#

Update the target for a frame.

Parameters:

frame (str) – The name of the frame to update.
target (Union[np.ndarray, tuple[np.ndarray, np.ndarray]]) – The new target position, rotation, or pose.

Raises:

ValueError – If the target is of an invalid type.
ValueError – If the frame is not a pose target.
RuntimeError – If the optimization problem has already been built.

set_initial_guess(base_transform: numpy.ndarray, joint_values: numpy.ndarray)[source]#

Set the initial guess for the optimization problem.

Parameters:

base_transform (np.ndarray) – 4x4 transformation matrix for the floating base.
joint_values (np.ndarray) – Initial joint values for the robot.

solve()[source]#: Solve the optimization problem.

get_solution()[source]#

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.
Return type:: Tuple[np.ndarray, np.ndarray]

base_transform()[source]#: Symbolic 4x4 transform of the floating base (SX).

_ensure_graph_modifiable()[source]#

_finalize_problem()[source]#

Finalize the optimization problem.

Raises:: RuntimeError – If the optimization problem has already been built.

_check_target_type(frame: str, expected: TargetType)[source]#

Check the target type for a given frame.

Parameters:

frame (str) – The name of the frame to check.
expected (TargetType) – The expected target type.

Raises:

ValueError – If the target type does not match the expected type.

add_cost(cost: casadi.MX)[source]#

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