Kinematics

Factory Function

create_ik_solver(chain_name: str, config: IKConfig | PinkIKConfig | None = None, side: str | None = None, urdf_path: str | None = None, backend: str = 'trac_ik', *, joint_names: list[str] | None = None, self_collision: bool = False) -> IKSolverBase

Factory function to create an IK solver for a named chain.

Input

chain_name: Name of the chain (e.g. "left_arm", "whole_body"). config: IK configuration — IKConfig for trac_ik, PinkIKConfig for pink (uses defaults if None). side: Optional "left" or "right" suffix for compound names. urdf_path: Override the default URDF file path. backend: "trac_ik" (default) or "pink". joint_names: (pink only) Explicit controlled joint names. Derived from the kinematic chain if None. self_collision: (pink only) Build a collision model from the URDF's co-located SRDF for collision avoidance.

Output: An IKSolverBase instance (TracIKSolver or PinkIKSolver).

Examples:

create_ik_solver("left_arm") create_ik_solver("whole_body", side="left") create_ik_solver("whole_body", side="left", backend="pink", config=PinkIKConfig(com_cost=0.1))

Source code in autolife_planning/kinematics/ik_solver_base.py

def create_ik_solver(
    chain_name: str,
    config: IKConfig | PinkIKConfig | None = None,
    side: str | None = None,
    urdf_path: str | None = None,
    backend: str = "trac_ik",
    *,
    joint_names: list[str] | None = None,
    self_collision: bool = False,
) -> IKSolverBase:
    """Factory function to create an IK solver for a named chain.

    Input:
        chain_name: Name of the chain (e.g. "left_arm", "whole_body").
        config: IK configuration — ``IKConfig`` for trac_ik,
            ``PinkIKConfig`` for pink (uses defaults if None).
        side: Optional "left" or "right" suffix for compound names.
        urdf_path: Override the default URDF file path.
        backend: ``"trac_ik"`` (default) or ``"pink"``.
        joint_names: (pink only) Explicit controlled joint names.
            Derived from the kinematic chain if None.
        self_collision: (pink only) Build a collision model from the
            URDF's co-located SRDF for collision avoidance.
    Output:
        An IKSolverBase instance (TracIKSolver or PinkIKSolver).

    Examples:
        create_ik_solver("left_arm")
        create_ik_solver("whole_body", side="left")
        create_ik_solver("whole_body", side="left", backend="pink",
                         config=PinkIKConfig(com_cost=0.1))
    """
    chain_config = _resolve_chain_config(chain_name, side, urdf_path)

    if backend == "trac_ik":
        try:
            from autolife_planning.kinematics.trac_ik_solver import TracIKSolver
        except ModuleNotFoundError as exc:
            if exc.name == "pinocchio":
                raise ModuleNotFoundError(
                    "TRAC-IK backend requires 'pinocchio' (PyPI package: 'pin'). "
                    "Install a compatible pin release for your Python version "
                    "(for Python 3.8, pin==2.6.21 is known to work), and run "
                    "with a clean environment (avoid ROS PYTHONPATH/LD_LIBRARY_PATH "
                    "overrides)."
                ) from exc
            raise

        ik_config = config if isinstance(config, IKConfig) else None
        return TracIKSolver(chain_config, ik_config)

    if backend == "pink":
        from autolife_planning.kinematics.collision_model import build_collision_model
        from autolife_planning.kinematics.pink_ik_solver import PinkIKSolver

        pink_config = config if isinstance(config, PinkIKConfig) else None

        collision_context = None
        if self_collision:
            collision_context = build_collision_model(chain_config.urdf_path)

        return PinkIKSolver(
            chain_config,
            config=pink_config,
            joint_names=joint_names,
            collision_context=collision_context,
        )

    raise ValueError(f"Unknown backend '{backend}'. Choose 'trac_ik' or 'pink'.")

TRAC-IK Solver

TracIKSolver(chain_config: ChainConfig, config: IKConfig | None = None)

Bases: IKSolverBase

IK solver wrapping the TRAC-IK C++ library.

Source code in autolife_planning/kinematics/trac_ik_solver.py

