portfolio home/robotics lab/ notebook

← Back to experiment

Guided analysis

leg workspace and reachability

Exported without re-executing the notebook.

Source

experiments/2026-08-13-c1n-leg-workspace/c1n_leg_workspace.ipynb

Commit

1075e95d2c56ee08d8b757cf6ed45eaa9ac6337a

a local model path is redacted in this display export. numerical outputs and source notebooks are unchanged.

guided analysis: some cells remain unrun. the constrained solve below was executed; its retained output and the written conclusion support the one-target reachability result.

download the .ipynb ↗

C-1N front-left leg workspace — analysis companion

tl;dr

The current two-joint leg does not reach the target within the 1 cm grid tolerance. Its nearest sampled position is [+0.341, +0.267, -0.301] m, 0.067 m from [+0.30, +0.32, -0.30] m.

This is a gravity-free kinematic result. It identifies missing lateral placement freedom; it does not make a torque or controller claim.

Context & Methods

Key assumptions

• Source model: spider/model/spider.xml at the current workspace revision.
• Torso is fixed at the body-frame origin.
• Gravity is disabled.
• Only front_left_hip and front_left_knee vary within their declared limits.
• Target foot position is [+0.30, +0.32, -0.30] m in the torso frame.
• A nearest-sample distance of at most 0.01 m counts as reached for this 1° grid probe.

Data

1. Load current C-1N kinematics

Show code

import mujoco
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

from c1n_leg_workspace import (
    MODEL_PATH,
    REACH_TOLERANCE_M as TOLERANCE_M,
    TARGET,
    load_fixed_model,
)

model, data = load_fixed_model()
torso_id = model.body('torso').id
site_id = model.site('front_left_contact_site').id
hip = model.joint('front_left_hip')
knee = model.joint('front_left_knee')
print({'model': str(MODEL_PATH), 'hip_axis': hip.axis.tolist(), 'knee_axis': knee.axis.tolist(), 'hip_range_deg': np.rad2deg(hip.range).round(1).tolist(), 'knee_range_deg': np.rad2deg(knee.range).round(1).tolist()})

{'model': '[local model path redacted]', 'hip_axis': [0.0, 1.0, 0.0], 'knee_axis': [0.0, 1.0, 0.0], 'hip_range_deg': [-45.0, 45.0], 'knee_range_deg': [-80.0, 80.0]}

Results

2. Sample the two-joint workspace

Show code

hip_samples = np.linspace(*hip.range, 91)
knee_samples = np.linspace(*knee.range, 161)
rows = []
for hip_angle in hip_samples:
    for knee_angle in knee_samples:
        data.qpos[:] = 0.0
        data.qpos[3] = 1.0
        data.qpos[hip.qposadr[0]] = hip_angle
        data.qpos[knee.qposadr[0]] = knee_angle
        mujoco.mj_forward(model, data)
        foot_body = data.site_xpos[site_id] - data.xpos[torso_id]
        rows.append((hip_angle, knee_angle, *foot_body))
workspace = pd.DataFrame(rows, columns=['hip_rad', 'knee_rad', 'x_m', 'y_m', 'z_m'])
workspace['distance_to_target_m'] = np.linalg.norm(workspace[['x_m', 'y_m', 'z_m']].to_numpy() - TARGET, axis=1)
nearest = workspace.nsmallest(1, 'distance_to_target_m').iloc[0]
reached = bool(nearest.distance_to_target_m <= TOLERANCE_M)
print(f'grid samples: {len(workspace)}')
print(f'target reached within {TOLERANCE_M:.3f} m: {reached}')
nearest

Show code

