Skip to content

Auto-fixes for cc/openarm-rust-adapter #2426

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
TomCC7 merged 13 commits into from
Jun 8, 2026
Merged

Auto-fixes for cc/openarm-rust-adapter #2426

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
13618f1
refactor: drop unused FloatArray alias
paul-nechifor Jun 8, 2026
8b9468f
refactor: drop unused OpenArmRSMotorSpecConfig alias
paul-nechifor Jun 8, 2026
872fc43
refactor: hoist time import in openarm adapter test
paul-nechifor Jun 8, 2026
cc738dc
refactor: stop restating openarm_rs adapter defaults in blueprint
paul-nechifor Jun 8, 2026
40dc78e
docs: explain lazy pinocchio imports in damiao base adapter
paul-nechifor Jun 8, 2026
1f9078d
refactor: share tick/cache defaults via named constants
paul-nechifor Jun 8, 2026
c26874e
refactor: single source of truth for default CAN address
paul-nechifor Jun 8, 2026
f98dbc3
perf: vectorize damiao state and command paths
paul-nechifor Jun 8, 2026
227e841
fix: use structured logging with tracebacks in damiao adapter
paul-nechifor Jun 8, 2026
a88758f
fix: log dropped gravity feed-forward in write_joint_positions
paul-nechifor Jun 8, 2026
36940ea
fix: narrow except in write_gravity_compensation
paul-nechifor Jun 8, 2026
e22e724
fix: reject unknown kwargs in OpenArmRSAdapter
paul-nechifor Jun 8, 2026
1b12e2d
docs: point to kp/kd source constants instead of restating them
paul-nechifor Jun 8, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
120 changes: 78 additions & 42 deletions dimos/hardware/manipulators/damiao/base_adapter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,15 +20,21 @@
from typing import Any, cast

import numpy as np
from numpy.typing import NDArray

from dimos.hardware.manipulators.damiao.specs import DamiaoArmSpec
from dimos.hardware.manipulators.spec import ControlMode, JointLimits, ManipulatorInfo
from dimos.utils.logging_config import setup_logger

logger = setup_logger()

FloatArray = NDArray[np.float64]
# Shared adapter defaults. Subclasses reference these so the base and its
# subclasses can't drift apart.
_DEFAULT_TICK_DEADLINE_US = 1_000
_DEFAULT_STATE_CACHE_TTL_S = 0.002
# Conventional first SocketCAN interface; used when no address is configured.
_DEFAULT_ADDRESS = "can0"
# Position of each control mode in declaration order, reported by read_state().
_CONTROL_MODE_INDEX = {mode: index for index, mode in enumerate(ControlMode)}

_can_motor_control: Any | None
_damiao: Any | None
Expand Down Expand Up @@ -110,11 +116,11 @@ def __init__(
gravity_model_path: str | Path | None = None,
gravity_torque_limits: list[float] | tuple[float, ...] | None = None,
supported_control_modes: tuple[ControlMode, ...] | None = None,
address: str | Path | None = "can0",
address: str | Path | None = _DEFAULT_ADDRESS,
config_path: str | Path | None = None,
use_mock_bus: bool = False,
tick_deadline_us: int = 1_000,
state_cache_ttl_s: float = 0.002,
tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US,
state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S,
) -> None:
arm_spec.validate()
if dof is not None and dof != arm_spec.dof:
Expand Down Expand Up @@ -157,7 +163,7 @@ def __init__(
self._last_positions: list[float] | None = None
self._pin_model = None
self._pin_data: object | None = None
self._address = str(address) if address is not None else "can0"
self._address = str(address) if address is not None else _DEFAULT_ADDRESS
self._config_path = str(config_path) if config_path is not None else None
self._arm_name = arm_spec.arm_name
self._bus_name = arm_spec.bus_name
Expand All @@ -182,18 +188,6 @@ def _validate_command_lengths(self, **commands: list[float]) -> None:
def _zero_vector(self) -> list[float]:
return [0.0] * self._dof

def _mit_command_rows(
self,
*,
q: list[float],
dq: list[float],
kp: list[float],
kd: list[float],
tau: list[float],
) -> list[tuple[float, float, float, float, float]]:
self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau)
return list(zip(q, dq, kp, kd, tau, strict=True))

