feat: add joint servo and cartesian controllers by mustafab0 · Pull Request #1116 · dimensionalOS/dimos

mustafab0 · 2026-01-26T22:56:33Z

JointServoTask for streaming joint position control (teleop, playback) and
JointVelocityTask for streaming joint velocity control (joystick) also,
Added CartesianIKTask with internal Pinocchio IK solver for cartesian control on any joint-space hardware
Add joint_command and cartesian_command input streams to ControlCoordinator for real-time streaming control via LCM
Add unified task_invoke RPC to interact with any ControlTask method
Add pygame-based cartesian jogger example for testing IK control

closes DIM-352

greptile-apps · 2026-01-26T23:00:07Z

Greptile Overview

Greptile Summary

Adds comprehensive streaming control capabilities to the control system with three new task types: JointServoTask for position streaming, JointVelocityTask for velocity streaming with safety timeouts, and CartesianIKTask for cartesian control with internal Pinocchio IK solver. The coordinator now supports real-time streaming control via joint_command and cartesian_command LCM inputs, with intelligent routing to tasks by joint name or task name.

Key changes:

New control tasks: Servo, velocity, and cartesian IK tasks with thread-safe state management and timeout handling
Streaming control: Added LCM input streams for joint_command (JointState) and cartesian_command (PoseStamped)
Unified RPC: New task_invoke method for calling any task method with auto-injection of current time
Hardware refactor: Adapter registry pattern with auto-discovery for cleaner hardware management
Safety features: Velocity timeout with zero-on-timeout, IK joint delta clamping, warm-start optimization
Testing support: Mock adapter for hardware-free testing, pygame-based cartesian jogger example

Implementation quality:

Proper thread synchronization throughout
Consistent use of coordinator time (no direct time.time() calls in tasks)
Comprehensive safety checks (timeouts, joint limits, singularity handling)
Clean separation of concerns between tasks, coordinator, and hardware
Good test coverage with updated blueprints

Confidence Score: 4/5

Safe to merge with minor considerations - well-structured implementation with proper safety mechanisms
High-quality implementation with proper thread safety, timeout handling, and safety checks. The IK solver includes singularity handling and motion limiting. Code follows existing patterns and includes comprehensive examples and tests. Score is 4/5 rather than 5/5 due to the complexity and scope of changes - while the implementation appears solid, this adds significant new functionality that should be thoroughly tested in real-world scenarios
Pay close attention to dimos/control/tasks/cartesian_ik_task.py for IK solver behavior under edge cases (singularities, workspace limits). Verify timeout behavior in dimos/control/tasks/velocity_task.py matches safety requirements for your hardware

Important Files Changed

Filename	Overview
dimos/control/tasks/servo_task.py	Added `JointServoTask` for streaming joint position control with timeout handling, thread-safe state management, and proper integration with the coordinator's arbitration system
dimos/control/tasks/velocity_task.py	Added `JointVelocityTask` for streaming velocity control with safety-critical zero-on-timeout behavior and proper thread synchronization
dimos/control/tasks/cartesian_ik_task.py	Added `CartesianIKTask` with Pinocchio-based IK solver, safety clamping, and warm-start optimization - includes proper singularity handling and motion limiting
dimos/control/coordinator.py	Added streaming control support with `joint_command` and `cartesian_command` inputs, unified `task_invoke` RPC method, and task factory for config-based task creation
dimos/control/blueprints.py	Reorganized and expanded blueprints with new servo, velocity, and cartesian IK task configurations, plus streaming and cartesian transport helpers
dimos/hardware/manipulators/registry.py	Added adapter registry with auto-discovery for manipulator adapters, enabling dynamic adapter creation by name

Sequence Diagram

