Skip to content

Kinematics

Forward Kinematics (Pinocchio)

pinocchio_fk

Pinocchio-based forward kinematics and Jacobian computation.

For example the pose of the gimbal camera for a configuration::

from drone_planning.x500 import x500_robot_config as cfg

ctx = create_pinocchio_context(cfg.urdf_path, cfg.camera.link_name)
pose = compute_forward_kinematics(ctx, q)   # q: 6-DOF configuration

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 drone_planning/kinematics/pinocchio_fk.py 
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
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 drone_planning/kinematics/pinocchio_fk.py 
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
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 drone_planning/kinematics/pinocchio_fk.py 
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
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