Skip to content

Krishna/feat/openarm damiao - #3351

Merged
KrishnaH96 merged 11 commits into
cc/feat/openyam-driverfrom
krishna/feat/openarm-damiao
Aug 6, 2026
Merged

Krishna/feat/openarm damiao#3351
KrishnaH96 merged 11 commits into
cc/feat/openyam-driverfrom
krishna/feat/openarm-damiao

Conversation

@KrishnaH96

@KrishnaH96 KrishnaH96 commented Aug 4, 2026

Copy link
Copy Markdown
Contributor

Contribution path

Problem

OpenArm support predates the whole-body Damiao stack: a hand-rolled CAN driver plus a manipulator-protocol adapter, one instance per bus, grippers present on the bus but never wired, no sim/real switch, and robot-specific gravity compensation code.

Solution

Stacked on #3129. This ports OpenArm to the generic DamiaoWholeBodyAdapter, mirroring the OpenYAM integration:

  • OpenArmDamiaoAdapter: both v10 arms (2x DM8006, 2x DM4340, 3x DM4310, send ids 0x01..0x07) and both grippers (DM4310 at 0x08) as one whole-body device over two CAN buses (left=can1, right=can0), commanded in one synchronized tick. Gravity compensation uses the bimanual URDF from openarm_description (validated in pinocchio: 14 joints, order left1..7 then right1..7, finite torques).
  • Config rewritten on the OpenYAM pattern: one 16-joint WHOLE_BODY component, mock/real selection via global_config.simulation, hardware-measured MIT gains carried over from the legacy adapter, and per-side planning models now declare an explicit coordinator-to-URDF joint name mapping.
  • Blueprints: coordinator-openarm, openarm-planner-coordinator, keyboard-teleop-openarm, and keyboard-teleop-openarm-planner. The keyboard jogs the left arm while the right arm's twist task holds pose. The [ and ] keys drive both grippers through a single servo task, enabled by a new KeyboardTeleopConfig.gripper_joint_names field.
  • The legacy driver, adapter, and CAN scripts are removed as superseded. Registry golden sets are updated in both directions. The e2e planning-groups test moves to openarm-planner-coordinator, since the test harness passes --simulation and now gets the in-memory adapter automatically.

How to Test

dimos can setup can0
dimos can setup can1
dimos run keyboard-teleop-openarm

Automated validation:

uv run pytest dimos/hardware/whole_body dimos/hardware/test_adapter_registries.py
uv run pytest dimos/robot/test_all_blueprints.py -k openarm

Hardware validation

Validated on a physical bimanual OpenArm 2.0 (PEAK 2ch adapter, one 1 Mbps
classic CAN bus per arm).

  • All 16 motors respond at the declared ids; DM8009 shoulders confirmed.
  • Read-only phase: viser mirrors all 14 joints live, mapping and directions
    correct. This phase caught and fixed two adapter bugs (gripper reads raise
    before calibration; feedback only pumped by the write path).
  • Powered at reduced gains: zero-lurch enable, gravity comp holds a lifted
    arm, keyboard teleop per arm, and planned motion executed per arm and with
    both arms in one plan.
  • Still pending: gripper actuation and full gains (bring-up follow-ups).

AI assistance

Claude Code with Fable 5 assisted throughout: studying the #3129 adapter stack, implementing the adapter, config, blueprints, and tests, validating the bimanual gravity URDF in pinocchio, and writing the docs. The author directed the design decisions (bimanual-first scope, gain carryover from the legacy adapter, legacy driver removal) and reviewed the changes. Mock and unit test validation only; physical OpenArm motion will be tested and findings willl be updated here.

Checklist

  • I have read and approved the CLA.

@TomCC7 TomCC7 left a comment

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

lgtm overall. however, there's some other changes needed to make sure everything is working with openarm:

  1. manipulation module will definitely fail because current srdf generation does not support dual arm setup (on intention). We need to define the srdf and feed manually
  2. how to support multiple gripper, but this can be a separate pr

And a general (safe) testing tip: don't write the command to robot (can just comment out the write function call) and just open visualization to make sure robot joint looks correct (read working), and once it's verified we can enable motor write

Comment thread dimos/hardware/whole_body/openarm_damiao/adapter.py Outdated
Comment thread dimos/hardware/whole_body/openarm_damiao/adapter.py
Comment thread dimos/teleop/keyboard/keyboard_teleop_module.py Outdated
@codecov

