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` / `Slide`	1	Translation along a single axis
`Ball`	3	Spherical joint (3 rotational DOF)
`Fixed`	0	Rigid attachment
`Free`	6	Unconstrained (floating base)

Featherstone Algorithms¶

Forward Kinematics (FK)¶

Computes body transforms from joint coordinates q:

transforms = novaphy.forward_kinematics(joints, q)

Inverse Dynamics (RNEA)¶

Computes joint torques required for a given motion:

tau = novaphy.inverse_dynamics(joints, bodies, q, qd, qdd, gravity)

Mass Matrix (CRBA)¶

Computes the joint-space mass matrix:

H = novaphy.mass_matrix_crba(joints, bodies, q)

Forward Dynamics¶

Computes joint accelerations from applied torques:

qdd = novaphy.forward_dynamics(joints, bodies, q, qd, tau, gravity)

Creating Articulated Bodies¶

import numpy as np
import novaphy

# Define a revolute joint
joint = novaphy.Joint()
joint.type = novaphy.JointType.Revolute
joint.axis = np.array([0, 0, 1])
joint.parent = -1

# Define the link body
body = novaphy.RigidBody.from_box(1.0, np.array([0.1, 0.5, 0.1]))

joints = [joint]
bodies = [body]

Driving the Featherstone solver¶

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

builder = novaphy.ModelBuilder()
builder.add_ground_plane(y=0.0)
body_index = builder.add_body(body)
dof = novaphy.JointDofConfig()
dof.axis = np.array([0, 0, 1], dtype=np.float32)
joint_index = builder.add_joint_revolute(parent=-1, child=body_index, axis=dof)
builder.add_articulation([joint_index])
model = builder.finalize()

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

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

Spatial Algebra Convention¶

NovaPhy follows the Featherstone convention for 6D spatial vectors:

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

Free joint generalized coordinates:

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

Flat rigid-body state views use component order instead: state.body_qd[i] == [vx, vy, vz, wx, wy, wz] and state.body_f[i] == [fx, fy, fz, tx, ty, tz].

Demos¶

Demo	Description
`demo_newtons_cradle_pendulum.py`	Newton's cradle with hinge suspension
`demo_rope.py`	15-link chain released from horizontal
`demo_wrecking_ball.py`	Chain pendulum smashing a box tower
`demo_seesaw.py`	Box dropped onto revolute seesaw
`demo_motor_arm.py`	Motor-driven arm sweeping boxes
`demo_100_robots.py`	Quadruped grid via Featherstone / XPBD