fig, axes = plt.subplots(1, 2, figsize=(12, 5))
axes[0].scatter(workspace.x_m, workspace.y_m, s=1, alpha=0.25, color='#2f69ad')
axes[0].scatter(*TARGET[:2], marker='x', s=90, linewidths=2, color='#c43c3c', label='target')
axes[0].set(xlabel='body X (m)', ylabel='body Y (m)', title='Top view: reachable foot positions')
axes[0].axis('equal'); axes[0].legend()
axes[1].scatter(workspace.x_m, workspace.z_m, s=1, alpha=0.25, color='#2f69ad')
axes[1].scatter(TARGET[0], TARGET[2], marker='x', s=90, linewidths=2, color='#c43c3c', label='target')
axes[1].set(xlabel='body X (m)', ylabel='body Z (m)', title='Side view: reachable foot positions')
axes[1].axis('equal'); axes[1].legend()
fig.tight_layout()

Geometry preview before the experimental DOF test

This section does not sample a modified leg or answer whether a third DOF reaches the target. It only draws the current front-left hip frame at the neutral pose.

• Blue is the current hinge axis. Both existing joints use this local axis, so they bend in the same plane.
• Orange is one hypothetical, perpendicular proximal axis. It is shown only as a candidate experimental direction.
• The translucent blue disk is the current bending plane.
• The red point is the outward-and-down target.

Read the picture as geometry: which motion can each axis create at the foot? Before executing any changed-workspace sweep, state your prediction in the experiment log.

Show code

# Draw the neutral hip frame in torso coordinates; no modified model is sampled here.
from mpl_toolkits.mplot3d import Axes3D  # registers the 3D projection

data.qpos[:] = 0.0
data.qpos[3] = 1.0
mujoco.mj_forward(model, data)
hip_body = model.body('front_left').id
hip_origin = data.xpos[hip_body] - data.xpos[torso_id]
hip_rotation = data.xmat[hip_body].reshape(3, 3)
current_axis = hip_rotation @ np.array([0.0, 1.0, 0.0])
hypothetical_axis = hip_rotation @ np.array([0.0, 0.0, 1.0])
link_direction = hip_rotation @ np.array([1.0, 0.0, 0.0])

fig = plt.figure(figsize=(8, 7))
axis = fig.add_subplot(111, projection='3d')
for vector, color, label in [
    (current_axis, '#2f69ad', 'current shared hinge axis'),
    (hypothetical_axis, '#e6822d', 'hypothetical perpendicular axis'),
    (link_direction, '#555555', 'neutral upper-link direction'),
]:
    axis.quiver(*hip_origin, *vector, length=0.16, normalize=True, color=color, linewidth=2, label=label)

# The current bend plane is orthogonal to the current hinge axis.
plane_u = link_direction / np.linalg.norm(link_direction)
plane_v = np.cross(current_axis, plane_u); plane_v /= np.linalg.norm(plane_v)
angles = np.linspace(0, 2 * np.pi, 100)
ring = hip_origin[:, None] + 0.14 * (plane_u[:, None] * np.cos(angles) + plane_v[:, None] * np.sin(angles))
axis.plot(ring[0], ring[1], ring[2], color='#2f69ad', alpha=0.45, label='current bend plane')
axis.scatter(*hip_origin, color='black', s=35, label='front-left hip')
axis.scatter(*TARGET, color='#c43c3c', marker='x', s=100, linewidths=3, label='support target')
axis.set(xlabel='body X (m)', ylabel='body Y (m)', zlabel='body Z (m)', title='Neutral hip geometry in torso coordinates')
axis.legend(loc='upper left')
axis.set_box_aspect((1, 1, 1))
fig.tight_layout()

Your next prediction

Use the picture to make your own prediction. Do not look for a right answer from the notebook yet.

1. If the orange axis were enabled at the hip, which direction do you think the foot would first move when that joint turns positive?

• orthogonal to the body of the robot?

2. Would that motion help, hurt, or leave unchanged the miss between the nearest current foot point and the red target?

• it would help

3. What observation in the next workspace sweep would make you revise that prediction?

• a different angular direction of the foot when the joint turns positive -> finding out that the motion pulled us further from the red target

Experiment log