codecov Bot commented Aug 4, 2026

Copy link
Copy Markdown

❌ 1 Tests Failed:

Tests completed Failed Passed Skipped
3932 1 3931 68
View the top 1 failed test(s) by shortest run time
dimos.e2e_tests.test_manipulation_planning_groups::test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator
Stack Traces | 18.5s run time
dimos.protocol.rpc.rpc_utils.RemoteError: [Remote builtins.RuntimeError] Invalid goal configuration requested, cannot plan!

Remote traceback:
Traceback (most recent call last):
  File ".../protocol/rpc/pubsubrpc.py", line 280, in execute_and_respond
    response = f(*args[0], **args[1])
               ^^^^^^^^^^^^^^^^^^^^^^
  File ".../protocol/rpc/spec.py", line 116, in override_f
    return getattr(module, fname)(*args, **kwargs)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File ".../dimos/manipulation/manipulation_module.py", line 1044, in plan_to_joint_targets
    return self.generate_plan_to_joint_targets(joint_targets) is not None
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File ".../dimos/manipulation/manipulation_module.py", line 1083, in generate_plan_to_joint_targets
    return self._plan_selected_path(group_ids, start, goal, planning_epoch)
           ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File ".../dimos/manipulation/manipulation_module.py", line 773, in _plan_selected_path
    result = self._planner.plan_selected_joint_path(
             ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  File ".../planning/world/roboplan_world.py", line 523, in plan_selected_joint_path
    return self._plan_group(
           ^^^^^^^^^^^^^^^^^
  File ".../planning/world/roboplan_world.py", line 1134, in _plan_group
    result = self._run_native_rrt(
             ^^^^^^^^^^^^^^^^^^^^^
  File ".../planning/world/roboplan_world.py", line 1178, in _run_native_rrt
    result = planner.plan(start, goal)
             ^^^^^^^^^^^^^^^^^^^^^^^^^
RuntimeError: Invalid goal configuration requested, cannot plan!


The above exception was the direct cause of the following exception:

lcm_spy = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b03d5f0aa80>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7b03d5af7ba0>

    def test_dual_arm_plans_and_dispatches_both_arms_through_control_coordinator(
        lcm_spy: LcmSpy,
        start_blueprint: Callable[..., DimosCliCall],
    ) -> None:
        """Plan one generated plan over both arms and dispatch through one trajectory task."""
        _start_openarm_mock_planner(start_blueprint, lcm_spy)
    
        client = RPCClient(None, ManipulationModule)
        coordinator_client = RPCClient(None, ControlCoordinator)
        try:
            info = _wait_for_robot_info(client, ROBOT_NAME)
            group_ids = _planning_group_ids(info)
    
            tasks = coordinator_client.list_tasks()
            assert tasks == [DEFAULT_TRAJECTORY_TASK_NAME]
    
            _prepare_for_planning(client, (ROBOT_NAME,))
    
>           planned = client.plan_to_joint_targets(
                {
                    group_ids["left_manipulator"]: _offset_target(client, LEFT_SLICE, 0.02),
                    group_ids["right_manipulator"]: _offset_target(client, RIGHT_SLICE, -0.02),
                }
            )

client     = <dimos.core.rpc_client.RPCClient object at 0x7b03d5b67dd0>
coordinator_client = <dimos.core.rpc_client.RPCClient object at 0x7b03d5b67bf0>
group_ids  = {'left_manipulator': 'openarm/left_manipulator', 'right_manipulator': 'openarm/right_manipulator'}
info       = {'base_link': 'openarm_body_link0', 'end_effector_link': None, 'has_joint_name_mapping': True, 'home_joints': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ...], ...}
lcm_spy    = <dimos.e2e_tests.lcm_spy.LcmSpy object at 0x7b03d5f0aa80>
start_blueprint = <function start_blueprint.<locals>.set_name_and_start at 0x7b03d5af7ba0>
tasks      = ['traj_arm']

dimos/e2e_tests/test_manipulation_planning_groups.py:211: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
dimos/core/rpc_client.py:93: in __call__
    result, unsub_fn = self._rpc.call_sync(
        args       = ({'openarm/left_manipulator': JointState(ts=1785976440.99215, frame_id='', name=[], position=[0.02, 0.02, 0.02, 0.02, ...0.9928071, frame_id='', name=[], position=[-0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02], velocity=[], effort=[])},)
        kwargs     = {}
        self       = <dimos.core.rpc_client.RpcCall object at 0x7b03d5f25a60>
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <dimos.protocol.rpc.pubsubrpc.LCMRPC object at 0x7b03d5b67bc0>
name = 'ManipulationModule/plan_to_joint_targets'
arguments = (({'openarm/left_manipulator': JointState(ts=1785976440.99215, frame_id='', name=[], position=[0.02, 0.02, 0.02, 0.02,...8071, frame_id='', name=[], position=[-0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02], velocity=[], effort=[])},), {})
rpc_timeout = 120.0

    def call_sync(
        self, name: str, arguments: Args, rpc_timeout: float | None = None
    ) -> tuple[Any, Callable[[], None]]:
        if rpc_timeout is None:
            method = name.rsplit("/", 1)[-1]
            rpc_timeout = self.rpc_timeouts.get(name) or self.rpc_timeouts.get(
                method, self.default_rpc_timeout
            )
        event = threading.Event()
    
        def receive_value(val) -> None:  # type: ignore[no-untyped-def]
            event.result = val  # type: ignore[attr-defined]  # attach to event
            event.set()
    
        unsub_fn = self.call(name, arguments, receive_value)
        if not event.wait(rpc_timeout):
            # Retries register new callbacks. Remove this expired callback so
            # repeated timeouts do not accumulate entries in the shared response map.
            unsub_fn()
            raise TimeoutError(f"RPC call to '{name}' timed out after {rpc_timeout} seconds")
    
        # Check if the result is an exception and raise it
        result = event.result  # type: ignore[attr-defined]
        if isinstance(result, BaseException):
>           raise result
E           RuntimeError: Invalid goal configuration requested, cannot plan!

arguments  = (({'openarm/left_manipulator': JointState(ts=1785976440.99215, frame_id='', name=[], position=[0.02, 0.02, 0.02, 0.02,...8071, frame_id='', name=[], position=[-0.02, -0.02, -0.02, -0.02, -0.02, -0.02, -0.02], velocity=[], effort=[])},), {})
event      = <threading.Event at 0x7b03d5f257f0: set>
method     = 'plan_to_joint_targets'
name       = 'ManipulationModule/plan_to_joint_targets'
receive_value = <function RPCClient.call_sync.<locals>.receive_value at 0x7b03d5af54e0>
result     = RuntimeError('Invalid goal configuration requested, cannot plan!')
rpc_timeout = 120.0
self       = <dimos.protocol.rpc.pubsubrpc.LCMRPC object at 0x7b03d5b67bc0>
unsub_fn   = <function PubSubRPCMixin.call_cb.<locals>.unsubscribe_callback at 0x7b03d5ac8e00>

.../protocol/rpc/spec.py:88: RuntimeError

To view more test analytics, go to the Test Analytics Dashboard
📋 Got 3 mins? Take this short survey to help us improve Test Analytics.

…ody adapter

Add OpenArmDamiaoAdapter: both v10 arms (2x DM8006, 2x DM4340, 3x DM4310,
send ids 0x01..0x07) and both grippers (DM4310 at 0x08) as one whole-body
device over two CAN buses (left=can1, right=can0), with gravity compensation
from the bimanual URDF (14 joints, validated order left1..7 then right1..7).

Rewrite the OpenArm hardware config on the OpenYAM pattern: one 16-joint
WHOLE_BODY component, mock/real selection via global_config.simulation,
hardware-measured MIT gains carried over from the legacy adapter, and
per-side planning models gaining an explicit coordinator->URDF joint name
mapping.

Blueprints: coordinator-openarm, openarm-planner-coordinator,
keyboard-teleop-openarm and keyboard-teleop-openarm-planner. The keyboard
jogs the left arm while the right arm's twist task holds pose; a single
servo task drives both grippers, enabled by a new
KeyboardTeleopConfig.gripper_joint_names field. The e2e planning-groups
test moves to openarm-planner-coordinator since the harness's --simulation
flag now selects the in-memory adapter.
The hand-rolled Damiao CAN driver and manipulator-protocol adapter are fully
replaced by OpenArmDamiaoAdapter on the whole-body path, which also wires the
previously unimplemented grippers. Drop the legacy CAN bring-up scripts
(superseded by 'dimos can setup') and the openarm manipulator registry entry.
DM8009 shoulders (DM8006 was a legacy typo), MotorSpecs as plain lists,
keyboard teleop module restored to upstream with gripper bindings deferred
to a follow-up PR, and dual-arm planning switched to a single bimanual
robot model with left_manipulator and right_manipulator groups fed by a
hand-written SRDF, since generated SRDFs cannot express cross-robot
collision exclusions. Planner blueprint verified against the in-memory
adapter in simulation.
@KrishnaH96
KrishnaH96 force-pushed the krishna/feat/openarm-damiao branch from b7c5e7f to 7be88df Compare August 4, 2026 19:32
Replace the stale v1.0 data package with URDFs generated from the official
OpenArm v2.0 preset pipeline (enactic/openarm_description @ 6c7b720f1ba,
default_bimanual plus per-side pinch gripper presets). Pinch gripper finger
joints are fixed in the generated models so each arm exposes exactly its
seven driven joints while keeping gripper geometry and mass; mesh URIs stay
package relative and ros2_control blocks are dropped. The package ships
v2.0 meshes, the three URDFs, and a PROVENANCE file, and shrinks from 70 MB
to 8 MB.

The v2.0 generator collapses link7, so planning tips move to
openarm_{side}_ee_base_link and the SRDF now disables the sibling finger
pair per hand. Joint naming is unchanged from v1.0, so the adapter topology
and gravity joint order carry over. Verified with pinocchio (14 and 7 DOF
models, finite gravity) and a planner blueprint run in simulation.
RoboPlan generated composite planning groups only for selections spanning
two or more robots, so a bimanual robot modeled as one URDF with two
planning groups could not plan both arms in a single request. Drop the
robot-count restriction; the joint-disjointness requirement and the
composite group cap still apply, and overlapping selections are already
rejected at the selection layer. Verified in process on the OpenArm 2.0
bimanual model: left, right, and combined fourteen-joint plans all
succeed.
Converted OBJ files were named by stem only, so a robot whose visual and
collision meshes share a stem (OpenArm v2.0 uses visual/link3.dae and
collision/link3.stl) had the collision conversion overwrite the visual
one, and viewers rendered collision geometry in visual mode. Suffix the
converted name with a hash of the source path and pin the behavior with
tests.
Regenerate the v2.0 URDFs with emit_grasp_frame enabled and move the
planning group tip links from the ee flange to openarm_{side}_grasp_frame,
matching the OpenYAM gripper_tip convention. Model DOF, joint order, and
gravity behavior are unchanged; planning verified for left, right, and
combined requests.
The optimistic target ghost rebuilt its per-robot joint values from the
current state for every selected group, so with two planning groups on
one robot the group processed last discarded the other group's target
and the ghost only ever showed one arm's goal. Seed the merge once per
robot and overlay each group's target into the same values.
@KrishnaH96
KrishnaH96 force-pushed the krishna/feat/openarm-damiao branch from 642e299 to 64efdee Compare August 5, 2026 03:22
Comment thread dimos/robot/manipulators/openarm/openarm_v20_bimanual.srdf Outdated
The pinch gripper finger joints are fixed in the generated models and
their collision meshes do not intersect at the fixed pose, so the manual
exclusions were dead weight. Verified left, right, and combined plans
still succeed without the file.
Gripper opening calibrates during activation, and reading it earlier
raises, so a connected but not activated whole-body adapter failed the
entire state read and read-only bring-up sessions (connect without
enable) streamed nothing. Report placeholder gripper states until the
adapter is active; arms stream immediately and real openings appear
after activation. Found on OpenArm hardware during the read-only phase.
Damiao feedback only updates when the bus is ticked, which the write
path does once per control cycle while the adapter is active. A
connected but not activated adapter never ticked, so read-only sessions
streamed the connect-time snapshot forever. Refresh from the read path
whenever the adapter is inactive; the active path is unchanged and
still ticks exactly once per cycle.
joint_names=openarm_arm_joints(side),
priority=priority,
params={
"model_path": OPENARM_LEFT_MODEL if side == "left" else OPENARM_RIGHT_MODEL,

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

I prefer no left/right model at all but I understand that this is for testing, I'll cleanup later once teleop works

_teleop_hw = openarm_single_hardware()
# The keyboard publishes twists to one task by name; the right arm's task
# keeps holding its anchor pose.
KEYBOARD_EEF_TASK_NAME = "eef_twist_left_arm"

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

if you only wired left side here, why wiring both side eef task in control coordinator? should only wire one side as well

@TomCC7
TomCC7 marked this pull request as ready for review August 6, 2026 01:06
@github-actions github-actions Bot added the ready-to-merge Required CI checks have passed on this PR label Aug 6, 2026
@greptile-apps

greptile-apps Bot commented Aug 6, 2026

Copy link
Copy Markdown
Contributor

Greptile Summary

This PR replaces OpenArm’s legacy per-arm CAN integration with the generic bimanual Damiao whole-body stack.

  • Adds a 16-joint OpenArm whole-body adapter spanning two CAN buses, including both grippers and bimanual gravity compensation.
  • Reworks OpenArm hardware, planner, coordinator, and keyboard-teleop blueprints around a single bimanual model.
  • Updates planning-group composition, mesh conversion naming, inactive Damiao feedback, Viser state merging, tests, registries, assets, and documentation.

Confidence Score: 5/5

The PR appears safe to merge based on the reviewed changes, with no unacknowledged actionable defects identified.

The whole-body joint ordering, planner mapping, adapter topology, simulation selection, and blueprint registry changes are internally consistent, while the unavailable keyboard gripper behavior is explicitly documented as deferred.

Important Files Changed

Filename Overview
dimos/hardware/whole_body/openarm_damiao/adapter.py Defines the physical two-bus OpenArm motor topology, joint ordering, grippers, and bimanual gravity model.
dimos/hardware/whole_body/damiao/adapter.py Refreshes feedback during inactive reads and emits placeholder gripper states before activation.
dimos/robot/manipulators/openarm/config.py Consolidates OpenArm into one simulation-aware whole-body component and maps coordinator joints to the bimanual URDF.
dimos/robot/manipulators/openarm/blueprints/basic.py Replaces separate per-arm blueprints with unified coordinator and planner-coordinator configurations.
dimos/robot/manipulators/openarm/blueprints/teleop.py Adds bimanual EEF-twist teleop and planner variants, while documented keyboard gripper support remains deferred.
dimos/manipulation/planning/world/roboplan_model.py Allows combined planning groups belonging to the same robot when their joints do not overlap.
dimos/manipulation/planning/utils/mesh_utils.py Prevents same-stem mesh conversions from overwriting one another by adding a deterministic source-path tag.
dimos/manipulation/visualization/viser/gui.py Reuses merged robot state while applying planning-group targets in Viser.
dimos/robot/all_blueprints.py Registers the new unified OpenArm coordinator, planner, and teleop blueprints and removes superseded names.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  Keyboard[Keyboard teleop] --> Coordinator[ControlCoordinator]
  Planner[Bimanual planner] --> Coordinator
  Coordinator --> Hardware[OpenArm whole-body component]
  Hardware --> Adapter[OpenArmDamiaoAdapter]
  Adapter --> LeftBus[Left CAN bus]
  Adapter --> RightBus[Right CAN bus]
  LeftBus --> LeftArm[Left arm and gripper]
  RightBus --> RightArm[Right arm and gripper]
  Adapter --> Gravity[Bimanual URDF gravity model]
Loading

Reviews (1): Last reviewed commit: "fix(manipulation): pump feedback from th..." | Re-trigger Greptile


ADAPTER_FACTORIES = {
"openarm": "dimos.hardware.manipulators.openarm.adapter:OpenArmAdapter",
"openarm_damiao": ("dimos.hardware.whole_body.openarm_damiao.adapter:OpenArmDamiaoAdapter"),

@TomCC7 TomCC7 Aug 6, 2026

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

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

this should just be openarm as we removed in-house solution

@KrishnaH96
KrishnaH96 merged commit b7a63c2 into cc/feat/openyam-driver Aug 6, 2026
49 of 52 checks passed
@KrishnaH96
KrishnaH96 deleted the krishna/feat/openarm-damiao branch August 6, 2026 17:58
@TomCC7
TomCC7 restored the krishna/feat/openarm-damiao branch August 6, 2026 18:00
@TomCC7

TomCC7 commented Aug 6, 2026

Copy link
Copy Markdown
Member

Replacement PR opened as #3388. GitHub cannot reopen this PR because it was already merged; #3388 restores the same head branch after revert #3387.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

ready-to-merge Required CI checks have passed on this PR

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants