Skip to content

Kinematics

Forward Kinematics (Pinocchio)

Pinocchio-based forward kinematics and Jacobian computation.

Build a context on a robot's URDF (RobotConfig.urdf_path) with the camera link as the end-effector frame to get the camera pose of a configuration::

from nav_planning.locobot import locobot_robot_config as config

ctx = create_pinocchio_context(config.urdf_path, config.camera.link_name)
pose = compute_forward_kinematics(ctx, q)     # q: full 5-DOF config

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

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 nav_planning/kinematics/pinocchio_fk.py 
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
84
85
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 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 nav_planning/kinematics/pinocchio_fk.py 
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
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 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 nav_planning/kinematics/pinocchio_fk.py 
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
143
144
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