Stage	Record
Iteration 0	The target should be reachable. Joint-axis orientation is the uncertainty.
Current-model result	Target is not reached within 1 cm. Nearest sample is 6.7 cm away, chiefly 5.3 cm short in body-Y outward placement.
Interpretation	The two existing hinges share local axis [0, 1, 0], so the leg lacks an independent yaw-like placement direction. This is a kinematic result; gravity and torque were disabled.
Experimental-DOF prediction	User predicts the added orthogonal proximal axis helps. Falsifier: a positive turn moves the foot farther from the target.

Experimental orthogonal-hinge comparison

Recorded prediction

The user predicts that enabling the orange proximal axis will help the foot approach the red target. They would revise this if the actual positive rotation moves the foot in a direction that increases its target distance.

3. Add one candidate proximal axis only in this fixture

The code below changes an in-memory copy of the current XML. It adds a front_left_coxa_experimental hinge with local axis [0, 0, 1] immediately before the existing front-left hip. It does not edit spider.xml or claim a morphology decision. We hold gravity off and repeat the same target comparison.

Show code

from itertools import product

xml_text = MODEL_PATH.read_text(encoding='utf-8')
needle = '<joint name="front_left_hip" type="hinge" axis="0 1 0" range="-45 45"/>'
replacement = (
    '<joint name="front_left_coxa_experimental" type="hinge" axis="0 0 1" range="-45 45"/>'
    + needle
)
assert xml_text.count(needle) == 1
experimental_model = mujoco.MjModel.from_xml_string(xml_text.replace(needle, replacement))
experimental_model.opt.gravity[:] = 0.0
experimental_data = mujoco.MjData(experimental_model)
experimental_torso = experimental_model.body('torso').id
experimental_site = experimental_model.site('front_left_contact_site').id
coxa = experimental_model.joint('front_left_coxa_experimental')
experimental_hip = experimental_model.joint('front_left_hip')
experimental_knee = experimental_model.joint('front_left_knee')
print({
    'added_axis_local': coxa.axis.tolist(),
    'added_range_deg': np.rad2deg(coxa.range).round(1).tolist(),
    'current_hip_axis_local': experimental_hip.axis.tolist(),
})

{'added_axis_local': [0.0, 0.0, 1.0], 'added_range_deg': [-45.0, 45.0], 'current_hip_axis_local': [0.0, 1.0, 0.0]}

4. Measure the immediate positive-axis motion

This is the direct check for the direction part of the prediction. It holds the old nearest hip and knee angles fixed, then applies a small positive experimental angle. It reports the foot displacement in torso coordinates and whether that step reduces target distance.

Show code

def foot_position(model, data, coxa_angle, hip_angle, knee_angle):
    data.qpos[:] = 0.0
    data.qpos[3] = 1.0
    data.qpos[coxa.qposadr[0]] = coxa_angle
    data.qpos[experimental_hip.qposadr[0]] = hip_angle
    data.qpos[experimental_knee.qposadr[0]] = knee_angle
    mujoco.mj_forward(model, data)
    return data.site_xpos[experimental_site] - data.xpos[experimental_torso]

base = foot_position(experimental_model, experimental_data, 0.0, nearest.hip_rad, nearest.knee_rad)
positive = foot_position(experimental_model, experimental_data, np.deg2rad(5.0), nearest.hip_rad, nearest.knee_rad)
print({
    'base_foot_m': np.round(base, 4),
    'positive_5deg_foot_m': np.round(positive, 4),
    'positive_displacement_m': np.round(positive - base, 4),
    'base_target_distance_m': round(float(np.linalg.norm(base - TARGET)), 4),
    'positive_target_distance_m': round(float(np.linalg.norm(positive - TARGET)), 4),
})

5. Coarse sweep, then local refinement

The target and gravity setting remain fixed. The first grid maps the added degree of freedom at a readable cost. The second grid refines only around the nearest coarse sample. This is a sampling strategy, not an additional model change.

Show code

# Coarse workspace map.
coxa_samples = np.linspace(*coxa.range, 21)
experimental_hip_samples = np.linspace(*experimental_hip.range, 31)
experimental_knee_samples = np.linspace(*experimental_knee.range, 41)
experimental_rows = []
for coxa_angle, hip_angle, knee_angle in product(coxa_samples, experimental_hip_samples, experimental_knee_samples):
    foot_body = foot_position(experimental_model, experimental_data, coxa_angle, hip_angle, knee_angle)
    experimental_rows.append((coxa_angle, hip_angle, knee_angle, *foot_body))
experimental_workspace = pd.DataFrame(experimental_rows, columns=['coxa_rad', 'hip_rad', 'knee_rad', 'x_m', 'y_m', 'z_m'])
experimental_workspace['distance_to_target_m'] = np.linalg.norm(experimental_workspace[['x_m', 'y_m', 'z_m']].to_numpy() - TARGET, axis=1)
coarse_nearest = experimental_workspace.nsmallest(1, 'distance_to_target_m').iloc[0]

# Refine one coarse-grid interval on each side of the nearest sample.
coxa_step = coxa_samples[1] - coxa_samples[0]
hip_step = experimental_hip_samples[1] - experimental_hip_samples[0]
knee_step = experimental_knee_samples[1] - experimental_knee_samples[0]
refined_rows = []
for coxa_angle, hip_angle, knee_angle in product(
    np.linspace(coarse_nearest.coxa_rad - coxa_step, coarse_nearest.coxa_rad + coxa_step, 25),
    np.linspace(coarse_nearest.hip_rad - hip_step, coarse_nearest.hip_rad + hip_step, 25),
    np.linspace(coarse_nearest.knee_rad - knee_step, coarse_nearest.knee_rad + knee_step, 25),
):
    if not (coxa.range[0] <= coxa_angle <= coxa.range[1] and experimental_hip.range[0] <= hip_angle <= experimental_hip.range[1] and experimental_knee.range[0] <= knee_angle <= experimental_knee.range[1]):
        continue
    foot_body = foot_position(experimental_model, experimental_data, coxa_angle, hip_angle, knee_angle)
    refined_rows.append((coxa_angle, hip_angle, knee_angle, *foot_body))
refined = pd.DataFrame(refined_rows, columns=['coxa_rad', 'hip_rad', 'knee_rad', 'x_m', 'y_m', 'z_m'])
refined['distance_to_target_m'] = np.linalg.norm(refined[['x_m', 'y_m', 'z_m']].to_numpy() - TARGET, axis=1)
experimental_nearest = refined.nsmallest(1, 'distance_to_target_m').iloc[0]
experimental_reached = bool(experimental_nearest.distance_to_target_m <= TOLERANCE_M)
print(f'coarse grid samples: {len(experimental_workspace)}; refinement samples: {len(refined)}')
print(f'target reached within {TOLERANCE_M:.3f} m: {experimental_reached}')
experimental_nearest

Show code

fig, axes = plt.subplots(1, 2, figsize=(12, 5))
for frame, color, label in [(workspace, '#a7bdd3', 'current 2-DOF'), (experimental_workspace, '#2f69ad', 'experimental 3-DOF')]:
    axes[0].scatter(frame.x_m, frame.y_m, s=0.25, alpha=0.10, color=color, label=label)
    axes[1].scatter(frame.x_m, frame.z_m, s=0.25, alpha=0.10, color=color, label=label)
for axis, coordinates, title in [(axes[0], TARGET[:2], 'Top view'), (axes[1], TARGET[[0, 2]], 'Side view')]:
    axis.scatter(*coordinates, marker='x', s=100, linewidths=2, color='#c43c3c', label='target')
    axis.set(xlabel='body X (m)', title=title); axis.axis('equal'); axis.legend(markerscale=10)
axes[0].set_ylabel('body Y (m)')
axes[1].set_ylabel('body Z (m)')
fig.tight_layout()

Constrained inverse-kinematics analysis

The workspace grid shows shape, but it estimates the nearest point at its chosen resolution. This section sets up the more precise reachability question:

$$ q^* = \operatorname*{arg\,min}_{q_{\min} \leq q \leq q_{\max}} \frac{1}{2}\lVert p(q)-p^*\rVert^2 $$

p(q) is the foot position in torso coordinates. MuJoCo supplies the analytic position Jacobian $J(q) = \partial p / \partial q$. At an interior optimum, $J(q)^T (p(q)-p^*) = 0$. At a joint limit, a bound can also prevent further improvement.

This is a kinematic analysis only. It cannot establish contact force, static support, or dynamic stability.

Show code

from scipy.optimize import differential_evolution, least_squares

def foot_and_jacobian(model, data, torso_id, site_id, joint_names, angles):
    """Return torso-frame foot position and analytic translational Jacobian."""
    data.qpos[:] = 0.0
    data.qpos[3] = 1.0
    dof_addresses = []
    for joint_name, angle in zip(joint_names, angles):
        joint = model.joint(joint_name)
        data.qpos[joint.qposadr[0]] = angle
        dof_addresses.append(joint.dofadr[0])
    mujoco.mj_forward(model, data)
    torso_rotation = data.xmat[torso_id].reshape(3, 3)
    foot_body = torso_rotation.T @ (data.site_xpos[site_id] - data.xpos[torso_id])
    world_jacobian = np.zeros((3, model.nv))
    rotation_jacobian = np.zeros((3, model.nv))
    mujoco.mj_jacSite(model, data, world_jacobian, rotation_jacobian, site_id)
    body_jacobian = torso_rotation.T @ world_jacobian[:, dof_addresses]
    return foot_body, body_jacobian

def solve_target(model, data, torso_id, site_id, joint_names):
    joints = [model.joint(name) for name in joint_names]
    lower = np.array([joint.range[0] for joint in joints])
    upper = np.array([joint.range[1] for joint in joints])

    def residual(angles):
        foot, _ = foot_and_jacobian(model, data, torso_id, site_id, joint_names, angles)
        return foot - TARGET

    def jacobian(angles):
        _, jacobian_value = foot_and_jacobian(model, data, torso_id, site_id, joint_names, angles)
        return jacobian_value

    # Deterministic global search selects a basin; analytic-Jacobian least
    # squares then refines its local optimum under the same joint bounds.
    global_result = differential_evolution(
        lambda angles: 0.5 * np.dot(residual(angles), residual(angles)),
        bounds=list(zip(lower, upper)), seed=0, tol=1e-10, polish=False,
    )
    local_result = least_squares(
        residual, global_result.x, jac=jacobian, bounds=(lower, upper),
        xtol=1e-13, ftol=1e-13, gtol=1e-13, max_nfev=500,
    )
    foot, jacobian_value = foot_and_jacobian(model, data, torso_id, site_id, joint_names, local_result.x)
    return {
        'joint_names': joint_names,
        'angles_rad': local_result.x,
        'angles_deg': np.rad2deg(local_result.x),
        'foot_m': foot,
        'residual_m': foot - TARGET,
        'residual_norm_m': float(np.linalg.norm(foot - TARGET)),
        'jacobian_m_per_rad': jacobian_value,
        'gradient': jacobian_value.T @ (foot - TARGET),
        'active_lower_bounds': np.isclose(local_result.x, lower, atol=1e-8),
        'active_upper_bounds': np.isclose(local_result.x, upper, atol=1e-8),
        'optimizer_status': local_result.message,
    }

Your Iteration 0 for the solve

Before running the next cell, write your own predictions below. Focus on the meaning of the optimization, not on guessing a preferred outcome.

1. Do you expect the two-DOF solution to stop at a joint limit or at an interior closest point? Why?

• interior closest poiont; stopping at the actual joint limit sounds like it would cause the instability we're trying to address; from a calculus perspective it makes more sense to stop short as well, since the limit point is also an inflection point

2. Do you expect the experimental three-DOF solution to leave a nonzero residual, or reach the target? Why?

