{"id":50739843,"url":"https://github.com/mstoelzle/focodyn","last_synced_at":"2026-06-10T16:30:26.160Z","repository":{"id":354740104,"uuid":"1224956589","full_name":"mstoelzle/focodyn","owner":"mstoelzle","description":"Differentiable floating-base dynamics and contact-pose Jacobians for legged robots.","archived":false,"fork":false,"pushed_at":"2026-04-29T22:56:36.000Z","size":45618,"stargazers_count":0,"open_issues_count":0,"forks_count":0,"subscribers_count":0,"default_branch":"main","last_synced_at":"2026-04-29T23:08:12.233Z","etag":null,"topics":["humanoid","robot-learning","robotics"],"latest_commit_sha":null,"homepage":"","language":"Python","has_issues":true,"has_wiki":null,"has_pages":null,"mirror_url":null,"source_name":null,"license":null,"status":null,"scm":"git","pull_requests_enabled":true,"icon_url":"https://github.com/mstoelzle.png","metadata":{"files":{"readme":"README.md","changelog":null,"contributing":null,"funding":null,"license":null,"code_of_conduct":null,"threat_model":null,"audit":null,"citation":null,"codeowners":null,"security":null,"support":null,"governance":null,"roadmap":null,"authors":null,"dei":null,"publiccode":null,"codemeta":null,"zenodo":null,"notice":null,"maintainers":null,"copyright":null,"agents":null,"dco":null,"cla":null}},"created_at":"2026-04-29T19:57:08.000Z","updated_at":"2026-04-29T22:56:39.000Z","dependencies_parsed_at":null,"dependency_job_id":null,"html_url":"https://github.com/mstoelzle/focodyn","commit_stats":null,"previous_names":["mstoelzle/differentiable-humanoid-dynamics"],"tags_count":null,"template":false,"template_full_name":null,"purl":"pkg:github/mstoelzle/focodyn","repository_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/mstoelzle%2Ffocodyn","tags_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/mstoelzle%2Ffocodyn/tags","releases_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/mstoelzle%2Ffocodyn/releases","manifests_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/mstoelzle%2Ffocodyn/manifests","owner_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners/mstoelzle","download_url":"https://codeload.github.com/mstoelzle/focodyn/tar.gz/refs/heads/main","sbom_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories/mstoelzle%2Ffocodyn/sbom","scorecard":null,"host":{"name":"GitHub","url":"https://github.com","kind":"github","repositories_count":286080680,"owners_count":34161283,"icon_url":"https://github.com/github.png","version":null,"created_at":"2022-05-30T11:31:42.601Z","updated_at":"2026-05-26T15:22:16.424Z","status":"online","status_checked_at":"2026-06-10T02:00:07.152Z","response_time":89,"last_error":null,"robots_txt_status":"success","robots_txt_updated_at":"2025-07-24T06:49:26.215Z","robots_txt_url":"https://github.com/robots.txt","online":true,"can_crawl_api":true,"host_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub","repositories_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repositories","repository_names_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/repository_names","owners_url":"https://repos.ecosyste.ms/api/v1/hosts/GitHub/owners"}},"keywords":["humanoid","robot-learning","robotics"],"created_at":"2026-06-10T16:30:25.397Z","updated_at":"2026-06-10T16:30:26.154Z","avatar_url":"https://github.com/mstoelzle.png","language":"Python","funding_links":[],"categories":[],"sub_categories":[],"readme":"# FoCoDyn\n\nFoCoDyn, short for Floating Contact Dynamics, provides differentiable\nfloating-base dynamics and contact-pose Jacobians for legged robots. It is\naimed at CBF/CLF, trajectory optimization, and learning workflows that need\ngradients through rigid-body dynamics and contact kinematics.\n\nThe package wraps [Adam](https://github.com/gbionics/adam) through its PyTorch\nbackend. Adam provides the floating-base mass matrix, Coriolis/centrifugal\nterms, gravity terms, Jacobians, and forward kinematics. This repository adds a\nlegged-robot layer around those quantities:\n\n- an installable Python package with `uv` and Python \u003e=3.11,\n- vendored Unitree G1 URDF/MJCF assets to start,\n- a shared `torch.nn.Module` exposing control-affine `f(x)` and `g(x)`,\n- differentiable foot contact-pose forward kinematics and Jacobians, and\n- a Viser-based viewer that overlays contact frames on a retargeted G1 motion\n  reference.\n\n## Install\n\nThe distribution and import package name is `focodyn`.\n\n```bash\nuv sync --extra dev\n```\n\nFor the optional browser visualization:\n\n```bash\nuv sync --extra dev --extra viz\n```\n\nRun the Adam/G1 smoke check:\n\n```bash\nuv run focodyn-check-adam-g1\n```\n\nRun the root-level dynamics inspection example:\n\n```bash\nuv run python examples/inspect_g1_dynamics.py\n```\n\nRun the test suite:\n\n```bash\nuv run pytest\n```\n\n## Dynamics API\n\n```python\nimport torch\nfrom focodyn import FloatingBaseDynamics\n\nmodel = FloatingBaseDynamics(\n    \"unitree_g1\",\n    include_contact_forces=True,\n    contact_mode=\"feet_corners\",\n    dtype=torch.float64,\n)\n\nx = model.neutral_state()\nxdot_drift = model.f(x)\ncontrol_map = model.g(x)\nxdot_drift, control_map = model.f_and_g(x)  # shared mass matrix + solve\n```\n\nThe dynamics are represented as:\n\n```text\nx_dot = f(x) + g(x) u\nM(q) nu_dot + h(q, nu) = S^T tau + J_c(q)^T lambda\n```\n\n`u` is stacked as joint torques first, then optional contact forces\n`lambda = (f_c0, f_c1, ...)` with three force components per contact. By\ndefault contact-force inputs are world-frame vectors. Pass\n`contact_force_frame=\"contact\"` to express each force in its contact frame; the\nmodule rotates those forces into world coordinates before applying `J_c^T`.\n\n## State Convention\n\nThe state is `x = (q, nu)`.\n\n`q = (p_WB, quat_WB, s)`:\n\n- `p_WB` is the world-frame position of the floating-base/root link origin.\n- `quat_WB` is a unit quaternion in `(w, x, y, z)` order and maps base-frame\n  vectors into the world frame.\n- `s` is ordered exactly as `model.joint_names`, which is parsed from the URDF\n  and passed to Adam.\n\n`nu = (v_WB, omega_WB, s_dot)` uses Adam's mixed representation:\n\n- `v_WB` is the base linear velocity in the world frame.\n- `omega_WB` is the base angular velocity in the world frame.\n- `s_dot` follows the same joint order as `s`.\n\nThe generalized acceleration is `nu_dot = (v_dot_WB, omega_dot_WB, s_ddot)`.\n\n## Contacts\n\n`FloatingBaseContactModel` initializes contact candidates from URDF collision\ngeometry. For the Unitree G1, each ankle-roll link has four small sphere\ncollisions; `feet_corners` uses those eight sphere origins, and `feet_centers`\nuses one averaged point per foot. The same interface is intended to support\nother floating-base legged robots as assets and contact extraction modes are\nadded.\n\nContacts are full `SE(3)` frames, not only points. `contact_poses(...)` returns\nworld positions, `(w, x, y, z)` contact-to-world quaternions, and homogeneous\n`W_H_C` matrices. This matches the Adam transform convention used elsewhere in\nthe package. The contact normal is the contact frame's positive z-axis\nexpressed in world coordinates and is available through `contact_normals(...)`.\n\nThe translational contact Jacobian maps Adam mixed generalized velocity to\nstacked world-frame contact point velocities. The contact force contribution to\nthe equations of motion is `J_c(q)^T lambda` for world-frame forces, or\n`J_c(q)^T R_WC lambda_C` for contact-frame forces.\n\n`FlatTerrainContactDetector` evaluates each configured contact candidate\nagainst a flat terrain plane and returns nearest terrain points, signed\ndistances, penetration depths, world normals, and binary contact flags.\n`BasicContactForceResolver` provides a deterministic normal-force estimate for\ndebugging and visualization. It returns forces in the world frame by default,\nor in each local contact frame with `force_frame=\"contact\"`.\n\n## Motion Derivatives\n\nKinematic references can be differentiated with `torch-dxdt` Whittaker-Eilers\nfilters to estimate `nu` and `nu_dot` for inverse-dynamics inspection. To\ncompare smoothing parameters and save a plot of raw/smoothed motion,\nvelocities, and accelerations:\n\n```bash\nuv run --extra viz python examples/plot_g1_motion_derivatives.py --lambdas 10 100 1000 10000 --output outputs/motion_derivative_lambdas.pdf\n```\n\n## Visualization\n\nThe `viz` extra provides three Viser-based viewers.\n\n### Kinematic Trajectory Viewer\n\nStart the standard kinematic trajectory viewer with:\n\n```bash\nuv run --extra viz focodyn-visualize-g1\n```\n\nOr run the root-level visualization example:\n\n```bash\nuv run --extra viz python examples/visualize_g1.py\n```\n\nThe viewer loads the local G1 URDF, plays the bundled retargeted AMASS/G1\nkinematic reference, and overlays the differentiable contact-frame FK estimates.\nThe Viser side panel includes playback buttons, a frame scrubber, and a motion\ndropdown for switching between bundled kinematic references.\nPass `--synthetic-motion` to recover the older deterministic fallback sequence,\nor `--motion-reference /path/to/file.npz` / `--motion-reference /path/to/file.npy`\nto visualize another supported retargeted G1 motion.\n\nTo export the standard kinematic visualization as an MP4, use the Viser render\nexport path rather than a manual start/stop recording button:\n\n```bash\nuv run --extra viz focodyn-visualize-g1 \\\n  --export-video outputs/videos/kinematic_trajectory.mp4 \\\n  --export-width 1280 \\\n  --export-height 720 \\\n  --port 0\n```\n\n### Dynamics Verification Viewer\n\nFor inspecting the dynamics maps, use the specialized viewer:\n\n```bash\nuv run --extra viz focodyn-visualize-dynamics --whittaker-lambda 100\n```\n\nThe dynamics viewer inherits the kinematic trajectory viewer and adds two\nmodes. `f(x): inverse dynamics` ignores contacts and visualizes the virtual\nroot wrench plus the largest joint torques from `M(q) nu_dot + h(q,nu)`.\n`g(x): contact projection` resolves flat-ground contact forces, projects them\nthrough the contact-force input block, and visualizes the resulting root wrench,\njoint torques, and contact force vectors. The dynamics viewer renders the robot\nsemi-transparently by default and includes a `Joint torque viz` selector for\nshowing torque arrows, text labels, or both. Override the transparency with\n`--robot-opacity 0.5` when needed.\n\nExport both dynamics verification videos with:\n\n```bash\nuv run --extra viz focodyn-visualize-dynamics \\\n  --export-dynamics-videos outputs/videos \\\n  --export-width 1280 \\\n  --export-height 720 \\\n  --port 0\n```\n\nVideo export requires `ffmpeg` and a Chromium-compatible browser. The exporter\nlaunches a temporary headless browser client, captures each frame with Viser,\nand writes H.264 MP4 files. Pass `--export-frames N` for a shorter clip during\ntests, or `--export-browser /path/to/browser` if Chrome/Chromium is not on the\nstandard path.\n\n### Input Constraint Verification Viewer\n\nFor checking the affine input constraints along a kinematic reference, use:\n\n```bash\nuv run --extra viz focodyn-visualize-input-constraints --whittaker-lambda 100\n```\n\nThis viewer follows the same derivative-estimation path as the dynamics\nverification viewer. For each frame, it estimates `nu_dot`, computes\n`M(q) nu_dot + h(q, nu)`, uses the actuated block as the joint torque estimate,\nand resolves rough flat-ground contact forces in the local contact frames. It\nthen builds `u = [tau, lambda_C]` and evaluates the joint torque, positive\nnormal force, and linearized friction cone constraints.\n\nRed arrows indicate constraint violations. Joint arrows are drawn at the\nviolating joints and point along the joint axis, while contact arrows mark\nnegative normal-force or friction-cone violations at the contact frames. The\nside panel exposes the URDF effort-limit scale, friction coefficient, number of\nfriction facets (eight by default), conservative/outer cone mode, tangential\ndamping used by the rough contact resolver, and separate scales for force and\ntorque violation arrows.\n\nThe same mode is available from the main viewer:\n\n```bash\nuv run --extra viz focodyn-visualize-g1 --input-constraint-verification\n```\n\n## Sources And Attribution\n\nRigid-body dynamics are computed by\n[Adam](https://github.com/gbionics/adam), distributed on PyPI as\n`adam-robotics`. This package depends on `adam-robotics[pytorch]` and does not\nreimplement CRBA, RNEA, ABA, frame Jacobians, or forward kinematics.\n\nRobot assets under\n`src/focodyn/assets/robots/unitree_g1` come from\n[unitreerobotics/unitree_ros](https://github.com/unitreerobotics/unitree_ros/tree/master/robots/g1_description).\nThe upstream BSD 3-Clause license is included at\n`src/focodyn/assets/robots/unitree_g1/LICENSE.unitree_ros`.\n\nThe default bundled sample kinematic motion under\n`src/focodyn/assets/motions/g1_fleaven_retargeted` is\n`g1/Transitions_mocap/mazen_c3d/JOOF_walk_poses_120_jpos.npy` from\n[fleaven/Retargeted_AMASS_for_robotics](https://huggingface.co/datasets/fleaven/Retargeted_AMASS_for_robotics).\nThat dataset is distributed under CC-BY-4.0 and contains AMASS motions\nretargeted to Unitree G1. The raw root quaternion is documented by the source\ndataset in `(x, y, z, w)` order; this package converts it to the `(w, x, y, z)`\nconvention used by Adam-facing APIs.\n\nAn additional bundled reference under\n`src/focodyn/assets/motions/g1_amass_retargeted` is\n`g1/CMU/06/06_01_poses_120_jpos.npz` from\n[ember-lab-berkeley/AMASS_Retargeted_for_G1](https://huggingface.co/datasets/ember-lab-berkeley/AMASS_Retargeted_for_G1).\nThat dataset is distributed under CC-BY-4.0 and contains AMASS motions\nretargeted to Unitree G1. The original CMU source motion is from the\n[CMU Graphics Lab Motion Capture Database](https://mocap.cs.cmu.edu/).\n\nFiles ending in `.adam.urdf` are generated compatibility copies of the upstream\nURDFs. They add identity joint origins where the upstream URDF omits them and\nremove Unitree/MuJoCo-specific XML that `urdf_parser_py` warns about. The\noriginal upstream URDFs remain vendored unchanged and are used for source\ninspection and visualization.\n\nThe visualization uses [Viser](https://viser.studio/) when the optional\n`viz` extra is installed. Whittaker-Eilers derivatives use\n[torch-dxdt](https://github.com/mstoelzle/torch-dxdt).\n","project_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fmstoelzle%2Ffocodyn","html_url":"https://awesome.ecosyste.ms/projects/github.com%2Fmstoelzle%2Ffocodyn","lists_url":"https://awesome.ecosyste.ms/api/v1/projects/github.com%2Fmstoelzle%2Ffocodyn/lists"}