def __init__(
    self,
    chain_config: ChainConfig,
    config: IKConfig | None = None,
) -> None:
    if config is None:
        config = IKConfig()

    import pytracik

    # Read URDF string for TRAC-IK constructor
    with open(chain_config.urdf_path) as f:
        urdf_string = f.read()

    # Map SolveType enum
    cpp_solve_type = getattr(pytracik.SolveType, _SOLVE_TYPE_MAP[config.solve_type])

    self._trac_ik = pytracik.TRAC_IK(
        chain_config.base_link,
        chain_config.ee_link,
        urdf_string,
        config.timeout,
        config.epsilon,
        cpp_solve_type,
    )
    self._pytracik = pytracik
    self._chain_config = chain_config
    self._config = config

    # Cache joint limits
    self._lower_bounds = np.array(pytracik.get_joint_lower_bounds(self._trac_ik))
    self._upper_bounds = np.array(pytracik.get_joint_upper_bounds(self._trac_ik))
    self._n_joints = pytracik.get_num_joints(self._trac_ik)

    # Compute the base-link-to-world transform so FK/IK use world frame
    # (TRAC-IK's KDL chain returns poses relative to base_link).
    model = pin.buildModelFromUrdf(chain_config.urdf_path)
    data = model.createData()
    pin.forwardKinematics(model, data, pin.neutral(model))
    pin.updateFramePlacements(model, data)
    base_fid = model.getFrameId(chain_config.base_link)
    oMb = data.oMf[base_fid]
    self._base_R: np.ndarray = np.array(oMb.rotation, dtype=np.float64)
    self._base_t: np.ndarray = np.array(oMb.translation, dtype=np.float64)

joint_limits: tuple[np.ndarray, np.ndarray] property

Joint limits as (lower_bounds, upper_bounds) arrays.

set_joint_limits(lower: np.ndarray, upper: np.ndarray) -> None

Override joint limits for the solver.

Input

lower: Lower bounds array of length num_joints upper: Upper bounds array of length num_joints

Source code in autolife_planning/kinematics/trac_ik_solver.py

def set_joint_limits(self, lower: np.ndarray, upper: np.ndarray) -> None:
    """Override joint limits for the solver.

    Input:
        lower: Lower bounds array of length num_joints
        upper: Upper bounds array of length num_joints
    """
    lower = np.asarray(lower, dtype=np.float64)
    upper = np.asarray(upper, dtype=np.float64)
    if len(lower) != self._n_joints or len(upper) != self._n_joints:
        raise ValueError(
            f"Expected {self._n_joints} limits, "
            f"got lower={len(lower)}, upper={len(upper)}"
        )
    self._pytracik.set_joint_limits(self._trac_ik, lower, upper)
    self._lower_bounds = lower.copy()
    self._upper_bounds = upper.copy()

fk(joint_positions: np.ndarray) -> SE3Pose

Compute forward kinematics in world frame.

Input

joint_positions: Joint angles array of length num_joints

Output: SE3Pose of the end effector in world frame

Source code in autolife_planning/kinematics/trac_ik_solver.py

def fk(self, joint_positions: np.ndarray) -> SE3Pose:
    """Compute forward kinematics in world frame.

    Input:
        joint_positions: Joint angles array of length num_joints
    Output:
        SE3Pose of the end effector in world frame
    """
    q = np.asarray(joint_positions, dtype=np.float64)
    T = self._pytracik.fk(self._trac_ik, q)
    if T.size == 0:
        raise RuntimeError("FK failed — invalid joint configuration")
    local = SE3Pose.from_matrix(T)
    return SE3Pose(
        position=self._base_R @ local.position + self._base_t,
        rotation=self._base_R @ local.rotation,
    )

solve(target_pose: SE3Pose, seed: np.ndarray | None = None, config: IKConfig | None = None) -> IKResult

Solve IK with random restart loop.

Input

target_pose: Desired end-effector pose seed: Initial joint configuration (random if None) config: Override default IK config for this solve

Output: IKResult with solution status and joint positions

Source code in autolife_planning/kinematics/trac_ik_solver.py