def get_info(self) -> ManipulatorInfo:
return ManipulatorInfo(
vendor=self._arm_spec.vendor,
Expand Down Expand Up @@ -261,9 +255,12 @@ def connect(self) -> bool:
self.refresh_state(force=True)
except self._binding_error_type:
raise
except Exception as exc:
logger.error(
f"{type(self).__name__} {self._hardware_id}@{self._address} connect failed: {exc}"
except Exception:
logger.exception(
"damiao adapter connect failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
address=self._address,
)
self._robot = None
self._arm = None
Expand Down Expand Up @@ -304,9 +301,12 @@ def disconnect(self) -> None:
if self._robot is not None:
try:
self._robot.disable()
except Exception as exc:
except Exception:
logger.warning(
f"{type(self).__name__} {self._hardware_id} disable on disconnect failed: {exc}"
"damiao adapter disable on disconnect failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
exc_info=True,
)
self._enabled = False
self._connected = False
Expand All @@ -330,9 +330,9 @@ def refresh_state(self, *, force: bool = False) -> tuple[list[float], list[float
self._arm.refresh()
self._robot.tick(self._tick_deadline_us)
state = (
[float(value) for value in self._arm.positions().astype(np.float64)],
[float(value) for value in self._arm.velocities().astype(np.float64)],
[float(value) for value in self._arm.torques().astype(np.float64)],
self._arm.positions().astype(np.float64).tolist(),
self._arm.velocities().astype(np.float64).tolist(),
self._arm.torques().astype(np.float64).tolist(),
)
if any(len(values) != self._dof for values in state):
raise RuntimeError("can_motor_control state length does not match configured DOF")
Expand All @@ -353,7 +353,7 @@ def read_joint_efforts(self) -> list[float]:
def read_state(self) -> dict[str, int]:
return {
"state": 1 if self._enabled else 0,
"mode": list(ControlMode).index(self._control_mode),
"mode": _CONTROL_MODE_INDEX[self._control_mode],
}

def read_error(self) -> tuple[int, str]:
Expand All @@ -379,6 +379,14 @@ def write_joint_positions(self, positions: list[float], velocity: float = 1.0) -
try:
tau = self.compute_gravity_torques(self.read_joint_positions())
except RuntimeError:
# State read failed: fall back to zero feed-forward torque, but
# surface it so the dropped gravity term isn't silent.
logger.warning(
"damiao adapter dropping gravity feed-forward; state read failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
exc_info=True,
)
tau = self._zero_vector()
else:
tau = self._zero_vector()
Expand Down Expand Up @@ -413,12 +421,15 @@ def write_joint_torques(self, efforts: list[float]) -> bool:
def write_gravity_compensation(self, damping: float | list[float] = 0.0) -> bool:
try:
q, dq, _ = self.refresh_state(force=True)
tau = self.compute_gravity_torques(q)
except Exception as exc:
except RuntimeError:
logger.warning(
f"Skipping {type(self).__name__} gravity compensation due to invalid state: {exc}"
"skipping damiao gravity compensation; state read failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
exc_info=True,
)
return False
tau = self.compute_gravity_torques(q)
kd = [float(damping)] * self._dof if isinstance(damping, int | float) else list(damping)
return self.write_mit_commands(q=q, dq=dq, kp=self._zero_vector(), kd=kd, tau=tau)

Expand All @@ -427,10 +438,9 @@ def write_mit_commands(
) -> bool:
if self._arm is None or self._robot is None or not self._enabled:
return False
rows = self._mit_command_rows(q=q, dq=dq, kp=kp, kd=kd, tau=tau)
self._arm.mit_control(
np.array([(row[2], row[3], row[0], row[1], row[4]) for row in rows], dtype=np.float64)
)
self._validate_command_lengths(q=q, dq=dq, kp=kp, kd=kd, tau=tau)
# MIT command columns are (kp, kd, q, dq, tau) per joint.
self._arm.mit_control(np.column_stack([kp, kd, q, dq, tau]).astype(np.float64))
self._robot.tick(self._tick_deadline_us)
self._state_cache = None
self._last_positions = list(q)
Expand All @@ -456,8 +466,13 @@ def write_stop(self) -> bool:
)
try:
self._robot.disable()
except Exception as exc:
logger.warning(f"{type(self).__name__} {self._hardware_id} stop disable failed: {exc}")
except Exception:
logger.warning(
"damiao adapter stop disable failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
exc_info=True,
)
return False
self._enabled = False
return True
Expand All @@ -467,14 +482,23 @@ def write_enable(self, enable: bool) -> bool:
return False
try:
self._robot.enable() if enable else self._robot.disable()
except Exception as exc:
logger.error(f"{type(self).__name__} {self._hardware_id} enable={enable} failed: {exc}")
except Exception:
logger.exception(
"damiao adapter enable failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
enable=enable,
)
return False
self._enabled = enable
if enable:
positions = self.read_joint_positions()
if not self.write_joint_positions(positions):
logger.error(f"{type(self).__name__} {self._hardware_id} startup hold failed")
logger.error(
"damiao adapter startup hold failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
)
return False
return True

Expand All @@ -484,19 +508,29 @@ def write_clear_errors(self) -> bool:
try:
self._robot.disable()
self._robot.enable()
except Exception as exc:
logger.error(f"{type(self).__name__} {self._hardware_id} clear errors failed: {exc}")
except Exception:
logger.exception(
"damiao adapter clear errors failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
)
return False
self._enabled = True
positions = self.read_joint_positions()
if not self.write_joint_positions(positions):
logger.error(f"{type(self).__name__} {self._hardware_id} clear-error hold failed")
logger.error(
"damiao adapter clear-error hold failed",
adapter=type(self).__name__,
hardware_id=self._hardware_id,
)
return False
return True

def _load_gravity_model(self) -> None:
if not self._gravity_comp or self._gravity_model_path is None:
return
# Lazy import: pinocchio is a heavy optional dep only needed when
# gravity compensation is enabled, so keep it out of module import.
import pinocchio

build_model_from_urdf = _dynamic_attr(pinocchio, "buildModelFromUrdf")
Expand All @@ -507,6 +541,8 @@ def compute_gravity_torques(self, q: list[float]) -> list[float]:
self._validate_length("q", q)
if self._pin_model is None or self._pin_data is None:
return [0.0] * self._dof
# Lazy import: pinocchio is a heavy optional dep only needed when
# gravity compensation is enabled, so keep it out of module import.
import pinocchio

compute_generalized_gravity = _dynamic_attr(pinocchio, "computeGeneralizedGravity")
Expand Down
2 changes: 1 addition & 1 deletion dimos/hardware/manipulators/damiao/test_base_adapter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ def test_base_adapter_info_limits_and_modes() -> None:
def test_base_adapter_validates_command_lengths() -> None:
adapter = DamiaoArmAdapterBase(arm_spec=_arm_spec())
with pytest.raises(ValueError, match="q length 1 does not match dof 2"):
adapter._mit_command_rows(
adapter._validate_command_lengths(
q=[0.0],
dq=[0.0, 0.0],
kp=[0.0, 0.0],
Expand Down
3 changes: 1 addition & 2 deletions dimos/hardware/manipulators/openarm/test_adapter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

from __future__ import annotations

import time
from unittest.mock import MagicMock

import pytest
Expand All @@ -24,8 +25,6 @@

class FakeState:
def __init__(self, index: int) -> None:
import time

self.q = 0.1 * index
self.dq = 0.2 * index
self.tau = 0.3 * index
Expand Down
13 changes: 6 additions & 7 deletions dimos/hardware/manipulators/openarm_rs/adapter.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,9 @@
from typing import TYPE_CHECKING

from dimos.hardware.manipulators.damiao.base_adapter import (
_DEFAULT_ADDRESS,
_DEFAULT_STATE_CACHE_TTL_S,
_DEFAULT_TICK_DEADLINE_US,
Comment on lines 20 to +23

@greptile-apps greptile-apps Bot Jun 8, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

P2 Importing module-private constants across package boundary

_DEFAULT_ADDRESS, _DEFAULT_TICK_DEADLINE_US, and _DEFAULT_STATE_CACHE_TTL_S carry the single-underscore "module-private" convention, yet they are explicitly imported by a sibling package. This works at runtime but conflicts with the convention and means tools like pylint or flake8 will flag the import. If the intent is for these to be shared across the package, exporting them without a leading underscore (or collecting them in a _constants.py sub-module) would make the intent explicit without relying on the convention-override.

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

DamiaoArmAdapterBase,
DamiaoBindingUnavailableError,
)
Expand All @@ -26,8 +29,6 @@
if TYPE_CHECKING:
from dimos.hardware.manipulators.registry import AdapterRegistry

OpenArmRSMotorSpecConfig = DamiaoMotorSpec


class OpenArmRSBindingUnavailableError(DamiaoBindingUnavailableError):
pass
Expand Down Expand Up @@ -55,7 +56,7 @@ class OpenArmRSAdapter(DamiaoArmAdapterBase):

def __init__(
self,
address: str | Path | None = "can0",
address: str | Path | None = _DEFAULT_ADDRESS,
dof: int = 7,
*,
hardware_id: str = "arm",
Expand All @@ -73,11 +74,10 @@ def __init__(
kp: list[float] | None = None,
kd: list[float] | None = None,
gravity_comp: bool = True,
tick_deadline_us: int = 1_000,
state_cache_ttl_s: float = 0.002,
tick_deadline_us: int = _DEFAULT_TICK_DEADLINE_US,
state_cache_ttl_s: float = _DEFAULT_STATE_CACHE_TTL_S,
gravity_model_path: str | Path | None = None,
gravity_torque_limits: list[float] | None = None,
**_: object,
) -> None:
if dof != len(self._DEFAULT_OPENARM_MOTORS):
raise ValueError(f"OpenArmRSAdapter only supports 7 DOF (got {dof})")
Expand Down Expand Up @@ -128,6 +128,5 @@ def register(registry: AdapterRegistry) -> None:
__all__ = [
"OpenArmRSAdapter",
"OpenArmRSBindingUnavailableError",
"OpenArmRSMotorSpecConfig",
"register",
]
8 changes: 3 additions & 5 deletions dimos/robot/manipulators/openarm/blueprints.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -59,12 +59,10 @@
# replaced / factory-reset).
AUTO_SET_MIT_MODE = True

# gravity_comp and canfd are already True by default on OpenArmRSAdapter, so
# the blueprint only overrides the gravity model path.
_ADAPTER_KWARGS = {"auto_set_mit_mode": AUTO_SET_MIT_MODE}
_OPENARM_RS_ADAPTER_KWARGS = {
"gravity_model_path": OPENARM_V10_RIGHT_MODEL,
"gravity_comp": True,
"canfd": True,
}
_OPENARM_RS_ADAPTER_KWARGS = {"gravity_model_path": OPENARM_V10_RIGHT_MODEL}
_left_hw = _openarm(
side="left",
address=LEFT_CAN,
Expand Down
7 changes: 1 addition & 6 deletions docs/capabilities/manipulation/openarm_integration.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -258,12 +258,7 @@ No other code changes are needed. The `coordinator-openarm-rs` and `openarm-rs-p

### Gain tuning (MIT kp/kd)

Defaults live in the adapter implementations. The binding-backed `openarm_rs` path uses the upstream OpenArm ROS2 hardware presets from `openarm_hardware/openarm_simple_hardware.hpp`; the in-tree `openarm` adapter keeps its existing gains. OpenArm RS/OpenArm ROS2 presets are:

```python
_DEFAULT_KP = [70.0, 70.0, 70.0, 60.0, 10.0, 10.0, 10.0]
_DEFAULT_KD = [2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5]
```
Defaults live in the adapter implementations — pointing here instead of restating the numbers so docs can't drift from code. The binding-backed `openarm_rs` path uses the upstream OpenArm ROS2 hardware presets from `openarm_hardware/openarm_simple_hardware.hpp`; see `OpenArmRSAdapter._DEFAULT_KP` / `_DEFAULT_KD` in [openarm_rs/adapter.py](/dimos/hardware/manipulators/openarm_rs/adapter.py). The in-tree `openarm` adapter keeps its own `_DEFAULT_KP` / `_DEFAULT_KD` in [openarm/adapter.py](/dimos/hardware/manipulators/openarm/adapter.py).

Guidelines:
- `kp ∈ [0, 500]` in MIT mode. Higher kp = stiffer position tracking; too high → oscillation.
Expand Down
Loading