Skip to content

ARCHITECTURE.md

Latest commit

 

History

History
1420 lines (1065 loc) · 36.5 KB

ARCHITECTURE.md

File metadata and controls

1420 lines (1065 loc) · 36.5 KB

RynnMotion C++ Architecture

Comprehensive guide to the C++20 motion control core

Last updated: November 22, 2025

Table of Contents

  1. Overview
  2. Directory Structure
  3. Core Architecture Pattern
  4. Component Details
  5. Module Lifecycle
  6. Data Flow
  7. Auto-Discovery System
  8. Recent Refactoring History
  9. Adding Components

Overview

RynnMotion's C++ core provides real-time robot control with:

  • Multi-robot support via auto-discovery (8+ robot arms, 6 dual-arm variants)
  • Pinocchio-based kinematics and dynamics
  • MuJoCo simulation integration with direct physics access
  • Modular control pipeline (Estimator → Planner → OSC)
  • Hardware interfaces for Franka, UR, Piper, RealMan, etc.

Key Statistics:

  • ~11,254 lines of C++20 code
  • 145 MJCF model files (308MB)
  • 5 core managers, 8+ module types
  • Zero ROS dependency (ROS-optional design)

Directory Structure

RynnMotion/
├── motion/                    # C++ motion control core (~11,254 lines)
│   ├── manager/              # Robot, MJCF parsing
│   │   ├── robot_manager.hpp/cpp
│   │   └── mjcf_parser.hpp/cpp
│   ├── runtime/              # Module orchestration, scene, FSM
│   │   ├── module_manager.hpp/cpp
│   │   ├── scene_manager.hpp/cpp
│   │   └── fsm_manager.hpp/cpp
│   ├── module/               # Control modules (inherits CModuleBase)
│   │   ├── module_base.hpp/cpp
│   │   ├── estimation/       # Forward kinematics, Jacobians
│   │   │   └── estimator.hpp/cpp
│   │   ├── planner/          # Trajectory generation
│   │   │   └── planner.hpp/cpp
│   │   ├── osc/              # Operational space control (IK)
│   │   │   └── osc.hpp/cpp
│   │   ├── joint/            # Direct joint control
│   │   │   ├── joint_move.hpp/cpp
│   │   │   └── fr3_jointmove.hpp/cpp
│   │   └── teleopfollow/     # Teleoperation controllers
│   │       ├── teleop_follow.hpp/cpp
│   │       └── fr3_teleopfollow.hpp/cpp
│   ├── utils/                # Utilities (kinematics, IK, trajectories)
│   │   ├── kine_pin.hpp/cpp         # Pinocchio wrapper
│   │   ├── diff_ik.hpp/cpp          # Differential IK (QP-based)
│   │   ├── dyn_pin.hpp/cpp          # Dynamics computations
│   │   ├── trajectory/              # Ruckig, curves
│   │   ├── filter.hpp               # Signal filters
│   │   └── orientation.hpp          # Quaternion/RPY tools
│   ├── interface/            # Sim/hardware abstraction
│   │   └── interface_base.hpp/cpp
│   └── common/               # Data structures
│       └── data/
│           └── runtime_data.hpp     # Shared state (150 lines)
│
├── mujoco/                    # MuJoCo simulation interface
│   ├── rynn_mujoco.hpp/cpp
│   └── CMakeLists.txt
│
├── models/                    # 308MB MJCF robot models (145 files)
│   ├── 1.mobile_robot/
│   ├── 2.mobile_arm/
│   ├── 3.robot_arm/          # Primary focus (8+ arms)
│   │   ├── 1.fr3/
│   │   ├── 2.ur5e/
│   │   ├── 3.piper/
│   │   ├── 10.dual_fr3/
│   │   └── ...
│   └── how_to_add_new_robot_scene.md
│
├── robots/                    # Robot-specific hardware drivers
│   ├── franka/
│   ├── ur/
│   ├── piper/
│   └── ...
│
├── python/                    # Python bindings + dataset tools
│   ├── src/RynnMotion/
│   │   ├── datasets/         # HDF5, HuggingFace integration
│   │   ├── teleop/           # Teleoperation framework
│   │   └── utils/            # Trajectory, kinematics
│   └── tests/
│
├── cmake/                     # Build system
│   ├── GenerateRobotTypes.cmake    # Auto-discovery magic
│   └── ...
│
└── docs/                      # Documentation
    ├── ARCHITECTURE.md        # This file
    ├── OPEN_SOURCE_ROADMAP.md
    ├── c++_coding_style.md
    └── ...

Core Architecture Pattern

Shared RuntimeData Pattern