def solve(
    self,
    target_pose: SE3Pose,
    seed: np.ndarray | None = None,
    config: IKConfig | None = None,
) -> IKResult:
    """Solve IK with random restart loop.

    Input:
        target_pose: Desired end-effector pose
        seed: Initial joint configuration (random if None)
        config: Override default IK config for this solve
    Output:
        IKResult with solution status and joint positions
    """
    cfg = config if config is not None else self._config

    # Convert world-frame target to base-link frame for pytracik
    R_inv = self._base_R.T
    local_pos = R_inv @ (target_pose.position - self._base_t)
    local_rot = R_inv @ target_pose.rotation
    x, y, z = local_pos
    quat_xyzw = Rotation.from_matrix(local_rot).as_quat()  # [x,y,z,w]
    qx, qy, qz, qw = quat_xyzw

    best_result: IKResult | None = None

    for attempt in range(cfg.max_attempts):
        if seed is not None and attempt == 0:
            q_seed = np.asarray(seed, dtype=np.float64)
        else:
            # Random seed within joint limits
            q_seed = np.random.uniform(self._lower_bounds, self._upper_bounds)

        result_arr = self._pytracik.ik(
            self._trac_ik, q_seed, x, y, z, qx, qy, qz, qw
        )

        ret_code = int(result_arr[0])
        if ret_code >= 0:
            q_solution = result_arr[1:]

            # Validate solution via FK
            achieved_pose = self.fk(q_solution)
            pos_err = float(
                np.linalg.norm(achieved_pose.position - target_pose.position)
            )
            # Orientation error: angle of rotation difference
            R_err = achieved_pose.rotation.T @ target_pose.rotation
            ori_err = float(np.linalg.norm(Rotation.from_matrix(R_err).as_rotvec()))

            result = IKResult(
                status=IKStatus.SUCCESS,
                joint_positions=q_solution,
                final_error=pos_err + ori_err,
                iterations=attempt + 1,
                position_error=pos_err,
                orientation_error=ori_err,
            )

            # Check if within post-solve tolerances
            if (
                pos_err < cfg.position_tolerance
                and ori_err < cfg.orientation_tolerance
            ):
                return result

            # Keep best result so far
            if best_result is None or result.final_error < best_result.final_error:
                best_result = result

    # Return best result found, or failure
    if best_result is not None:
        return best_result

    return IKResult(
        status=IKStatus.FAILED,
        joint_positions=None,
        final_error=float("inf"),
        iterations=cfg.max_attempts,
        position_error=float("inf"),
        orientation_error=float("inf"),
    )

Pink Constrained IK Solver

PinkIKSolver(chain_config: ChainConfig, config: PinkIKConfig | None = None, joint_names: list[str] | None = None, collision_context: CollisionContext | None = None)

Bases: IKSolverBase

Constrained IK solver built on Pink's QP-based differential IK.

Supports Levenberg-Marquardt damping (singularity avoidance), posture regularization, CoM stability, and collision barriers.

Use via the unified factory::

solver = create_ik_solver("whole_body", side="left", backend="pink",
                          config=PinkIKConfig(com_cost=0.1))

Source code in autolife_planning/kinematics/pink_ik_solver.py

def __init__(
    self,
    chain_config: ChainConfig,
    config: PinkIKConfig | None = None,
    joint_names: list[str] | None = None,
    collision_context: CollisionContext | None = None,
) -> None:
    self._chain_config = chain_config
    self._config = config if isinstance(config, PinkIKConfig) else PinkIKConfig()

    # Reuse the collision context's model when available so that the
    # kinematic model and collision model share the same instance.
    if collision_context is not None:
        self._model = collision_context.model
        self._data = collision_context.data
    else:
        self._model = pin.buildModelFromUrdf(chain_config.urdf_path)
        self._data = self._model.createData()

    # Resolve EE frame
    if not self._model.existFrame(chain_config.ee_link):
        available = [
            self._model.frames[i].name for i in range(int(self._model.nframes))
        ]
        raise ValueError(
            f"Frame '{chain_config.ee_link}' not found. " f"Available: {available}"
        )
    self._ee_frame_id = self._model.getFrameId(chain_config.ee_link)

    # Determine controlled joints
    if joint_names is not None:
        model_names = list(self._model.names)
        self._joint_ids: list[int] = []
        for name in joint_names:
            if name not in model_names:
                raise ValueError(f"Joint '{name}' not in model")
            self._joint_ids.append(self._model.getJointId(name))
        self._joint_names = list(joint_names)
    else:
        self._joint_ids = _get_chain_joint_ids(
            self._model, chain_config.base_link, chain_config.ee_link
        )
        self._joint_names = [str(self._model.names[jid]) for jid in self._joint_ids]

    # Pre-compute velocity-index set for controlled joints
    self._controlled_idx_v: set[int] = set()
    for jid in self._joint_ids:
        idx_v = self._model.joints[jid].idx_v
        nv = self._model.joints[jid].nv
        for k in range(nv):
            self._controlled_idx_v.add(idx_v + k)

    self._collision_context = collision_context

