Articulated Bodies¶

Overview¶

NovaPhy implements the Featherstone articulated body algorithms for efficient simulation of jointed rigid body systems (robots, pendulums, chains, etc.).

Joint Types¶

Type	DOF	Description
`Revolute`	1	Rotation about a single axis
`Prismatic`	1	Translation along a single axis
`Ball`	3	Spherical joint (3 rotational DOF)
`Fixed`	0	Rigid attachment
`Free`	6	Unconstrained (floating base)

Articulation Evaluators¶

The public Python evaluator surface operates on a finalized Model, caller-owned SimState, and the model's flat generalized-coordinate buffers. Use these helpers when you need kinematic or matrix readback without stepping a solver.

Forward Kinematics (FK)¶

Writes body transforms and velocities into a SimState from flat joint coordinates:

joint_q = state.joint_q.numpy()
joint_qd = state.joint_qd.numpy()
novaphy.eval_fk(model, joint_q, joint_qd, state)

Inverse Kinematics Projection¶

Writes flat joint coordinates / velocities from a maximal body state into writable arrays:

joint_q = state.joint_q.numpy().copy()
joint_qd = state.joint_qd.numpy().copy()
novaphy.eval_ik(model, state, joint_q, joint_qd)

Jacobian and Mass Matrix¶

Return per-articulation matrices, or fill preallocated outputs when provided:

J = novaphy.eval_jacobian(model, state)
H = novaphy.eval_mass_matrix(model, state)

The older list-based helper names (forward_kinematics, inverse_dynamics, mass_matrix_crba, forward_dynamics) are not part of the current top-level novaphy.__all__ surface. Prefer the eval_* helpers or the full SolverFeatherstone path below.

Creating Articulated Bodies¶

import numpy as np
import novaphy

builder = novaphy.ModelBuilder()

link = builder.add_link(
    xform=novaphy.Transform.from_translation(
        np.array([0.0, 1.0, 0.0], dtype=np.float32)
    ),
    mass=1.0,
    inertia=np.diag(np.array([0.1, 0.02, 0.1], dtype=np.float32)),
    lock_inertia=True,
    label="pendulum",
)
builder.add_shape_box(
    link,
    hx=0.1,
    hy=0.5,
    hz=0.1,
    cfg=novaphy.ShapeConfig(density=0.0),
)

hinge = builder.add_joint_revolute(
    parent=-1,
    child=link,
    axis=np.array([0.0, 0.0, 1.0], dtype=np.float32),
    label="hinge",
)
builder.add_articulation([hinge], label="pendulum")

Driving the Featherstone solver¶

For full simulation with collision detection, construct novaphy.solvers.SolverFeatherstone(model, ...) directly and call its Newton-aligned step:

model = builder.finalize()

config = novaphy.solvers.SolverFeatherstone.Config()
config.angular_damping = 0.05
config.update_mass_matrix_interval = 1
solver = novaphy.solvers.SolverFeatherstone(model, config)
state    = model.state()
control  = model.control()
collision_pipeline = novaphy.CollisionPipeline(model)
contacts = collision_pipeline.contacts()

for _ in range(1000):
    state.clear_forces()
    collision_pipeline.collide(state, contacts)
    solver.step(state, state, control, contacts, 1.0 / 120.0)

Spatial Algebra Convention¶

NovaPhy uses Newton-compatible linear-first 6D spatial vectors throughout:

Spatial velocity: [linear_x, linear_y, linear_z, angular_x, angular_y, angular_z]
Spatial force: [force_x, force_y, force_z, torque_x, torque_y, torque_z]

Free joint generalized coordinates:

q = [px, py, pz, qx, qy, qz, qw] (7 DOF: position + quaternion)
qd = [vx, vy, vz, wx, wy, wz] (6 DOF: linear + angular velocity)

The same component order is used by C++ novaphy::SpatialVector, spatial-algebra helpers, and the flat rigid-body state views: state.body_qd[i] == [vx, vy, vz, wx, wy, wz] and state.body_f[i] == [fx, fy, fz, tx, ty, tz].

Demos¶

Demo	Description
`featherstone/demo_fs_pgs_cradle.py`	Newton's cradle with hinge suspension and contacts
`featherstone/demo_fs_pgs_rope.py`	Articulated chain
`featherstone/demo_fs_pgs_wrecking_ball.py`	Chain pendulum and rigid contact
`featherstone/demo_fs_pgs_seesaw.py`	Revolute joint and impact
`featherstone/demo_fs_pgs_motor_arm.py`	Driven articulated arm
`featherstone/demo_fs_pgs_100_robots.py`	Robot-grid stress scene

Run these paths from python/demos/, for example:

python python/demos/featherstone/demo_fs_pgs_rope.py \
  --headless 120