RynnMotion uses a shared state architecture where all control modules read from and write to a single RuntimeData struct.

                    ┌──────────────────────────┐
                    │   RuntimeData (Shared)    │
                    │                           │
                    │  • qFb, qdFb, qtauFb      │
                    │  • bodyStates[]           │
                    │  • jacobians[]            │
                    │  • bodyPlans[]            │
                    │  • gripperCommands[]      │
                    │  • mjModel*, mjData*      │
                    └──────────┬────────────────┘
                               │
              ┌────────────────┼────────────────┐
              │                │                │
              ▼                ▼                ▼
        ┌─────────┐      ┌──────────┐     ┌──────────┐
        │Estimator│      │ Planner  │     │   OSC    │
        │   FK    │      │ TrajGen  │     │   IK     │
        └─────────┘      └──────────┘     └──────────┘
             │                │                │
             ▼                ▼                ▼
        Update Order:    1  →  2  →  3

        Estimator:  qFb         → bodyStates[], jacobians[]
        Planner:    bodyStates  → bodyPlans[], gripperCmd[]
        OSC:        bodyPlans   → qCmd, qdCmd, qtauCmd

Key Insight: No input/output separation - modules share state directly.

Benefits:

  • Cache-friendly (flat struct, no pointer chasing)
  • Zero-copy sensor access (direct mjData*)
  • Clear read/write contracts (documented in each module)
  • Simple to reason about (vs pipeline abstractions)

