openarm-control 0.1.0__tar.gz

openarm_control-0.1.0/LICENSE.txt

@@ -0,0 +1,202 @@
+
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.

openarm_control-0.1.0/PKG-INFO

@@ -0,0 +1,81 @@
+Metadata-Version: 2.4
+Name: openarm_control
+Version: 0.1.0
+Summary: Kinematics and control utilities for OpenArm
+Author: Enactic, Inc.
+License: Apache-2.0
+Project-URL: Changelog, https://github.com/enactic/openarm_control/releases
+Project-URL: Issues, https://github.com/enactic/openarm_control/issues
+Project-URL: Repository, https://github.com/enactic/openarm_control.git
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+License-File: LICENSE.txt
+Requires-Dist: daqp==0.7.2
+Requires-Dist: mink>=1.1.0
+Requires-Dist: mujoco>=3.6.0
+Requires-Dist: numpy
+Requires-Dist: openarm-mujoco
+Dynamic: license-file
+
+# OpenArm Control
+
+Reusable kinematics and control utilities for OpenArm, backed by MuJoCo and [mink](https://github.com/kevinzakka/mink).
+
+## Install
+
+```bash
+uv sync
+```
+
+## Usage
+
+### `Kinematics`
+
+```python
+from openarm_control import Kinematics, IKParams, ArmSetup
+
+# FK only
+kin = Kinematics(setup)
+pose = kin.fk("right", joints)               # float32[7]
+pose_r, pose_l = kin.fk_bimanual(r, l)       # single mj_forward
+
+# IK
+kin = Kinematics(setup, IKParams(damping=0.25, posture_cost=0.01))
+kin.set_target("right", pose_r)
+kin.set_target("left", pose_l)
+result = kin.solve(dt=0.1, n_iters=5)        # float32[16] right[8]+left[8]
+```
+
+### `IKParams`
+
+Solver configuration passed to `Kinematics`. All fields have defaults.
+
+| Field | Default | Description |
+|---|---|---|
+| `position_cost` | `1.0` | Position task weight |
+| `orientation_cost` | `1.0` | Orientation task weight |
+| `lm_damping` | `0.01` | Per-task Levenberg-Marquardt damping |
+| `damping` | `0.25` | Global Tikhonov regularization |
+| `solver` | `"daqp"` | QP backend |
+| `posture_cost` | `0.01` | Neutral posture task weight (0 = disabled) |
+| `diag_reg` | `0.0` | QP diagonal regularization |
+| `dt` | `0.1` | Integration timestep per iteration |
+| `max_iters` | `5` | IK iterations per solve |
+| `velocity_limits` | `None` | Per-joint velocity limits (applied in rad/s from `config.py`); `None` = disabled |
+
+Build from CLI args with `register_ik_args` + `ik_params_from_args`:
+
+## Related links
+
+- 💬 Join the community on [Discord](https://discord.gg/FsZaZ4z3We)
+- 📬 Contact us through <openarm@enactic.ai>
+
+## License
+
+Licensed under the Apache License 2.0. See [LICENSE.txt](LICENSE.txt) for details.
+
+Copyright 2026 Enactic, Inc.
+
+## Code of Conduct
+
+All participation in the OpenArm project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md).

openarm_control-0.1.0/README.md

@@ -0,0 +1,62 @@
+# OpenArm Control
+
+Reusable kinematics and control utilities for OpenArm, backed by MuJoCo and [mink](https://github.com/kevinzakka/mink).
+
+## Install
+
+```bash
+uv sync
+```
+
+## Usage
+
+### `Kinematics`
+
+```python
+from openarm_control import Kinematics, IKParams, ArmSetup
+
+# FK only
+kin = Kinematics(setup)
+pose = kin.fk("right", joints)               # float32[7]
+pose_r, pose_l = kin.fk_bimanual(r, l)       # single mj_forward
+
+# IK
+kin = Kinematics(setup, IKParams(damping=0.25, posture_cost=0.01))
+kin.set_target("right", pose_r)
+kin.set_target("left", pose_l)
+result = kin.solve(dt=0.1, n_iters=5)        # float32[16] right[8]+left[8]
+```
+
+### `IKParams`
+
+Solver configuration passed to `Kinematics`. All fields have defaults.
+
+| Field | Default | Description |
+|---|---|---|
+| `position_cost` | `1.0` | Position task weight |
+| `orientation_cost` | `1.0` | Orientation task weight |
+| `lm_damping` | `0.01` | Per-task Levenberg-Marquardt damping |
+| `damping` | `0.25` | Global Tikhonov regularization |
+| `solver` | `"daqp"` | QP backend |
+| `posture_cost` | `0.01` | Neutral posture task weight (0 = disabled) |
+| `diag_reg` | `0.0` | QP diagonal regularization |
+| `dt` | `0.1` | Integration timestep per iteration |
+| `max_iters` | `5` | IK iterations per solve |
+| `velocity_limits` | `None` | Per-joint velocity limits (applied in rad/s from `config.py`); `None` = disabled |
+
+Build from CLI args with `register_ik_args` + `ik_params_from_args`:
+
+## Related links
+
+- 💬 Join the community on [Discord](https://discord.gg/FsZaZ4z3We)
+- 📬 Contact us through <openarm@enactic.ai>
+
+## License
+
+Licensed under the Apache License 2.0. See [LICENSE.txt](LICENSE.txt) for details.
+
+Copyright 2026 Enactic, Inc.
+
+## Code of Conduct
+
+All participation in the OpenArm project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md).

openarm_control-0.1.0/pyproject.toml

@@ -0,0 +1,54 @@
+# Copyright 2026 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+[build-system]
+requires = ["setuptools>=64", "wheel"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "openarm_control"
+version = "0.1.0"
+description = "Kinematics and control utilities for OpenArm"
+authors = [{name = "Enactic, Inc."}]
+license = {text = "Apache-2.0"}
+readme = "README.md"
+requires-python = ">=3.10"
+
+dependencies = [
+    "daqp==0.7.2",
+    "mink>=1.1.0",
+    "mujoco>=3.6.0",
+    "numpy",
+    "openarm-mujoco",
+]
+
+[project.urls]
+Changelog = "https://github.com/enactic/openarm_control/releases"
+Issues = "https://github.com/enactic/openarm_control/issues"
+Repository = "https://github.com/enactic/openarm_control.git"
+
+[tool.setuptools]
+package-dir = {"" = "src"}
+
+[tool.setuptools.packages.find]
+where = ["src"]
+
+[tool.ruff.lint]
+extend-select = [
+  "D",   # pydocstyle
+  "UP",
+]
+
+[tool.ruff.lint.per-file-ignores]
+"!src/**.py" = ["D"]

openarm_control-0.1.0/setup.cfg

@@ -0,0 +1,4 @@
+[egg_info]
+tag_build = 
+tag_date = 0
+

openarm_control-0.1.0/src/openarm_control/__init__.py

@@ -0,0 +1,41 @@
+# Copyright 2026 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Kinematics and control utilities for OpenArm."""
+
+from .config import ArmSetup, register_common_args, setup_from_args
+from .kinematics import (
+    IKParams,
+    Kinematics,
+    register_ik_args,
+    ik_params_from_args,
+)
+from .poses import read_ee_pose, pose_to_se3, se3_to_pose
+
+__all__ = [
+    # context
+    "ArmSetup",
+    # high-level interface
+    "Kinematics",
+    "IKParams",
+    # CLI helpers
+    "register_common_args",
+    "register_ik_args",
+    "setup_from_args",
+    "ik_params_from_args",
+    # pose utilities
+    "read_ee_pose",
+    "pose_to_se3",
+    "se3_to_pose",
+]

openarm_control-0.1.0/src/openarm_control/config.py

@@ -0,0 +1,195 @@
+# Copyright 2026 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Shared MuJoCo context for OpenArm FK, IK, and controller nodes."""
+
+from __future__ import annotations
+
+import argparse
+
+import mujoco
+import numpy as np
+import openarm_mujoco.v2 as openarm_mujoco
+from openarm_mujoco.v2 import JointResolver
+
+_DEFAULT_XML = openarm_mujoco.openarm_cell_xml()
+
+_DEFAULT_FRAME_RIGHT = "right_ee_control_point"
+_DEFAULT_FRAME_TYPE_RIGHT = "site"
+_DEFAULT_FRAME_LEFT = "left_ee_control_point"
+_DEFAULT_FRAME_TYPE_LEFT = "site"
+
+_FRAME_OBJ = {
+    "body": mujoco.mjtObj.mjOBJ_BODY,
+    "site": mujoco.mjtObj.mjOBJ_SITE,
+    "geom": mujoco.mjtObj.mjOBJ_GEOM,
+}
+
+# Per-arm-joint velocity caps in rad/s. Enabled via --limit-velocity.
+ARM_JOINT_VELOCITY_LIMITS_RAD_S: list[float] = [
+    1.57,  # joint1 DM 8009
+    1.57,  # joint2 DM 8009
+    3.14,  # joint3 DM 4340
+    3.14,  # joint4 DM 4340
+    12.6,  # joint5 DM 4310
+    12.6,  # joint6 DM 4310
+    12.6,  # joint7 DM 4310
+]
+
+
+class ArmSetup:
+    """MuJoCo context shared across FK, IK, and controller nodes.
+
+    Bundles the model, data, joint resolver, active arm sides, and per-arm
+    EE frame IDs/types. Instantiate once per process; pass into any solver
+    or controller that needs model access.
+
+    Pose convention throughout: float32[7] = [px, py, pz, qw, qx, qy, qz]
+    """
+
+    def __init__(
+        self,
+        model: mujoco.MjModel,
+        data: mujoco.MjData,
+        joint_resolver: JointResolver,
+        sides: list[str],
+        frame_ids: dict[str, int],
+        frame_types: dict[str, str],
+    ) -> None:
+        """Initialize."""
+        self.model = model
+        self.data = data
+        self.joint_resolver = joint_resolver
+        self.sides = sides
+        self.frame_ids = frame_ids  # side → MuJoCo object ID
+        self.frame_types = frame_types  # side → "body" | "site" | "geom"
+
+    @classmethod
+    def from_args(
+        cls,
+        xml: str,
+        mode: str,
+        frame_right: str,
+        frame_type_right: str,
+        frame_left: str,
+        frame_type_left: str,
+        keyframe: str | None = "home",
+    ) -> ArmSetup:
+        """Build from XML."""
+        model = mujoco.MjModel.from_xml_path(xml)
+        data = mujoco.MjData(model)
+
+        if keyframe:
+            key_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_KEY, keyframe)
+            if key_id >= 0:
+                mujoco.mj_resetDataKeyframe(model, data, key_id)
+            else:
+                print(f"Warning: keyframe '{keyframe}' not found, using defaults.")
+
+        mujoco.mj_forward(model, data)
+
+        sides: list[str] = []
+        if mode in ("right", "bimanual"):
+            sides.append("right")
+        if mode in ("left", "bimanual"):
+            sides.append("left")
+
+        frame_ids: dict[str, int] = {}
+        frame_types: dict[str, str] = {}
+        for side in sides:
+            name = frame_right if side == "right" else frame_left
+            ftype = frame_type_right if side == "right" else frame_type_left
+            frame_ids[side] = _resolve_frame_id(model, name, ftype)
+            frame_types[side] = ftype
+
+        return cls(
+            model=model,
+            data=data,
+            joint_resolver=JointResolver(model),
+            sides=sides,
+            frame_ids=frame_ids,
+            frame_types=frame_types,
+        )
+
+    def read_ee_pose(self, side: str) -> np.ndarray:
+        """Return float32[7] = [px, py, pz, qw, qx, qy, qz] for the given arm."""
+        from openarm_control.poses import read_ee_pose
+
+        return read_ee_pose(self.data, self.frame_ids[side], self.frame_types[side])
+
+
+def _resolve_frame_id(model: mujoco.MjModel, name: str, ftype: str) -> int:
+    obj = _FRAME_OBJ.get(ftype)
+    if obj is None:
+        raise ValueError(f"Unknown frame_type '{ftype}'. Expected body/site/geom.")
+    fid = mujoco.mj_name2id(model, obj, name)
+    if fid < 0:
+        raise ValueError(f"{ftype.capitalize()} '{name}' not found in model.")
+    return fid
+
+
+def register_common_args(parser: argparse.ArgumentParser) -> None:
+    """Register shared CLI flags used by all arm nodes: --xml, --keyframe, --mode, --frame-*."""
+    parser.add_argument(
+        "--xml",
+        default=_DEFAULT_XML,
+        help=f"MJCF scene file (default: {_DEFAULT_XML})",
+    )
+    parser.add_argument(
+        "--keyframe",
+        "-k",
+        default="home",
+        help="Initial keyframe name (default: home)",
+    )
+    parser.add_argument(
+        "--mode",
+        choices=["right", "left", "bimanual"],
+        default="bimanual",
+        help="Which arm(s) to compute (default: bimanual)",
+    )
+    parser.add_argument(
+        "--frame-right",
+        default=_DEFAULT_FRAME_RIGHT,
+        help=f"EE frame name for right arm (default: {_DEFAULT_FRAME_RIGHT})",
+    )
+    parser.add_argument(
+        "--frame-type-right",
+        choices=["body", "site", "geom"],
+        default=_DEFAULT_FRAME_TYPE_RIGHT,
+        help="EE frame type for right arm (default: site)",
+    )
+    parser.add_argument(
+        "--frame-left",
+        default=_DEFAULT_FRAME_LEFT,
+        help=f"EE frame name for left arm (default: {_DEFAULT_FRAME_LEFT})",
+    )
+    parser.add_argument(
+        "--frame-type-left",
+        choices=["body", "site", "geom"],
+        default=_DEFAULT_FRAME_TYPE_LEFT,
+        help="EE frame type for left arm (default: site)",
+    )
+
+
+def setup_from_args(args: argparse.Namespace) -> ArmSetup:
+    """Build ArmSetup from a namespace that contains the common CLI flags."""
+    return ArmSetup.from_args(
+        xml=args.xml,
+        mode=args.mode,
+        frame_right=args.frame_right,
+        frame_type_right=args.frame_type_right,
+        frame_left=args.frame_left,
+        frame_type_left=args.frame_type_left,
+        keyframe=args.keyframe,
+    )

openarm_control-0.1.0/src/openarm_control/kinematics.py

@@ -0,0 +1,357 @@
+# Copyright 2026 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""High-level FK + IK interface for OpenArm.
+
+Usage:
+    # FK only
+    kin = Kinematics(setup)
+    pose = kin.fk("right", joints)          # float32[7]
+    pose_r, pose_l = kin.fk_bimanual(r, l)  # single mj_forward
+
+    # FK + IK
+    kin = Kinematics(setup, IKParams())
+    kin.set_target("right", pose)
+    kin.set_target("left", pose)
+    result = kin.solve(dt=0.1, n_iters=5)   # float32[16] or None
+"""
+
+from __future__ import annotations
+
+import argparse
+from dataclasses import dataclass
+
+import mink
+import mink.exceptions
+import mujoco
+import numpy as np
+
+from openarm_control.config import ARM_JOINT_VELOCITY_LIMITS_RAD_S, ArmSetup
+from openarm_control.poses import pose_to_se3
+
+
+@dataclass
+class IKParams:
+    """Configuration for the mink QP-based IK solver."""
+
+    position_cost: float = 1.0
+    orientation_cost: float = 1.0
+    lm_damping: float = 0.01
+    damping: float = 0.25
+    solver: str = "daqp"
+    posture_cost: float = 0.01
+    diag_reg: float = 0.0
+    dt: float = 0.1
+    max_iters: int = 5
+    velocity_limits: dict[str, float] | None = None
+
+
+class Kinematics:
+    """Unified FK + IK for OpenArm, backed by MuJoCo + mink.
+
+    FK is always available. IK is enabled by passing ``IKParams``.
+    Both share the same ``ArmSetup`` context (model, resolver, frame IDs).
+    """
+
+    def __init__(self, setup: ArmSetup, ik_params: IKParams | None = None) -> None:
+        """Initialize."""
+        self.setup = setup
+        self._ik: _IKSolver | None = (
+            _IKSolver(setup, ik_params) if ik_params is not None else None
+        )
+
+    # ── FK ───────────────────────────────────────────────────────────────────
+
+    def fk(self, side: str, joints: np.ndarray) -> np.ndarray:
+        """Set qpos for one arm, run mj_forward, return float32[7] EE pose."""
+        self.setup.joint_resolver.set_qpos(self.setup.data.qpos, joints, side)
+        mujoco.mj_forward(self.setup.model, self.setup.data)
+        return self.setup.read_ee_pose(side)
+
+    def fk_bimanual(
+        self, right: np.ndarray, left: np.ndarray
+    ) -> tuple[np.ndarray, np.ndarray]:
+        """Set both arms and run a single mj_forward. Returns (pose_right, pose_left)."""
+        self.setup.joint_resolver.set_qpos(self.setup.data.qpos, right, "right")
+        self.setup.joint_resolver.set_qpos(self.setup.data.qpos, left, "left")
+        mujoco.mj_forward(self.setup.model, self.setup.data)
+        return self.setup.read_ee_pose("right"), self.setup.read_ee_pose("left")
+
+    # ── IK ───────────────────────────────────────────────────────────────────
+
+    def set_target(self, side: str, pose: np.ndarray) -> None:
+        """Set EE target for one arm. pose: float32[7] = [px, py, pz, qw, qx, qy, qz]."""
+        self._require_ik().set_target(side, pose)
+
+    def sync(self, values16: np.ndarray) -> None:
+        """Sync IK internal config from float32[16] driver state (right[8]+left[8])."""
+        self._require_ik().sync(values16)
+
+    def ready(self) -> bool:
+        """Return True once all active arms have received at least one target this cycle."""
+        return self._require_ik().ready()
+
+    def solve(self) -> np.ndarray | None:
+        """Run IK. Returns float32[16] (right[8]+left[8]) or None on failure."""
+        return self._require_ik().solve()
+
+    def set_gripper(self, side: str, value: float) -> None:
+        """Pass through a gripper value; IK does not solve for it."""
+        idx = 0 if side == "right" else 1
+        self._require_ik()._gripper[idx] = value
+
+    def _require_ik(self) -> _IKSolver:
+        if self._ik is None:
+            raise RuntimeError("Kinematics was not initialized with IKParams.")
+        return self._ik
+
+
+# ── internal IK implementation ────────────────────────────────────────────────
+
+
+class _IKSolver:
+    """mink QP-based differential IK. Managed by Kinematics; not public API."""
+
+    def __init__(self, setup: ArmSetup, params: IKParams) -> None:
+        self._sides = setup.sides
+        self._solver_name = params.solver
+        self._posture_cost = params.posture_cost
+        self._joint_resolver = setup.joint_resolver
+        self._dt = params.dt
+        self._max_iters = params.max_iters
+
+        self._config = mink.Configuration(setup.model)
+        self._config.update(q=setup.data.qpos.copy())
+        mid_qpos = self._config.data.qpos.copy()
+
+        task_kwargs = dict(
+            position_cost=params.position_cost,
+            orientation_cost=params.orientation_cost,
+            lm_damping=params.lm_damping,
+        )
+        self._tasks: dict[str, mink.FrameTask] = {
+            side: mink.FrameTask(
+                frame_name=_frame_name(setup, side),
+                frame_type=setup.frame_types[side],
+                **task_kwargs,
+            )
+            for side in setup.sides
+        }
+
+        active_qpos: set[int] = set(
+            setup.joint_resolver._right.arm_qpos.tolist()
+        ) | set(setup.joint_resolver._left.arm_qpos.tolist())
+        freeze_dofs = [
+            int(setup.model.jnt_dofadr[j])
+            for j in range(setup.model.njnt)
+            if setup.model.jnt_qposadr[j] not in active_qpos
+        ]
+        print(active_qpos)
+        print(freeze_dofs)
+        self._freeze_task: mink.DofFreezingTask | None = (
+            mink.DofFreezingTask(model=setup.model, dof_indices=freeze_dofs)
+            if freeze_dofs
+            else None
+        )
+
+        self._limits = [mink.ConfigurationLimit(setup.model)]
+
+        if params.velocity_limits is not None:
+            self._limits.append(mink.VelocityLimit(setup.model, params.velocity_limits))
+
+        self._posture_task = mink.PostureTask(setup.model, cost=params.posture_cost)
+        self._posture_task.set_target(mid_qpos)
+
+        self._solver_params: dict = {"damping": params.damping}
+        if params.diag_reg > 0.0:
+            self._solver_params["diag_reg"] = params.diag_reg
+
+        self._pending: set[str] = set(setup.sides)
+        self._gripper = np.zeros(2, dtype=np.float32)
+
+    def set_target(self, side: str, pose: np.ndarray) -> None:
+        self._tasks[side].set_target(pose_to_se3(pose))
+        self._pending.discard(side)
+
+    def sync(self, values16: np.ndarray) -> None:
+        qpos = self._config.data.qpos.copy()
+        self._joint_resolver.set_qpos(qpos, values16[:8], "right")
+        self._joint_resolver.set_qpos(qpos, values16[8:16], "left")
+        self._config.update(q=qpos)
+        self._gripper[0] = values16[7]
+        self._gripper[1] = values16[15]
+
+    def ready(self) -> bool:
+        return len(self._pending) == 0
+
+    def solve(self) -> np.ndarray | None:
+        tasks = list(self._tasks.values())
+        if self._posture_cost > 0.0:
+            tasks.append(self._posture_task)
+        constraints = [self._freeze_task] if self._freeze_task else []
+
+        for _ in range(self._max_iters):
+            try:
+                vel = mink.solve_ik(
+                    self._config,
+                    tasks,
+                    self._dt,
+                    self._solver_name,
+                    limits=self._limits,
+                    constraints=constraints,
+                    safety_break=False,
+                    **self._solver_params,
+                )
+            except mink.exceptions.NoSolutionFound:
+                try:
+                    vel = mink.solve_ik(
+                        self._config,
+                        tasks,
+                        self._dt,
+                        self._solver_name,
+                        limits=[],
+                        constraints=constraints,
+                        safety_break=False,
+                        **self._solver_params,
+                    )
+                except mink.exceptions.NoSolutionFound:
+                    print(
+                        "Warning: IK solver failed (constrained and unconstrained). Skipping step."
+                    )
+                    return None
+            self._config.integrate_inplace(vel, self._dt)
+
+        self._pending = set(self._sides)
+
+        qpos = self._config.data.qpos
+        right_joints, _ = self._joint_resolver.get_driver(qpos, "right")
+        left_joints, _ = self._joint_resolver.get_driver(qpos, "left")
+        return np.concatenate(
+            [
+                np.append(right_joints, self._gripper[0]),
+                np.append(left_joints, self._gripper[1]),
+            ]
+        ).astype(np.float32)
+
+
+def _frame_name(setup: ArmSetup, side: str) -> str:
+    ftype = setup.frame_types[side]
+    fid = setup.frame_ids[side]
+    obj = {
+        "body": mujoco.mjtObj.mjOBJ_BODY,
+        "site": mujoco.mjtObj.mjOBJ_SITE,
+        "geom": mujoco.mjtObj.mjOBJ_GEOM,
+    }[ftype]
+    return mujoco.mj_id2name(setup.model, obj, fid)
+
+
+def _convert_velocity(
+    rad_per_sec: float,
+    dt: float,
+    max_iters: int,
+    tick_hz: float,
+) -> float:
+    if max_iters <= 0 or dt <= 0.0 or tick_hz <= 0.0:
+        raise ValueError("max_iters, dt, and tick_hz must all be positive.")
+    return rad_per_sec / (max_iters * dt * tick_hz)
+
+
+# ── CLI helpers ───────────────────────────────────────────────────────────────
+
+
+def register_ik_args(parser: argparse.ArgumentParser) -> None:
+    """Register IK-specific CLI flags. Call after register_common_args."""
+    parser.add_argument(
+        "--pos-cost", type=float, default=1.0, help="Position task cost (default: 1.0)"
+    )
+    parser.add_argument(
+        "--ori-cost",
+        type=float,
+        default=1.0,
+        help="Orientation task cost (default: 1.0)",
+    )
+    parser.add_argument(
+        "--lm-damping",
+        type=float,
+        default=0.01,
+        help="Per-task LM damping (default: 0.01)",
+    )
+    parser.add_argument(
+        "--damping",
+        type=float,
+        default=0.25,
+        help="Global Tikhonov regularization (default: 0.25)",
+    )
+    parser.add_argument("--solver", default="daqp", help="QP backend (default: daqp)")
+    parser.add_argument(
+        "--max-iters", type=int, default=5, help="IK iterations per event (default: 5)"
+    )
+    parser.add_argument(
+        "--dt",
+        type=float,
+        default=0.1,
+        help="Integration timestep per iteration (default: 0.1)",
+    )
+    parser.add_argument(
+        "--posture-cost",
+        type=float,
+        default=0.01,
+        help="Posture task weight, 0=disabled (default: 0.01)",
+    )
+    parser.add_argument(
+        "--diag-reg",
+        type=float,
+        default=0.0,
+        help="QP diagonal regularization (default: 0.0)",
+    )
+    parser.add_argument(
+        "--limit-velocity",
+        action="store_true",
+        help="Enable per-joint velocity limits (caps in config.ARM_JOINT_VELOCITY_LIMITS_RAD_S).",
+    )
+    parser.add_argument(
+        "--tick-hz",
+        type=float,
+        default=500.0,
+        help="Dora tick rate in Hz; must match the dataflow timer (default: 500.0).",
+    )
+
+
+def ik_params_from_args(args: argparse.Namespace) -> IKParams:
+    """Build IKParams from parsed args (requires register_ik_args to have been called)."""
+    velocity_limits: dict[str, float] | None = None
+    if args.limit_velocity:
+        velocity_limits = {
+            f"openarm_{side}_joint{i + 1}": _convert_velocity(
+                rad_per_sec=v,
+                dt=args.dt,
+                max_iters=args.max_iters,
+                tick_hz=args.tick_hz,
+            )
+            for side in ("left", "right")
+            for i, v in enumerate(ARM_JOINT_VELOCITY_LIMITS_RAD_S)
+        }
+
+    return IKParams(
+        position_cost=args.pos_cost,
+        orientation_cost=args.ori_cost,
+        lm_damping=args.lm_damping,
+        damping=args.damping,
+        solver=args.solver,
+        posture_cost=args.posture_cost,
+        diag_reg=args.diag_reg,
+        dt=args.dt,
+        max_iters=args.max_iters,
+        velocity_limits=velocity_limits,
+    )

openarm_control-0.1.0/src/openarm_control/poses.py

@@ -0,0 +1,61 @@
+# Copyright 2026 Enactic, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""Pose conversion utilities shared across FK, IK, and controller nodes.
+
+Convention: float32[7] = [px, py, pz, qw, qx, qy, qz]
+"""
+
+from __future__ import annotations
+
+import mujoco
+import numpy as np
+
+
+def read_ee_pose(data: mujoco.MjData, fid: int, ftype: str) -> np.ndarray:
+    """Read EE pose from MjData after mj_forward.
+
+    Returns float32[7] = [px, py, pz, qw, qx, qy, qz].
+    """
+    if ftype == "body":
+        pos = data.xpos[fid]
+        quat = data.xquat[fid]
+    elif ftype == "site":
+        pos = data.site_xpos[fid]
+        quat = np.empty(4)
+        mujoco.mju_mat2Quat(quat, data.site_xmat[fid])
+    else:  # geom
+        pos = data.geom_xpos[fid]
+        quat = np.empty(4)
+        mujoco.mju_mat2Quat(quat, data.geom_xmat[fid])
+    return np.concatenate([pos, quat]).astype(np.float32)
+
+
+def pose_to_se3(pose: np.ndarray):  # -> mink.SE3
+    """float32[7] [px, py, pz, qw, qx, qy, qz] → mink.SE3.
+
+    mink internal layout: wxyz_xyz = [qw, qx, qy, qz, x, y, z].
+    """
+    import mink  # lazy: FK nodes don't depend on mink
+
+    wxyz_xyz = np.empty(7, dtype=np.float64)
+    wxyz_xyz[:4] = pose[3:7]  # quat: wxyz
+    wxyz_xyz[4:] = pose[:3]  # translation: xyz
+    return mink.SE3(wxyz_xyz=wxyz_xyz)
+
+
+def se3_to_pose(se3) -> np.ndarray:  # se3: mink.SE3
+    """mink.SE3 → float32[7] [px, py, pz, qw, qx, qy, qz]."""
+    wxyz_xyz = se3.wxyz_xyz
+    return np.concatenate([wxyz_xyz[4:], wxyz_xyz[:4]]).astype(np.float32)

openarm_control-0.1.0/src/openarm_control.egg-info/PKG-INFO

@@ -0,0 +1,81 @@
+Metadata-Version: 2.4
+Name: openarm_control
+Version: 0.1.0
+Summary: Kinematics and control utilities for OpenArm
+Author: Enactic, Inc.
+License: Apache-2.0
+Project-URL: Changelog, https://github.com/enactic/openarm_control/releases
+Project-URL: Issues, https://github.com/enactic/openarm_control/issues
+Project-URL: Repository, https://github.com/enactic/openarm_control.git
+Requires-Python: >=3.10
+Description-Content-Type: text/markdown
+License-File: LICENSE.txt
+Requires-Dist: daqp==0.7.2
+Requires-Dist: mink>=1.1.0
+Requires-Dist: mujoco>=3.6.0
+Requires-Dist: numpy
+Requires-Dist: openarm-mujoco
+Dynamic: license-file
+
+# OpenArm Control
+
+Reusable kinematics and control utilities for OpenArm, backed by MuJoCo and [mink](https://github.com/kevinzakka/mink).
+
+## Install
+
+```bash
+uv sync
+```
+
+## Usage
+
+### `Kinematics`
+
+```python
+from openarm_control import Kinematics, IKParams, ArmSetup
+
+# FK only
+kin = Kinematics(setup)
+pose = kin.fk("right", joints)               # float32[7]
+pose_r, pose_l = kin.fk_bimanual(r, l)       # single mj_forward
+
+# IK
+kin = Kinematics(setup, IKParams(damping=0.25, posture_cost=0.01))
+kin.set_target("right", pose_r)
+kin.set_target("left", pose_l)
+result = kin.solve(dt=0.1, n_iters=5)        # float32[16] right[8]+left[8]
+```
+
+### `IKParams`
+
+Solver configuration passed to `Kinematics`. All fields have defaults.
+
+| Field | Default | Description |
+|---|---|---|
+| `position_cost` | `1.0` | Position task weight |
+| `orientation_cost` | `1.0` | Orientation task weight |
+| `lm_damping` | `0.01` | Per-task Levenberg-Marquardt damping |
+| `damping` | `0.25` | Global Tikhonov regularization |
+| `solver` | `"daqp"` | QP backend |
+| `posture_cost` | `0.01` | Neutral posture task weight (0 = disabled) |
+| `diag_reg` | `0.0` | QP diagonal regularization |
+| `dt` | `0.1` | Integration timestep per iteration |
+| `max_iters` | `5` | IK iterations per solve |
+| `velocity_limits` | `None` | Per-joint velocity limits (applied in rad/s from `config.py`); `None` = disabled |
+
+Build from CLI args with `register_ik_args` + `ik_params_from_args`:
+
+## Related links
+
+- 💬 Join the community on [Discord](https://discord.gg/FsZaZ4z3We)
+- 📬 Contact us through <openarm@enactic.ai>
+
+## License
+
+Licensed under the Apache License 2.0. See [LICENSE.txt](LICENSE.txt) for details.
+
+Copyright 2026 Enactic, Inc.
+
+## Code of Conduct
+
+All participation in the OpenArm project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md).

openarm_control-0.1.0/src/openarm_control.egg-info/SOURCES.txt

@@ -0,0 +1,12 @@
+LICENSE.txt
+README.md
+pyproject.toml
+src/openarm_control/__init__.py
+src/openarm_control/config.py
+src/openarm_control/kinematics.py
+src/openarm_control/poses.py
+src/openarm_control.egg-info/PKG-INFO
+src/openarm_control.egg-info/SOURCES.txt
+src/openarm_control.egg-info/dependency_links.txt
+src/openarm_control.egg-info/requires.txt
+src/openarm_control.egg-info/top_level.txt

openarm_control-0.1.0/src/openarm_control.egg-info/dependency_links.txt

@@ -0,0 +1 @@
+

openarm_control-0.1.0/src/openarm_control.egg-info/requires.txt

@@ -0,0 +1,5 @@
+daqp==0.7.2
+mink>=1.1.0
+mujoco>=3.6.0
+numpy
+openarm-mujoco

openarm_control-0.1.0/src/openarm_control.egg-info/top_level.txt

@@ -0,0 +1 @@
+openarm_control