sequenceDiagram
    participant User as User/Teleop
    participant LCM as LCM Transport
    participant Coord as ControlCoordinator
    participant ServoTask as JointServoTask
    participant VelTask as JointVelocityTask
    participant IKTask as CartesianIKTask
    participant Pinocchio as Pinocchio IK
    participant TickLoop as TickLoop
    participant Hardware as Hardware Adapter

    Note over User,Hardware: Streaming Joint Position Control (Servo)
    User->>LCM: Publish JointState (positions)
    LCM->>Coord: joint_command callback
    Coord->>ServoTask: set_target_by_name(positions, t_now)
    ServoTask->>ServoTask: Update target & timestamp
    
    Note over TickLoop,Hardware: Control Tick (100Hz)
    TickLoop->>Hardware: read_joint_positions()
    Hardware-->>TickLoop: current positions
    TickLoop->>ServoTask: compute(state)
    ServoTask->>ServoTask: Check timeout
    ServoTask-->>TickLoop: JointCommandOutput (SERVO_POSITION)
    TickLoop->>TickLoop: Arbitrate (priority-based)
    TickLoop->>Hardware: write_joint_positions(commands)

    Note over User,Hardware: Streaming Velocity Control
    User->>LCM: Publish JointState (velocities)
    LCM->>Coord: joint_command callback
    Coord->>VelTask: set_velocities_by_name(velocities, t_now)
    VelTask->>VelTask: Update velocities & timestamp
    
    TickLoop->>VelTask: compute(state)
    VelTask->>VelTask: Check timeout
    alt Timeout
        VelTask-->>TickLoop: Zero velocities (safety)
    else Normal
        VelTask-->>TickLoop: JointCommandOutput (VELOCITY)
    end
    TickLoop->>Hardware: write_joint_velocities(commands)

    Note over User,Hardware: Cartesian IK Control
    User->>LCM: Publish PoseStamped (cartesian pose)
    LCM->>Coord: cartesian_command callback
    Coord->>IKTask: set_target_pose(pose, t_now)
    IKTask->>IKTask: Convert to SE3 & store
    
    TickLoop->>IKTask: compute(state)
    IKTask->>IKTask: Get current joints (warm-start)
    IKTask->>Pinocchio: solve_ik(target_pose, q_current)
    loop Max 100 iterations
        Pinocchio->>Pinocchio: Forward kinematics
        Pinocchio->>Pinocchio: Compute Jacobian
        Pinocchio->>Pinocchio: Damped least-squares update
        Pinocchio->>Pinocchio: Check convergence
    end
    Pinocchio-->>IKTask: q_solution, converged, error
    IKTask->>IKTask: Safety check (clamp joint deltas)
    IKTask-->>TickLoop: JointCommandOutput (SERVO_POSITION)
    TickLoop->>Hardware: write_joint_positions(ik_solution)

    Note over Coord: Unified RPC Interface
    User->>Coord: task_invoke("task_name", "start", kwargs)
    Coord->>ServoTask: start()
    ServoTask-->>Coord: activated

greptile-apps

_{No files reviewed, no comments}

_{Edit Code Review Agent Settings | Greptile}

dimos/control/blueprints.py

…oint type classes

… dataclass

…lity

…c rpcs to 1 rpc to interact with all ControlTasks

…ys there but not caught before mypy wasn't strict enough to catch them until pinocchio's numpy type annotations tightened the checking.

…#1079) * added robot_description folders to data/ folder with lfs tracking * Base types: RobotModelConfig, Obstacle, Protocol specs for defining manipulation scenes * Mesh conversion utils for converting any urdf to drake compatible mesh files * DrakeWorld implementation that maintains all objects, robots in the scene and owns the scene graph * Monitor system keeps the world model (drake planning world) in synb with real world. manages obstacle lifecycle and more * FK and IK solver implementation with drake * RRT path planner using default drake planner * factory for building implementation of the different world, viz, planner etc specs * utils for ik solver and path planning * manipulation module manages the world monitor, planner, kinematics planner and interfaces with other modules * added blueprints and manipulation client for testing * added unit test, e2e tests and readme * general cleanup * added multi robot management and control to the manipulation module * refactored planning stack * Refactored manipulation planning stack: seperated planners from world implementation, split kinematics into JacobianIK and DrakeOptimizationIK * updated README * Address greptile comments: manipulation_blueprint - Added optional add_gripper: bool = True to make xarm6 ans xarm7 config consistent. world_obstacle_monitor - Added warning log when obstacle not found during cleanup manipulation_client - Removed unused numpy impor path_utils - Added explicit tolerances atol=1e-6, rtol=0 to np.allclose() for stricter joint-space duplicate detection jacobian_ik - Added division-by-zero protection for velocity limits using nonzero_mask to skip zero-valued limits drake_world.py - added more specific exception handling (avoids hiding bugs) mesh_utils - regex fixes * fix mypy import error with manipulation interface (file is deprecated) * protocol now only requires solve() method. * moved to using standard LCM data types and added type alias for readability * updated all coordinator references * fixed all bluepirnts order * updated paths to use Path object instead of str * mypy type fixes * all public api boundaries use standard message types, sim specific internal storage/helpers still use NDArrays in some places * removed unused import * removed manipulation history reference * added a Jacobian typealias * fixed mypy errors * updated manipulation dependencies in pyproject.toml

spomichter · 2026-02-06T11:32:22Z

dimos/control/examples/cartesian_ik_jogger.py

+
+
+# Jog speeds
+LINEAR_SPEED = 0.05  # m/s


shouldnt these be at top and not buried down at bottom

Yeah, can also make this user parameter or task parameter

spomichter · 2026-02-06T11:35:36Z

dimos/control/examples/cartesian_ik_jogger.py

+    return max(min_val, min(max_val, value))
+
+
+def _get_piper_model_path() -> str:


Shouldnt be any arm specific stuff in generic methods. UNless this is just an example thing but seems like it may be used by other arms