set_collision_context(context: CollisionContext | None) -> None

Replace the collision context (e.g. after updating obstacles).

Source code in autolife_planning/kinematics/pink_ik_solver.py

def set_collision_context(self, context: CollisionContext | None) -> None:
    """Replace the collision context (e.g. after updating obstacles)."""
    self._collision_context = context

fk(joint_positions: np.ndarray) -> SE3Pose

Compute forward kinematics for the end effector.

Source code in autolife_planning/kinematics/pink_ik_solver.py

def fk(self, joint_positions: np.ndarray) -> SE3Pose:
    """Compute forward kinematics for the end effector."""
    q = self._to_full_q(joint_positions)
    pin.forwardKinematics(self._model, self._data, q)
    pin.updateFramePlacements(self._model, self._data)
    oMf = self._data.oMf[self._ee_frame_id]
    return SE3Pose(
        position=np.array(oMf.translation, dtype=np.float64),
        rotation=np.array(oMf.rotation, dtype=np.float64),
    )

solve(target_pose: SE3Pose, seed: np.ndarray | None = None, config: PinkIKConfig | None = None) -> IKResult

Solve IK, returning a standard :class:IKResult.

Accepts :class:PinkIKConfig; falls back to constructor default.

Source code in autolife_planning/kinematics/pink_ik_solver.py

def solve(
    self,
    target_pose: SE3Pose,
    seed: np.ndarray | None = None,
    config: PinkIKConfig | None = None,
) -> IKResult:
    """Solve IK, returning a standard :class:`IKResult`.

    Accepts :class:`PinkIKConfig`; falls back to constructor default.
    """
    pink_config = config if isinstance(config, PinkIKConfig) else None
    result = self.solve_constrained(target_pose, seed, pink_config)
    return IKResult(
        status=result.status,
        joint_positions=result.joint_positions,
        final_error=result.final_error,
        iterations=result.iterations,
        position_error=result.position_error,
        orientation_error=result.orientation_error,
    )

solve_constrained(target_pose: SE3Pose, seed: np.ndarray | None = None, config: PinkIKConfig | None = None) -> ConstrainedIKResult

Full constrained IK solve with trajectory output.

Input

target_pose: Desired end-effector pose. seed: Initial joint configuration (controlled joints). Uses Pinocchio neutral config if None. config: Override the default PinkIKConfig for this call.

Output: ConstrainedIKResult including the iteration trajectory.

Source code in autolife_planning/kinematics/pink_ik_solver.py

