feat: add joint servo and cartesian controllers

[mustafab0]

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

[greptile-apps]

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

Loading

[greptile-apps]

_{No files reviewed, no comments}

_{Edit Code Review Agent Settings | Greptile}

[spomichter]

isnt it clearer for documentation purposes to have the transports actually listed here instead of referenced from this method?

[mustafab0]

I reverted to old standalone blueprint style