Shouldnt have piper in here

You are right. This was implemented as an example with piper. I have asked @ruthwikdasyam to work on generic examples and extending support to more arms.

spomichter · 2026-02-06T11:38:47Z

dimos/control/tasks/cartesian_ik_task.py

+
+        return True, q_new
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:


Doesn't look like any of these on_preempted callbacks are used. Also not sure what they do its just for printing warning

When there are multiple task controlling a hardware, and one task overrides another. then it logs a warning that this task was preempted, just so that it doesn't silently fail.

spomichter · 2026-02-06T11:38:59Z

dimos/control/tasks/servo_task.py

+                mode=ControlMode.SERVO_POSITION,
+            )
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:


spomichter · 2026-02-06T11:39:05Z

dimos/control/tasks/velocity_task.py

+                mode=ControlMode.VELOCITY,
+            )
+
+    def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:


greptile-apps bot reviewed Jan 26, 2026

View reviewed changes

mustafab0 force-pushed the feature/mustafa_add_joint_servo_controllers branch 2 times, most recently from 7cf4229 to 0ae380d Compare January 28, 2026 18:51

mustafab0 changed the title ~~Feature/mustafa add joint servo controllers~~ feat: add joint servo and cartesian controllers Jan 28, 2026

spomichter reviewed Feb 1, 2026

View reviewed changes

dimos/control/blueprints.py Outdated Show resolved Hide resolved

mustafab0 and others added 18 commits February 2, 2026 12:48

added HardwareComponent defining universal schema fo robot type and j…

88bf5c6

…oint type classes

removed simple HardwareConfig and replaced with new HardwareComponent…

f8f9ec5

… dataclass

added JointState dataclass and JointName str alias for better readabi…

e27a3d8

…lity

renamed control orchestrator to control coordinator as its easier to say

0f8c265

added explicit type alias for JointName in ControlTask

a88aa5a

addded new joint position controller and joint velocity controller tasks

031ebff

added Input for coordinator for LCM streams and replaced task specifi…

a302ebe

…c rpcs to 1 rpc to interact with all ControlTasks

updated blueprints

7ee6c42

CI code cleanup

6696222

added robot description to lfs

925097d

PoseStamped stream adde to coordinator module

b633736

cartesian ik task with internal pinnochio ik solver

175e68b

updated blueprints and added a pygame jogger for teleop

dc68435

mypy type fixes

3a54b12

CI test fixes

e98d4c4

addressed latent type errors in testdata.py and seg.py that were alwa…

5c5fd70

…ys there but not caught before mypy wasn't strict enough to catch them until pinocchio's numpy type annotations tightened the checking.

reverted blueprints to standalone style

1da50f3

mustafab0 force-pushed the feature/mustafa_add_joint_servo_controllers branch from 5ecf320 to 1da50f3 Compare February 2, 2026 20:59

Merge branch 'dev' into feature/mustafa_add_joint_servo_controllers

50cb2d9

linear bot mentioned this pull request Feb 3, 2026

Add controllers to the Tasks #1012

Open

Merge branch 'dev' into feature/mustafa_add_joint_servo_controllers

65e06f2

spomichter reviewed Feb 6, 2026

View reviewed changes

spomichter approved these changes Feb 6, 2026

View reviewed changes

mustafab0 merged commit 168aef4 into dev Feb 6, 2026
27 of 28 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

feat: add joint servo and cartesian controllers#1116

feat: add joint servo and cartesian controllers#1116
mustafab0 merged 20 commits intodevfrom
feature/mustafa_add_joint_servo_controllers

mustafab0 commented Jan 26, 2026 •

edited

Loading

Uh oh!

greptile-apps bot commented Jan 26, 2026

Uh oh!

greptile-apps bot left a comment

Uh oh!

Uh oh!

spomichter Feb 6, 2026

Uh oh!

mustafab0 Feb 6, 2026

Uh oh!

spomichter Feb 6, 2026

Uh oh!

spomichter Feb 6, 2026

Uh oh!

mustafab0 Feb 6, 2026

Uh oh!

spomichter Feb 6, 2026

Uh oh!

mustafab0 Feb 6, 2026

Uh oh!

spomichter Feb 6, 2026

Uh oh!

spomichter Feb 6, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

		return max(min_val, min(max_val, value))


		def _get_piper_model_path() -> str:


		return True, q_new

		def on_preempted(self, by_task: str, joints: frozenset[str]) -> None:

Conversation

mustafab0 commented Jan 26, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

greptile-apps bot commented Jan 26, 2026

Greptile Overview

Greptile Summary

Confidence Score: 4/5

Important Files Changed

Sequence Diagram

Uh oh!

greptile-apps bot left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

mustafab0 commented Jan 26, 2026 •

edited

Loading