def solve_constrained(
    self,
    target_pose: SE3Pose,
    seed: np.ndarray | None = None,
    config: PinkIKConfig | None = None,
) -> ConstrainedIKResult:
    """Full constrained IK solve with trajectory output.

    Input:
        target_pose: Desired end-effector pose.
        seed: Initial joint configuration (controlled joints).
            Uses Pinocchio neutral config if None.
        config: Override the default PinkIKConfig for this call.
    Output:
        ConstrainedIKResult including the iteration trajectory.
    """
    import pink
    from pink.limits import ConfigurationLimit
    from pink.tasks import ComTask, FrameTask, PostureTask

    cfg = config or self._config

    # --- Initial full configuration ---
    if seed is not None:
        q_init = self._to_full_q(np.asarray(seed, dtype=np.float64))
    else:
        q_init = pin.neutral(self._model)

    # --- Pink Configuration ---
    ck = self._collision_context
    if ck is not None and cfg.self_collision:
        configuration = pink.Configuration(
            self._model,
            self._data,
            q_init,
            collision_model=ck.collision_model,
            collision_data=ck.collision_data,
        )
    else:
        configuration = pink.Configuration(self._model, self._data, q_init)

    # --- Target ---
    target_se3 = pin.SE3(
        np.asarray(target_pose.rotation, dtype=np.float64),
        np.asarray(target_pose.position, dtype=np.float64),
    )

    # --- Tasks ---
    ee_task = FrameTask(
        self._chain_config.ee_link,
        position_cost=cfg.position_cost,
        orientation_cost=cfg.orientation_cost,
        lm_damping=cfg.lm_damping,
    )
    ee_task.set_target(target_se3)

    posture_task = PostureTask(cost=cfg.posture_cost)
    posture_task.set_target(q_init)

    tasks: list = [ee_task, posture_task]

    if cfg.com_cost > 0:
        com_task = ComTask(cost=cfg.com_cost)
        com_task.set_target_from_configuration(configuration)
        tasks.append(com_task)

    if cfg.camera_frame and cfg.camera_cost > 0:
        if not self._model.existFrame(cfg.camera_frame):
            raise ValueError(
                f"Camera frame '{cfg.camera_frame}' not found in model"
            )
        camera_task = FrameTask(
            cfg.camera_frame,
            position_cost=cfg.camera_cost,
            orientation_cost=cfg.camera_cost,
            lm_damping=cfg.lm_damping,
        )
        camera_task.set_target_from_configuration(configuration)
        tasks.append(camera_task)

    # --- Limits ---
    limits = [ConfigurationLimit(self._model)]

    # --- Barriers ---
    barriers: list = []
    if cfg.self_collision and ck is not None:
        from pink.barriers import SelfCollisionBarrier

        n_pairs = len(ck.collision_model.collisionPairs)
        if n_pairs > 0:
            barriers.append(
                SelfCollisionBarrier(
                    n_collision_pairs=min(cfg.collision_pairs, n_pairs),
                    gain=cfg.collision_gain,
                    d_min=cfg.collision_d_min,
                )
            )

    # --- Iterative solve ---
    trajectory = [self._from_full_q(q_init)]

    for iteration in range(cfg.max_iterations):
        solver_name = cfg.solver
        velocity = None
        try:
            velocity = pink.solve_ik(
                configuration,
                tasks,
                cfg.dt,
                solver=solver_name,
                limits=limits,
                barriers=barriers or None,
            )
        except Exception as exc:
            # Some environments (e.g. py38 with qpsolvers extras) may not
            # have the requested solver backend. Fall back to osqp when the
            # configured solver is unavailable.
            msg = str(exc)
            solver_not_found = (
                "does not seem to be installed" in msg
                or exc.__class__.__name__ == "SolverNotFound"
            )
            if solver_name == "proxqp" and solver_not_found:
                try:
                    velocity = pink.solve_ik(
                        configuration,
                        tasks,
                        cfg.dt,
                        solver="osqp",
                        limits=limits,
                        barriers=barriers or None,
                    )
                except Exception as exc2:
                    exc = exc2
                else:
                    # Fallback succeeded, continue the solve loop.
                    pass
            else:
                # Keep ``exc`` for the failure path below.
                pass

            # If fallback succeeded, ``velocity`` exists in local scope.
            if velocity is not None:
                pass
            elif getattr(pink, "PinkError", None) is None or isinstance(
                exc, getattr(pink, "PinkError")
            ):
                # QP infeasible at this step (e.g. conflicting barriers) —
                # return best-effort result so far.
                q_current = configuration.q
                pos_err, ori_err = self._compute_errors(q_current, target_pose)
                return ConstrainedIKResult(
                    status=IKStatus.FAILED,
                    joint_positions=self._from_full_q(q_current),
                    final_error=pos_err + ori_err,
                    iterations=iteration,
                    position_error=pos_err,
                    orientation_error=ori_err,
                    trajectory=np.array(trajectory),
                )
            else:
                # Unknown backend exception style (pink/qpsolvers version
                # mismatch). Return best-effort result instead of crashing.
                q_current = configuration.q
                pos_err, ori_err = self._compute_errors(q_current, target_pose)
                return ConstrainedIKResult(
                    status=IKStatus.FAILED,
                    joint_positions=self._from_full_q(q_current),
                    final_error=pos_err + ori_err,
                    iterations=iteration,
                    position_error=pos_err,
                    orientation_error=ori_err,
                    trajectory=np.array(trajectory),
                )

        # Zero velocity for uncontrolled joints
        for i in range(self._model.nv):
            if i not in self._controlled_idx_v:
                velocity[i] = 0.0

        configuration.integrate_inplace(velocity, cfg.dt)

        q_current = configuration.q
        trajectory.append(self._from_full_q(q_current))

        # Convergence check
        pos_err, ori_err = self._compute_errors(q_current, target_pose)
        if pos_err < cfg.convergence_thresh and ori_err < cfg.orientation_thresh:
            return ConstrainedIKResult(
                status=IKStatus.SUCCESS,
                joint_positions=trajectory[-1],
                final_error=pos_err + ori_err,
                iterations=iteration + 1,
                position_error=pos_err,
                orientation_error=ori_err,
                trajectory=np.array(trajectory),
            )

    # Did not converge — return best effort
    q_final = configuration.q
    pos_err, ori_err = self._compute_errors(q_final, target_pose)
    return ConstrainedIKResult(
        status=IKStatus.MAX_ITERATIONS,
        joint_positions=trajectory[-1],
        final_error=pos_err + ori_err,
        iterations=cfg.max_iterations,
        position_error=pos_err,
        orientation_error=ori_err,
        trajectory=np.array(trajectory),
    )

Collision Model

CollisionContext(model: Any, collision_model: Any, data: Any, collision_data: Any) dataclass

Pinocchio model with collision geometry.

build_collision_model(urdf_path: str, srdf_path: str | None = None, mesh_dir: str | None = None, min_distance: float = 0.01) -> CollisionContext

Build a Pinocchio model with collision geometry from URDF + SRDF.

After SRDF filtering, pairs that are already closer than min_distance at the neutral configuration are removed. These are typically adjacent links whose meshes overlap and would make the QP infeasible.

Input

urdf_path: Path to the URDF file. srdf_path: Path to SRDF for collision pair filtering. If None, auto-resolves from same directory as URDF. mesh_dir: Directory for resolving package:// mesh URIs. Defaults to the URDF's parent directory. min_distance: Pairs closer than this at neutral are pruned (meters).

Output: CollisionContext with model, collision model, and associated data.

Source code in autolife_planning/kinematics/collision_model.py

def build_collision_model(
    urdf_path: str,
    srdf_path: str | None = None,
    mesh_dir: str | None = None,
    min_distance: float = 0.01,
) -> CollisionContext:
    """Build a Pinocchio model with collision geometry from URDF + SRDF.

    After SRDF filtering, pairs that are already closer than *min_distance*
    at the neutral configuration are removed.  These are typically adjacent
    links whose meshes overlap and would make the QP infeasible.

    Input:
        urdf_path: Path to the URDF file.
        srdf_path: Path to SRDF for collision pair filtering.
            If None, auto-resolves from same directory as URDF.
        mesh_dir: Directory for resolving ``package://`` mesh URIs.
            Defaults to the URDF's parent directory.
        min_distance: Pairs closer than this at neutral are pruned (meters).
    Output:
        CollisionContext with model, collision model, and associated data.
    """
    urdf_dir = os.path.dirname(os.path.abspath(urdf_path))

    if mesh_dir is None:
        mesh_dir = urdf_dir

    if srdf_path is None:
        base = os.path.splitext(os.path.basename(urdf_path))[0]
        candidate = os.path.join(urdf_dir, f"{base}.srdf")
        if os.path.exists(candidate):
            srdf_path = candidate

    model, collision_model, _ = pin.buildModelsFromUrdf(urdf_path, mesh_dir)

    collision_model.addAllCollisionPairs()
    if srdf_path is not None:
        pin.removeCollisionPairs(model, collision_model, srdf_path)

    # Prune pairs already in contact at neutral — these are adjacent-link
    # overlaps that would make any collision barrier infeasible.
    data = model.createData()
    collision_data = pin.GeometryData(collision_model)
    q_neutral = pin.neutral(model)
    pin.computeDistances(model, data, collision_model, collision_data, q_neutral)

    to_remove = []
    for i, cr in enumerate(collision_data.distanceResults):
        if cr.min_distance < min_distance:
            to_remove.append(collision_model.collisionPairs[i])

    for pair in to_remove:
        collision_model.removeCollisionPair(pair)

    # Rebuild data after pruning
    collision_data = pin.GeometryData(collision_model)

    return CollisionContext(
        model=model,
        collision_model=collision_model,
        data=data,
        collision_data=collision_data,
    )

add_pointcloud_obstacles(context: CollisionContext, points: np.ndarray, radius: float = 0.02, voxel_size: float | None = None) -> int

Add pointcloud obstacles as spheres to the collision model.

Each point becomes a sphere attached to the universe joint. Collision pairs are added between every robot geometry and every new obstacle sphere so that SelfCollisionBarrier checks them.

Input

context: Collision context to modify in place. points: (N, 3) array of obstacle positions in world frame. radius: Sphere radius in meters. voxel_size: If set, downsample the pointcloud to a voxel grid first.

Output: Number of obstacle spheres added.

Source code in autolife_planning/kinematics/collision_model.py

def add_pointcloud_obstacles(
    context: CollisionContext,
    points: np.ndarray,
    radius: float = 0.02,
    voxel_size: float | None = None,
) -> int:
    """Add pointcloud obstacles as spheres to the collision model.

    Each point becomes a sphere attached to the universe joint.
    Collision pairs are added between every robot geometry and every
    new obstacle sphere so that ``SelfCollisionBarrier`` checks them.

    Input:
        context: Collision context to modify *in place*.
        points: (N, 3) array of obstacle positions in world frame.
        radius: Sphere radius in meters.
        voxel_size: If set, downsample the pointcloud to a voxel grid first.
    Output:
        Number of obstacle spheres added.
    """
    import hppfcl

    points = np.asarray(points, dtype=np.float64)
    if points.ndim != 2 or points.shape[1] != 3:
        raise ValueError(f"Expected (N, 3) points array, got {points.shape}")

    if voxel_size is not None and voxel_size > 0:
        quantized = np.round(points / voxel_size).astype(np.int64)
        _, unique_idx = np.unique(quantized, axis=0, return_index=True)
        points = quantized[unique_idx].astype(np.float64) * voxel_size

    n_robot_geoms = context.collision_model.ngeoms
    n_added = 0

    for i, pt in enumerate(points):
        sphere = hppfcl.Sphere(radius)
        placement = pin.SE3(np.eye(3), pt)
        # Pinocchio bindings differ across major versions:
        #   3.x commonly accepts (name, parent_joint, geometry, placement)
        #   2.x may require (name, parent_frame, parent_joint, geometry, placement)
        # Try the concise form first, then fall back.
        name = f"obstacle_{i}"
        parent_joint = 0  # universe
        try:
            geom = pin.GeometryObject(name, parent_joint, sphere, placement)
        except Exception:
            parent_frame = 0  # universe frame
            geom = pin.GeometryObject(
                name,
                parent_frame,
                parent_joint,
                sphere,
                placement,
            )
        obs_id = context.collision_model.addGeometryObject(geom)

        for robot_id in range(n_robot_geoms):
            context.collision_model.addCollisionPair(
                pin.CollisionPair(robot_id, obs_id)
            )
        n_added += 1

    context.collision_data = pin.GeometryData(context.collision_model)
    return n_added

Forward Kinematics (Pinocchio)

pinocchio_fk

Pinocchio-based forward kinematics and Jacobian computation.

Used by motion planning. IK solving is handled by trac_ik_solver.py.

PinocchioContext(model: Any, data: Any, end_effector_frame_id: int, joint_names: list[str], joint_ids: list[int]) dataclass

Context holding Pinocchio model and data for FK/Jacobian computations.

create_pinocchio_context(urdf_path: str, end_effector_frame: str, joint_names: list[str] | None = None) -> PinocchioContext

Create a Pinocchio context from URDF file.

Input

urdf_path: Path to the URDF file end_effector_frame: Name of the end effector frame/link joint_names: Optional list of joint names to control (if None, uses all joints)

Output: PinocchioContext containing model and data for FK/Jacobian computations

Source code in autolife_planning/kinematics/pinocchio_fk.py

def create_pinocchio_context(
    urdf_path: str,
    end_effector_frame: str,
    joint_names: list[str] | None = None,
) -> PinocchioContext:
    """
    Create a Pinocchio context from URDF file.

    Input:
        urdf_path: Path to the URDF file
        end_effector_frame: Name of the end effector frame/link
        joint_names: Optional list of joint names to control (if None, uses all joints)
    Output:
        PinocchioContext containing model and data for FK/Jacobian computations
    """
    model = pin.buildModelFromUrdf(urdf_path)
    data = model.createData()

    # Get end effector frame ID
    if not model.existFrame(end_effector_frame):
        available_frames = [
            model.frames[i].name for i in range(int(model.nframes))  # type: ignore[arg-type]
        ]
        raise ValueError(
            f"Frame '{end_effector_frame}' not found. Available frames: {available_frames}"
        )
    ee_frame_id = model.getFrameId(end_effector_frame)

    # Get joint IDs for the specified joint names
    actual_joint_names: list[str]
    if joint_names is None:
        # Use all actuated joints (exclude universe joint)
        joint_ids = list(range(1, int(model.njoints)))  # type: ignore[arg-type]
        actual_joint_names = [str(model.names[i]) for i in joint_ids]  # type: ignore[index]
    else:
        joint_ids = []
        model_names_list = list(model.names)  # type: ignore[arg-type]
        for name in joint_names:
            if name not in model_names_list:
                raise ValueError(f"Joint '{name}' not found in model")
            joint_ids.append(model.getJointId(name))
        actual_joint_names = joint_names

    return PinocchioContext(
        model=model,
        data=data,
        end_effector_frame_id=ee_frame_id,
        joint_names=actual_joint_names,
        joint_ids=joint_ids,
    )

compute_forward_kinematics(context: PinocchioContext, joint_positions: np.ndarray) -> SE3Pose

Compute forward kinematics for the end effector.

Input

context: Pinocchio context with model and data joint_positions: Joint positions array

Output: SE3Pose of the end effector

Source code in autolife_planning/kinematics/pinocchio_fk.py

def compute_forward_kinematics(
    context: PinocchioContext,
    joint_positions: np.ndarray,
) -> SE3Pose:
    """
    Compute forward kinematics for the end effector.

    Input:
        context: Pinocchio context with model and data
        joint_positions: Joint positions array
    Output:
        SE3Pose of the end effector
    """
    q = _to_pinocchio_config(context, joint_positions)
    pin.forwardKinematics(context.model, context.data, q)
    pin.updateFramePlacements(context.model, context.data)

    oMf = context.data.oMf[context.end_effector_frame_id]

    return SE3Pose(
        position=np.array(oMf.translation),
        rotation=np.array(oMf.rotation),
    )

compute_jacobian(context: PinocchioContext, joint_positions: np.ndarray, local_frame: bool = False) -> np.ndarray

Compute the Jacobian matrix at the end effector.

Input

context: Pinocchio context with model and data joint_positions: Joint positions array local_frame: If True, compute Jacobian in local frame; else world frame

Output: Jacobian matrix, shape (6, n_joints)

Source code in autolife_planning/kinematics/pinocchio_fk.py

def compute_jacobian(
    context: PinocchioContext,
    joint_positions: np.ndarray,
    local_frame: bool = False,
) -> np.ndarray:
    """
    Compute the Jacobian matrix at the end effector.

    Input:
        context: Pinocchio context with model and data
        joint_positions: Joint positions array
        local_frame: If True, compute Jacobian in local frame; else world frame
    Output:
        Jacobian matrix, shape (6, n_joints)
    """
    q = _to_pinocchio_config(context, joint_positions)
    pin.computeJointJacobians(context.model, context.data, q)
    pin.updateFramePlacements(context.model, context.data)

    reference_frame = pin.LOCAL if local_frame else pin.LOCAL_WORLD_ALIGNED

    J_full = pin.getFrameJacobian(
        context.model,
        context.data,
        context.end_effector_frame_id,
        reference_frame,
    )

    # Extract columns for controlled joints
    J = _extract_controlled_jacobian(context, J_full)

    return J