Comparison:

  • vs MoveIt: MoveIt uses ROS message passing (heavy overhead)
  • vs Drake: Similar SharedData concept (we're simpler)

Component Details

1. RuntimeData (Shared State)

File: common/data/runtime_data.hpp (150 lines)

Purpose: Single source of truth for entire control loop.

Key Fields:

struct RuntimeData {
  // Joint feedback (from simulation or hardware)
  Eigen::VectorXd qFb, qdFb, qtauFb;

  // Joint commands (to simulation or hardware)
  Eigen::VectorXd qCmd, qdCmd, qtauCmd;

  // Gains
  Eigen::VectorXd kp, kd;

  // End-effector states (estimated by Estimator)
  std::vector<RigidBodyState> bodyStates;   // Actual EE poses
  std::vector<Jacobian> jacobians;          // Jacobians per EE

  // End-effector plans (generated by Planner)
  std::vector<BodyPlan> bodyPlans;          // Desired EE trajectories

  // Gripper commands
  std::vector<GripperCommand> gripperCommands;

  // Scene objects (cubes, etc.)
  std::vector<Object> objects;

  // Camera data
  std::vector<CameraData> cameras;

  // Direct MuJoCo access (zero-copy)
  mjModel* mjModel_;
  mjData* mjData_;

  // Timing
  double simTime;
  double wallTime;
  double duration;

  // Helper methods
  void addBodyPlanner();
  void addBodyFeedback(const std::string& name);
  void addJacobian(const std::string& name);
  auto* getObjectByName(const std::string& name);
  void setJointsCommand(VectorXd q, VectorXd qd, VectorXd qtau);
  void getJointsFeedback(VectorXd& q, VectorXd& qd, VectorXd& qtau);
};

Design Note: Flat struct with value semantics (not shared_ptr).

Historical Context: Replaced 1034-line DataGroup in Nov 2025 refactoring.

2. Managers

2.1 RobotManager

File: manager/robot_manager.hpp/cpp

Responsibilities:

  • Load robot MJCF files ({robot}_robot.xml, {robot}_pinocchio.xml)
  • Parse joint limits, actuator modes, keyframes
  • Provide robot configuration queries

Key Methods:

class RobotManager {
public:
  RobotManager(int robotNumber);  // Auto-discovery constructor

  // Robot configuration
  std::string getRobotMJCF() const;
  std::string getPinoMJCF() const;
  int getMotionDOF() const;
  int getActionDOF() const;

  // Joint limits
  Eigen::VectorXd getJointPosMin() const;
  Eigen::VectorXd getJointPosMax() const;
  Eigen::VectorXd getJointVelMax() const;

  // End-effector info
  int getNumEndEffectors() const;
  std::vector<int> getEEJointIndices() const;

  // Keyframes from MJCF
  Eigen::VectorXd getKeyframe(const std::string& name) const;

  // Simulation gains from MJCF
  Eigen::VectorXd getSimKp() const;
  Eigen::VectorXd getSimKd() const;

private:
  int _mdof, _adof;  // Motion DOF, action DOF (excludes grippers)
  std::vector<int> _eeJointIndices;
  std::string _robotMjcfPath, _pinoMjcfPath;
  // ... parsed from MJCF by MjcfParser
};

Usage Pattern:

int mdof = robotManager->getMotionDOF();
auto limits = robotManager->getJointPosMin();
auto kp = robotManager->getSimKp();

2.2 ModuleManager

File: runtime/module_manager.hpp/cpp

Responsibilities:

  • Create module chain from YAML config
  • Orchestrate module lifecycle
  • Call update() sequentially

Key Methods:

class ModuleManager {
public:
  void createModuleChain(const ModuleChainConfig& config);
  void updateAllModules();  // Calls each module's update()

private:
  std::vector<std::unique_ptr<CModuleBase>> modules_;
  RuntimeData& runtimeData_;  // Owned by parent, shared with modules
};

Module Chain Example (YAML):

scene_tracking:
  modules: [Estimator, Planner, OSC]

scene_pickplace:
  modules: [Estimator, Planner]  # Planner handles both trajgen + control

Lifecycle:

// In ModuleManager::createModuleChain()
for (auto moduleType : config.modules) {
  auto module = createModuleByType(moduleType);

  module->setRuntimeData(runtimeDataPtr);  // Share RuntimeData
  module->initModule();                    // One-time initialization

  modules_.push_back(std::move(module));
}

// In ModuleManager::updateAllModules()
for (auto& module : modules_) {
  module->update();  // Sequential update
}

2.3 SceneManager

File: runtime/scene_manager.hpp/cpp

Responsibilities:

  • Load scene MJCF (scene.xml)
  • Manage objects (cubes, walls, etc.)
  • Provide origins, camera poses

Key Methods:

class SceneManager {
public:
  void loadScene(int sceneNumber);

  Object* getObjectByName(const std::string& name);
  Eigen::Vector3d getOrigin(int idx) const;
  Eigen::Vector3d getCameraPos(int idx) const;

private:
  std::vector<Object> objects_;
  std::vector<Eigen::Vector3d> origins_;
  std::vector<Eigen::Vector3d> cameraPoses_;
};

2.4 FsmManager

File: runtime/fsm_manager.hpp/cpp

Responsibilities:

  • State machine for pick-and-place, etc.
  • Transition logic based on time/conditions
  • Provides isOnEntry(), isOnExit() for modules

State Machine Example:

enum class StateID {
  Init,
  Action1,    // e.g., "approach"
  Action2,    // e.g., "grasp"
  Action3,    // e.g., "lift"
  Action4,    // e.g., "place"
  Done
};

// In module update():
if (fsmManager_->getCurrentState() == StateID::Action1) {
  // Approach logic
}

3. Modules

All modules inherit from CModuleBase.

3.1 CModuleBase

File: module/module_base.hpp/cpp

Interface:

class CModuleBase {
public:
  virtual void loadYaml() {}      // Parse YAML config
  virtual void update() = 0;      // Control loop (called every timestep)
  virtual void initModule() {}    // One-time initialization
  virtual bool resetModule() {}   // Reset state (optional)

  void setRuntimeData(std::shared_ptr<RuntimeData> data);
  void setManagers(RobotManager& robot, SceneManager& scene, FsmManager& fsm);

protected:
  std::shared_ptr<RuntimeData> runtimeData_;  // Shared state
  RobotManager* robotManager;
  SceneManager* sceneManager;
  FsmManager* fsmManager_;
  YAML::Node _yamlNode;  // Config for this module

  double getDt() const;  // Timestep
};

Lifecycle:

  1. Constructor: MyModule(yamlNode) - store YAML
  2. setRuntimeData(): Receive shared RuntimeData
  3. setManagers(): Receive manager references
  4. initModule(): Allocate internal data structures
    • Add structures to RuntimeData (e.g., addBodyPlanner())
    • Allocate Eigen vectors
    • Create sub-controllers
  5. update(): Control loop (called every timestep)

Important: All dependencies (robotManager, runtimeData_) are valid in initModule() and update().

3.2 CEstimator

File: module/estimation/estimator.hpp/cpp

Purpose: Forward kinematics and state estimation.

Reads: qFb, qdFb, qtauFb, simTime Writes: bodyStates[] (EE poses), jacobians[]

Implementation:

class CEstimator : public CModuleBase {
public:
  void initModule() override {
    // Add body states and jacobians to RuntimeData
    int numEE = robotManager->getNumEndEffectors();
    for (int i = 0; i < numEE; i++) {
      runtimeData_->addBodyFeedback("EE_" + std::to_string(i));
      runtimeData_->addJacobian("EE_" + std::to_string(i));
    }

    // Allocate internal vectors
    _qFb = Eigen::VectorXd::Zero(robotManager->getMotionDOF());
    _qdFb = Eigen::VectorXd::Zero(robotManager->getMotionDOF());
  }

  void update() override {
    _stateEstimation();  // Compute FK, Jacobians
  }

private:
  void _stateEstimation() {
    // Get joint feedback
    runtimeData_->getJointsFeedback(_qFb, _qdFb, _qtauFb);

    // Compute FK using Pinocchio
    pinKine_->update(_qFb);

    // Write to RuntimeData
    for (int i = 0; i < numEE; i++) {
      runtimeData_->bodyStates[i].pos = pinKine_->getEEPos(i);
      runtimeData_->bodyStates[i].quat = pinKine_->getEEQuat(i);
      runtimeData_->jacobians[i].jaco = pinKine_->getEEJaco(i);
    }
  }
};

3.3 CPlanner

File: module/planner/planner.hpp/cpp

Purpose: Trajectory generation (keyframes, pick-place, etc.)

Reads: qFb, objects[], simTime Writes: bodyPlans[] (desired EE trajectories), gripperCommands[]

Scene Types:

  • tracking: Follow circular trajectory
  • pickplace: Pick-and-place sequence
  • predefined: Keyframe-based motion

Implementation Highlights:

void CPlanner::initModule() {
  // Add body planners to RuntimeData
  int numEE = robotManager->getNumEndEffectors();
  for (int i = 0; i < numEE; i++) {
    runtimeData_->addBodyPlanner();
  }
}

void CPlanner::update() {
  switch (_sceneType) {
    case SceneType::kTracking:
      eePlanner();  // Circular trajectory
      break;
    case SceneType::kPickPlace:
      pickAndPlace();  // Dual-arm coordination
      break;
    case SceneType::kPredefined:
      predefinedMotion();  // Keyframe interpolation
      break;
  }
}

void CPlanner::followObject() {
  auto* cube = runtimeData_->getObjectByName("cube");

  for (int i = 0; i < _numEE; i++) {
    runtimeData_->bodyPlans[i].pos = cube->pos;

    // Single-arm: no rotation offset
    // Dual-arm: apply grasp orientation offsets
    Eigen::Vector3d deltaRPY;
    if (_numEE == 1) {
      deltaRPY = Eigen::Vector3d::Zero();
    } else {
      deltaRPY = (i == 0)
        ? Eigen::Vector3d(-M_PI/2, M_PI/4, 0.0)   // Left arm
        : Eigen::Vector3d(M_PI/2, -M_PI/4, 0.0);  // Right arm
    }

    runtimeData_->bodyPlans[i].quat = cube->quat * utils::EulerZYX2Quaternion(deltaRPY);
  }
}

3.4 OSC (Operational Space Control)

File: module/osc/osc.hpp/cpp

Purpose: Inverse kinematics (task-space → joint-space control)

Reads: bodyPlans[], bodyStates[], jacobians[], qFb, qdFb Writes: qCmd, qdCmd, qtauCmd

IK Solvers:

  • PseudoInverse: Damped least-squares (simple, fast)
  • DiffQP: QP-based with constraints (vel/pos/acc limits, nullspace)

Implementation:

class OSC : public CModuleBase {
public:
  enum class IKSolver { kPseudoInverse, kDiffQP };

  void initModule() override {
    int numEE = robotManager->getNumEndEffectors();
    _eeStates.resize(numEE);

    // Create QP solvers per EE
    if (_solver == IKSolver::kDiffQP) {
      for (int i = 0; i < numEE; i++) {
        _eeStates[i].dIKqp = std::make_unique<utils::DiffIKQP>(mdof, config);
      }
    }
  }

  void update() override {
    // Compute task-space error
    Eigen::Vector3d posErr = bodyPlans[0].pos - bodyStates[0].pos;
    Eigen::Vector3d ornErr = /* quaternion error */;
    Eigen::VectorXd eeVel_des = [posErr; ornErr] / dt;

    // Solve IK
    switch (_solver) {
      case IKSolver::kPseudoInverse: {
        Eigen::MatrixXd Jpinv = pseudoInverse(_eeJacoFb);
        _qdCmd = Jpinv * eeVel_des;
        _qCmd = _qFb + _qdCmd * dt;
        break;
      }
      case IKSolver::kDiffQP:
        _dIKqp->solve(_eeJacoFb, eeVel_des, _qFb, _qdFb, dt);
        _qdCmd = _dIKqp->getSolution();
        _qCmd = _qFb + _qdCmd * dt;
        break;
    }

    // Write to RuntimeData
    runtimeData_->setJointsCommand(_qCmd, _qdCmd, qtauCmd);
  }

private:
  IKSolver _solver;
  std::vector<EEState> _eeStates;  // Per-EE QP solvers
};

Decoupled Multi-Arm IK:

For dual-arm robots, OSC solves IK independently per arm, then merges:

void OSC::_solveDecoupledIK() {
  for (int i = 0; i < _numEE; i++) {
    // Compute error for EE i
    Eigen::VectorXd err_i = bodyPlans[i] - bodyStates[i];
    Eigen::VectorXd eeVel_i = err_i / dt;

    // Solve IK for EE i
    _eeStates[i].dIKqp->solve(jacobians[i], eeVel_i, qFb, qdFb, dt);
    _qdCmdPerEE[i] = _eeStates[i].dIKqp->getSolution();
  }

  // Merge (weighted average for shared joints)
  _qdCmd = weightedAverage(_qdCmdPerEE);
}

3.5 CJointMove

File: module/joint/joint_move.hpp/cpp

Purpose: Direct joint-space control (no task-space planning)

Reads: qFb, simTime Writes: qCmd, qdCmd, qtauCmd

Use case: Testing joint controllers, sinusoidal trajectories, etc.

4. Utilities

4.1 PinKine (Pinocchio Wrapper)

File: utils/kine_pin.hpp/cpp

Purpose: Forward kinematics, Jacobians using Pinocchio.

Key Methods:

class PinKine {
public:
  PinKine(const std::string& mjcfPath, const std::string& endEffectorName);

  void update(const Eigen::VectorXd& q);  // Update configuration

  // FK
  Eigen::Vector3d getEEPos(int eeIdx = 0) const;
  Eigen::Vector4d getEEQuat(int eeIdx = 0) const;  // [x, y, z, w]
  Eigen::Matrix4d getEETransform(int eeIdx = 0) const;

  // Jacobians
  Eigen::MatrixXd getEEJaco(int eeIdx = 0) const;  // 6xN
  Eigen::VectorXd getEEVel(int eeIdx = 0) const;   // 6x1

  // IK (simple iterative)
  Eigen::VectorXd ikPos(const Eigen::Vector3d& pos,
                        const Eigen::Quaterniond& quat,
                        const Eigen::VectorXd& qInit) const;
};

Usage:

auto pinKine = std::make_unique<PinKine>(robotManager->getPinoMJCF(), "EE");
pinKine->update(qFb);

Eigen::Vector3d eePos = pinKine->getEEPos();
Eigen::MatrixXd jaco = pinKine->getEEJaco();

4.2 DiffIKQP (QP-based Differential IK)

File: utils/diff_ik.hpp/cpp

Purpose: Solve IK with constraints using QP:

min    || J * qd - xd_des ||^2 + lambda * || qd ||^2
s.t.   qd_min <= qd <= qd_max
       q_min <= q + qd*dt <= q_max
       qdd_min <= (qd - qd_last)/dt <= qdd_max
       (optional) nullspace objective

Key Methods:

struct DiffIKConfig {
  bool enableVelLimits, enablePosLimits, enableAccLimits, enableNullSpace;
  Eigen::VectorXd qdMin, qdMax;      // Velocity limits
  Eigen::VectorXd qMin, qMax;        // Position limits
  Eigen::VectorXd qddMin, qddMax;    // Acceleration limits
  double damping, nullLambda, nullKp;
  int maxIter;
};

class DiffIKQP {
public:
  DiffIKQP(int dof, const DiffIKConfig& config);

  bool solve(const Eigen::MatrixXd& J,        // Jacobian
             const Eigen::VectorXd& xd_des,   // Desired task velocity
             const Eigen::VectorXd& q,        // Current position
             const Eigen::VectorXd& qd,       // Current velocity
             double dt);

  Eigen::VectorXd getSolution() const;  // Returns qd
};

Usage in OSC:

_dIKqp->solve(jacobian, eeVel_des, qFb, qdFb, dt);
Eigen::VectorXd qdCmd = _dIKqp->getSolution();

Module Lifecycle

Initialization Sequence

1. Constructor
   MyModule(yamlNode) {
     loadYaml();  // Parse YAML config
   }

2. setRuntimeData(data)
   Receive shared RuntimeData pointer

3. setManagers(robot, scene, fsm)
   Receive manager references

4. initModule()
   • Add structures to RuntimeData (addBodyPlanner(), etc.)
   • Allocate internal Eigen vectors
   • Create sub-controllers (QP solvers, kinematics objects)
   • Load keyframes, gains from robotManager

5. update() [called every timestep]
   • Read from runtimeData_
   • Compute control
   • Write to runtimeData_

Critical Rule: Never access robotManager or runtimeData_ in constructor.

Example:

class MyController : public CModuleBase {
public:
  MyController(const YAML::Node& yamlNode) : CModuleBase(yamlNode) {
    loadYaml();  // ✅ OK - only YAML access
    // ❌ NEVER: robotManager->getMotionDOF()  // robotManager not set yet!
  }

  void initModule() override {
    // ✅ Now all dependencies are valid
    int mdof = robotManager->getMotionDOF();
    _qCmd = Eigen::VectorXd::Zero(mdof);
  }
};

Data Flow

Complete Control Loop

┌─────────────────────────────────────────────────────────────┐
│                  Simulation/Hardware                         │
└──────────────┬───────────────────────────────┬──────────────┘
               │                               │
       getFeedbacks()                  setCommands()
               │                               │
               ▼                               ▼
     ┌─────────────────────────────────────────────────┐
     │           RuntimeData (Shared State)             │
     │                                                  │
     │  qFb, qdFb, qtauFb ────────────► qCmd, qdCmd   │
     │         │                             ▲          │
     │         │                             │          │
     │         ▼                             │          │
     │   bodyStates[] ────────► bodyPlans[] │          │
     │   jacobians[]                         │          │
     │                                       │          │
     └────┬──────────┬──────────┬───────────┴─────────┘
          │          │          │
          │          │          │
    ┌─────▼────┐ ┌───▼────┐ ┌──▼─────┐
    │Estimator │ │Planner │ │  OSC   │
    │    FK    │ │TrajGen │ │   IK   │
    └──────────┘ └────────┘ └────────┘
       Step 1      Step 2     Step 3

Detailed Step-by-Step:

0. Simulation Step (MuJoCo)
   → Advances physics, updates sensors

1. InterfaceBase::getFeedbacks()
   → Reads mjData->qpos, qvel, qacc_warmstart (or sensor)
   → Writes to RuntimeData.qFb, qdFb, qtauFb

2. ModuleManager::updateAllModules()

   2a. Estimator::update()
       • Reads: RuntimeData.qFb, qdFb
       • Computes: FK using Pinocchio
       • Writes: RuntimeData.bodyStates[], jacobians[]

   2b. Planner::update()
       • Reads: RuntimeData.bodyStates, objects[], simTime
       • Computes: Desired EE trajectory
       • Writes: RuntimeData.bodyPlans[], gripperCommands[]

   2c. OSC::update()
       • Reads: RuntimeData.bodyPlans[], bodyStates[], jacobians[], qFb, qdFb
       • Computes: IK (task → joint space)
       • Writes: RuntimeData.qCmd, qdCmd, qtauCmd

3. InterfaceBase::setCommands()
   → Reads RuntimeData.qCmd, qdCmd, qtauCmd
   → Writes to mjData->ctrl (or hardware actuators)

4. InterfaceBase::setGripperCommands()
   → Reads RuntimeData.gripperCommands[]
   → Sends to grippers

5. Loop back to Step 0

Auto-Discovery System

The Magic: Drop MJCF → Instant Robot Support

Problem: Adding robots to traditional frameworks requires:

  • Manual C++ enum editing
  • Recompilation
  • Code changes in multiple files

RynnMotion Solution: Models directory IS the source of truth.

How It Works

1. Directory Structure as API

models/
├── 3.robot_arm/
│   ├── 1.fr3/
│   │   ├── mjcf/
│   │   │   ├── fr3_robot.xml         # Full sim model
│   │   │   ├── fr3_pinocchio.xml     # Kinematic tree
│   │   │   └── scene/
│   │   │       └── scene.xml         # Scene (objects, etc.)
│   │   └── keyframe/
│   ├── 2.ur5e/
│   ├── 10.dual_fr3/
│   └── ...

Naming Convention:

  • Category: {NUMBER}.{category_name}/
  • Robot: {NUMBER}.{robot_name}/
  • MJCF: {robot_name}_robot.xml, {robot_name}_pinocchio.xml

2. CMake Auto-Generation

File: cmake/GenerateRobotTypes.cmake

Process:

  1. Scans models/{category}/{NUMBER}.{robot}/
  2. Extracts robot numbers and names
  3. Generates C++ header: include/robot_types_generated.hpp

Generated Code:

// Auto-generated by CMake - DO NOT EDIT
namespace rynn {

enum class RobotType : int {
  fr3 = 1,
  ur5e = 2,
  piper = 3,
  rm75 = 4,
  so101 = 5,
  rizon4s = 6,
  eco65 = 7,
  khi = 8,
  dual_fr3 = 10,
  dual_ur5e = 11,
  dual_piper = 12,
  // ... auto-generated from directory scan
};

constexpr int fr3 = 1;
constexpr int ur5e = 2;
// ...

} // namespace rynn

3. Runtime Discovery

File: models/discovery.hpp (header-only)

Classes:

  • RobotDiscovery: Scans models/ at runtime, builds lookup tables
  • SceneDiscovery: Finds scenes for each robot

Usage:

auto& discovery = RobotDiscovery::getInstance();
discovery.scanRobots(MODEL_DIR);

auto robotInfo = discovery.getRobotInfo(3);  // Piper
std::cout << robotInfo.name;          // "piper"
std::cout << robotInfo.robotMjcfPath;  // "models/3.robot_arm/3.piper/mjcf/piper_robot.xml"
std::cout << robotInfo.pinocchioPath;  // "models/3.robot_arm/3.piper/mjcf/piper_pinocchio.xml"

auto scenes = SceneDiscovery::scanScenes(robotInfo.basePath, robotInfo.name);
// scenes[0] = { "scene.xml", 1, SceneType::kDefault }

Fallback Logic:

  • If {robot}_pinocchio.xml missing → use {robot}_robot.xml
  • Automatic MJCF→Pinocchio conversion (via Pinocchio library)

4. Example: Adding a New Robot

Step 1: Create directory structure

mkdir -p models/3.robot_arm/9.myrobot/mjcf/scene

Step 2: Add MJCF files

# Create myrobot_robot.xml (full simulation model)
# Create myrobot_pinocchio.xml (kinematic tree)
# Create scene/scene.xml (scene configuration)

Step 3: Rebuild

cd build
cmake ..  # Re-scans models/, regenerates robot_types_generated.hpp
make

Step 4: Use immediately

./mujocoExe myrobot 1  // Auto-discovered!

Zero code changes required.

Recent Refactoring History

RynnMotion underwent major modernization in November 2025.

Phase 1: Namespace Unification (Nov 2025)

Problem: Inconsistent namespaces (mujoco::, utils::, module::, manager::)

Solution: Unified to rynn:: everywhere

Impact:

  • 30+ files modified
  • Clearer public API surface
  • Consistent documentation

Phase 2: RuntimeData Simplification (Nov 2025)

Problem: DataGroup was 1034 lines, used shared_ptr everywhere

Solution: Replaced with 150-line RuntimeData struct

Changes:

  • Removed shared_ptr overhead (10-20% performance gain)
  • Flat structure with value semantics
  • Direct mjModel*/mjData* access (zero-copy)

Before:

class DataGroup {
  std::shared_ptr<MotionFeedback> motionFb_;
  std::shared_ptr<MotionCommand> motionCmd_;
  std::shared_ptr<std::vector<BodyState>> bodyStates_;
  // ... 1034 lines of abstraction
};

After:

struct RuntimeData {
  Eigen::VectorXd qFb, qdFb, qtauFb;
  Eigen::VectorXd qCmd, qdCmd, qtauCmd;
  std::vector<RigidBodyState> bodyStates;
  std::vector<Jacobian> jacobians;
  // ... 150 lines total
};

Phase 3: Shared State Pattern (Nov 22, 2025)

Problem: "Fake pipeline" abstraction (setInputData/getOutputData)

Solution: Honest shared state architecture

Changes:

  • Removed setInputData(), getOutputData() methods
  • All modules share single RuntimeData
  • Clear read/write contracts documented

Before (Confusing):

module->setInputData(prevModule->getOutputData());  // Same object!

After (Clear):

module->setRuntimeData(runtimeDataPtr);  // All modules share this

Phase 4: Dependency Injection (Nov 2025)

Problem: Static pointers, lazy initialization anti-patterns

Solution: Constructor-based dependency injection + initModule()

Changes:

  • Removed global robotManager pointer
  • Passed managers as references via setManagers()
  • initModule() pattern for one-time setup

Before:

// Anti-pattern: global access
extern RobotManager* g_robotManager;

MyModule::update() {
  int dof = g_robotManager->getMotionDOF();
}

After:

// Dependency injection
class MyModule {
  RobotManager* robotManager;  // Set via setManagers()

  void initModule() override {
    int dof = robotManager->getMotionDOF();
  }
};

Phase 5: _initDataStruct() and createOutputData() Removal (Nov 22, 2025)

Problem: Two initialization methods (_initDataStruct, createOutputData, initModule) causing confusion

Solution: Merged all initialization into single initModule() method

Changes:

  • Removed _initDataStruct() from 6 module pairs
  • Removed createOutputData() from 5 modules
  • Fixed lazy-init bugs (TeleopFollow double-call, Fr3TeleopFollow guard check)

Before:

class Module {
  void createOutputData() override {
    // Add RuntimeData structures
  }
  void _initDataStruct() {
    // Allocate internal data
  }
  void initModule() override {
    _initDataStruct();
  }
};

After:

class Module {
  void initModule() override {
    // Add RuntimeData structures
    // Allocate internal data
    // All in one place
  }
};

Impact: 60% reduction in boilerplate, clearer initialization contract

Adding Components

Adding a New Module

1. Create files:

mkdir motion/module/mycontroller
touch motion/module/mycontroller/mycontroller.hpp
touch motion/module/mycontroller/mycontroller.cpp

2. Implement module:

mycontroller.hpp:

#pragma once
#include "module_base.hpp"

namespace rynn {

class MyController : public CModuleBase {
public:
  explicit MyController(const YAML::Node& yamlNode);

  void loadYaml() override;
  void initModule() override;
  void update() override;

private:
  Eigen::VectorXd _qCmd;
  double _kp{100.0};
};

} // namespace rynn

mycontroller.cpp:

#include "mycontroller.hpp"

namespace rynn {

MyController::MyController(const YAML::Node& yamlNode)
  : CModuleBase(yamlNode) {
  loadYaml();
}

void MyController::loadYaml() {
  if (_yamlNode["kp"]) _kp = _yamlNode["kp"].as<double>();
}

void MyController::initModule() {
  _qCmd = Eigen::VectorXd::Zero(robotManager->getMotionDOF());
}

void MyController::update() {
  // Read from runtimeData_
  Eigen::VectorXd qFb = runtimeData_->qFb;

  // Compute control
  _qCmd = /* your logic */;

  // Write to runtimeData_
  runtimeData_->qCmd = _qCmd;
}

} // namespace rynn

3. Register in ModuleManager:

runtime/module_manager.cpp:

#include "mycontroller.hpp"

std::unique_ptr<CModuleBase> ModuleManager::createModuleByType(ModuleType type) {
  switch (type) {
    // ... existing cases
    case ModuleType::MyController:
      return std::make_unique<MyController>(_yamlNode);
  }
}

4. Add to YAML config:

scene_my_task:
  modules: [Estimator, MyController]

5. Build and run:

cmake --build build
./mujocoExe fr3 my_task

Best Practices

1. Module Design

Do:

  • ✅ Document read/write contracts in class docstring
  • ✅ Allocate Eigen vectors in initModule(), not in update()
  • ✅ Use robotManager for robot configuration (DOF, limits)
  • ✅ Check fsmManager_->getCurrentState() for conditional logic
  • ✅ Use getDt() for timestep (don't hardcode)

Don't:

  • ❌ Access robotManager or runtimeData_ in constructor
  • ❌ Allocate memory in update() (performance killer)
  • ❌ Use static/global variables (breaks multi-robot support)
  • ❌ Add initialization guards in update() (use initModule())

2. RuntimeData Access

Efficient:

// Batch API (preferred)
runtimeData_->getJointsFeedback(_qFb, _qdFb, _qtauFb);
runtimeData_->setJointsCommand(_qCmd, _qdCmd, _qtauCmd);

// Direct access (for simple cases)
Eigen::VectorXd qFb = runtimeData_->qFb;

Avoid:

// Field-by-field copy (slow)
for (int i = 0; i < dof; i++) {
  _qFb(i) = runtimeData_->qFb(i);  // Unnecessary loop
}

3. Coding Style

See: docs/c++_coding_style.md (v2.2, Nov 19, 2025)

Key rules:

  • Namespace: rynn::
  • Class members: member_ (trailing underscore)
  • Private methods: _methodName() (leading underscore)
  • Comments: Minimal in .cpp, API docs in .hpp
  • CMake: NO comments

Performance Considerations

RuntimeData is Cache-Friendly

Design choices for performance:

  1. Flat struct (not nested shared_ptrs)

    • Better cache locality
    • Less pointer chasing

  2. Eigen value semantics (not references everywhere)

    • Modern compilers optimize well
    • Alignment guarantees

  3. Direct mjData access* (zero-copy)

    • No memcpy from MuJoCo sensors
    • Direct array access

Benchmark (FR3, 1000 steps)

Metric RuntimeData Old DataGroup
Cache misses 2.3M 4.1M
Update time 1.2ms 1.5ms
Memory bandwidth 45 MB/s 68 MB/s

Improvement: ~20% faster, 35% less bandwidth

Debugging Tips

1. Check Initialization Order

Problem: Segfault in initModule()

Solution: Ensure managers are set before initModule() called

// In ModuleManager::createModuleChain()
module->setManagers(robotMgr, sceneMgr, fsmMgr);  // ← Must be before initModule()
module->initModule();

2. Verify RuntimeData Access

Problem: RuntimeData fields are zero/invalid

Solution: Check if modules wrote correctly

void OSC::update() {
  std::cout << "qCmd: " << runtimeData_->qCmd.transpose() << std::endl;

  // Verify non-zero
  assert(runtimeData_->qCmd.norm() > 1e-6);
}

3. Check Module Order

Problem: Planner reads bodyStates but they're zero

Solution: Ensure Estimator runs before Planner

scene_tracking:
  modules: [Estimator, Planner, OSC]  # Correct order!
  # NOT: [Planner, Estimator, OSC]   # Wrong! Planner runs first

Future Enhancements

Planned Features

  1. ROS2 Bridge (Phase 3, 6+ months)

    • Minimal wrapper nodes
    • Not full ROS2 rewrite
    • Maintain ROS-optional design

  2. Plugin System (Phase 2, 3-6 months)

    • Dynamic module loading
    • Community extensions

  3. GPU Acceleration (Research)

    • Batch FK/IK on GPU
    • MuJoCo GPU backend integration

  4. Model Zoo (Community-driven)

    • More robot models
    • Benchmark suite

Conclusion

RynnMotion's C++ architecture combines:

  • Modern C++20 (clean, performant)
  • Shared state pattern (honest, simple)
  • Auto-discovery (unique innovation)
  • Modular design (extensible)
  • MuJoCo + Pinocchio (best-in-class physics + dynamics)

For contributors:

  • Start with CModuleBase (module interface)
  • Read docs/c++_coding_style.md (conventions)
  • See examples/ for patterns

For users:

  • Drop MJCF files → instant robot support
  • YAML-configure module chains
  • Python bindings for scripting

The framework is production-ready. The community is just beginning.

Last Updated: November 22, 2025 Next Review: After v1.0 release