• non-zero residual, relatively small in size. I don't think we WANT a residual of zero? we want a residual of espilon as epsilon approaches zero instead, because actually hitting 0 would put us at an instable critical/inflection point in the stability graph

3. For the two-DOF solve, what would $J^T r \approx 0$ tell you about the remaining foot-target error?

• not sure, I think it tells you about the composed direction of movement required to minimize foot-target error; i'm treating it as analogous to a first order derivative in a canonical line of best fit problem, extended to vector calculus

Your notes:

current_solution = solve_target(
    model, data, torso_id, site_id, ['front_left_hip', 'front_left_knee']
)
experimental_solution = solve_target(
    experimental_model, experimental_data, experimental_torso, experimental_site,
    ['front_left_coxa_experimental', 'front_left_hip', 'front_left_knee'],
)

def summarize_solution(name, solution):
    print(f'\n{name}')
    print('angles (deg):', np.round(solution['angles_deg'], 4))
    print('foot (m):', np.round(solution['foot_m'], 6))
    print('residual (m):', np.round(solution['residual_m'], 6))
    print('residual norm (m):', f"{solution['residual_norm_m']:.9f}")
    print('J^T residual:', np.round(solution['gradient'], 10))
    print('active lower bounds:', solution['active_lower_bounds'])
    print('active upper bounds:', solution['active_upper_bounds'])

summarize_solution('Current two-DOF leg', current_solution)
summarize_solution('Experimental three-DOF leg', experimental_solution)

Current two-DOF leg
angles (deg): [-41.6549  76.0841]
foot (m): [ 0.341235  0.267222 -0.3     ]
residual (m): [ 0.041235 -0.052778 -0.      ]
residual norm (m): 0.066976422
J^T residual: [-0. -0.]
active lower bounds: [False False]
active upper bounds: [False False]

Experimental three-DOF leg
angles (deg): [  9.6905  29.9221 -73.952 ]
foot (m): [ 0.3   0.32 -0.3 ]
residual (m): [-0.  0.  0.]
residual norm (m): 0.000000000
J^T residual: [0. 0. 0.]
active lower bounds: [False False False]
active upper bounds: [False False False]

Read the result after you run it

• A nonzero minimum residual means the target is not exactly reachable under the model and bounds.
• $J^T r$ near zero means the solver cannot reduce distance through any locally allowed infinitesimal joint change.
• An active bound identifies a joint limit as part of the restriction.
• A near-zero residual means this target is kinematically reachable. It is still not a claim about contact force, torque, or stability.

Grid comparison

The grid is a visual check. After you run the constrained inverse-kinematics section, compare its residual and bound status with the grid result. The comparison tells us whether the grid resolution was adequate for this target.

Your conclusion

Use the measurements above. Write the interpretation in your own words.

1. Current mechanism: What does its best residual say about this target and its available foot-motion directions?

   Your answer: That the target was unreachable with the old configuration, is reachable with the new configruation, and we have the foot-motion directions required of the problem as we've constructed it mathematically/physically

2. Experimental mechanism: What changed when the orthogonal proximal hinge was available?

   Your answer: The legs gained a new DOF that allowed them to minimize the task space residual in the correct dimensions.

3. Meaning of the solve: What does a small $J^T r$ tell you here? What does it not tell you about a free, contacting robot?

   Your answer: That we were able to successfully minimize the J^t*r value, which in conjunction with minimized residual values, is required to reach the ground. This doesn't tell us about the ENTIRE robot, only about this leg configuration in task-space.

4. Morphology decision: Is this sufficient evidence to promote the experimental hinge into canonical C-1N morphology? State the evidence and the remaining uncertainty.

   Your answer: Yes. We see that adding the 3rd DOF reduces the residual domain to include zero. This is an objective increase in performance compared to the old morphology.

5. Next narrow claim: State one physical capability that must be established next before this workspace result can inform standing control. Name the measurement that would test only that capability.

   Your answer: Whether the leg can, in this configuration, transmit the required ground reaction force without slipping. Measure the ground reaction force for a foot under load and verify that it can produce that force without slipping.