# rawadaltagiuri/mp_3d architecture

> Architecture diagram of the GitHub repository rawadaltagiuri/mp_3d, generated by GitDiagram from its file tree, README and a sample of source files (last updated 2026-07-01). It is an AI-made overview: check details against the source.

- Interactive diagram: https://gitdiagram.com/rawadaltagiuri/mp_3d
- Repository: https://github.com/rawadaltagiuri/mp_3d

## Overview

1. **What this repo is**
   - This is a MATLAB-based robot motion-planning toolkit, likely for a soft robot or articulated robot moving through obstacles.
   - The repository is centered on computing a feasible path, checking collisions, and visualizing the result.

2. **Main entry point**
   - `MotionPlannerSolution.m` is the top-level driver mentioned in the README.
   - It appears to orchestrate setup, planning, collision evaluation, and final output.

3. **Problem configuration layer**
   - `RobotConfig.m` likely defines the robot’s geometry, kinematic parameters, and planner settings.
   - `actionCounter.m` suggests the planner tracks expansion steps or limits the number of actions explored.

4. **Planning/search subsystem**
   - `searchAlgorithm.m` is the core search controller.
   - `FullExpand.m` and `greedyExpand.m` indicate two expansion modes or branching strategies.
   - This suggests the planner can explore the state space either exhaustively or with a more directed greedy strategy.

5. **Cost and heuristic evaluation**
   - `calculateCost.m`, `calculateCostBasedOnAlgorithm.m`, and `getHeuristic.m` form the evaluation layer used by the search.
   - These functions likely score candidate states/transitions and guide the planner toward the goal.
   - The presence of both generic and algorithm-specific cost functions implies the search strategy is pluggable.

6. **Kinematics and pose computation**
   - `solveForwardKinematics_3D.m` and `solveForwardKinematicsWithObstacles_3D.m` compute robot shape/pose in 3D.
   - `GetRodriguesRotation.m` points to rigid-body rotation math used inside the kinematic chain.
   - These functions are the bridge from abstract plan state to physical robot geometry.

7. **Obstacle and collision subsystem**
   - `collisionCheck.m` is the main feasibility gate.
   - `findNearbyObstacles.m`, `findJointHigherThanAllObstacles.m`, and `intersectObstacle_2D.m` suggest supporting geometry tests for obstacle proximity, clearance, and intersection.
   - This layer filters the search so only physically valid motions are expanded.

8. **Native performance boundary**
   - `PDQ.cpp`, `PDQ.mexw64`, and `PDQquickMode.h` form a compiled C++ MEX component called from MATLAB.
   - This is the only clear non-MATLAB subsystem and is likely used for performance-critical planning or geometry operations.
   - Architecturally, this is the main optimization boundary between high-level MATLAB orchestration and low-level execution speed.

9. **Visualization/output**
   - `softRobot_animation.m` is the presentation layer for the final motion plan.
   - It likely takes the planned path/robot states and renders the robot moving through the environment.

10. **Likely end-to-end data flow**
   - `MotionPlannerSolution.m` loads robot/environment parameters from `RobotConfig.m`, invokes `searchAlgorithm.m`, which expands candidate states via `FullExpand.m` or `greedyExpand.m`.
   - Candidate states are evaluated with `getHeuristic.m` and the cost functions, validated by `collisionCheck.m`, and grounded in geometry by the forward-kinematics routines.
   - The resulting plan is then passed to `softRobot_animation.m` for visualization, with the MEX module accelerating the heavy computation path.

11. **Architecture summary for a diagram**
   - The repo is best drawn as a single MATLAB application with three major layers: **planning**, **geometry/collision**, and **visualization**.
   - A small native C++ acceleration module sits alongside the MATLAB code and is called from the planning path when performance matters.

Main entry points: [`searchAlgorithm.m`](https://github.com/rawadaltagiuri/mp_3d/blob/main/searchAlgorithm.m) (Search Core), [`collisionCheck.m`](https://github.com/rawadaltagiuri/mp_3d/blob/main/collisionCheck.m) (Collision Check), [`MotionPlannerSolution.m`](https://github.com/rawadaltagiuri/mp_3d/blob/main/MotionPlannerSolution.m) (Motion Planner), [`PDQ.mexw64`](https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQ.mexw64) (PDQ Native)

## Diagram (Mermaid)

```mermaid
flowchart TD

subgraph group_group_entry["Entry"]
  node_node_motion["Motion Planner<br/>driver"]
  node_node_config["Robot Config<br/>params<br/>[RobotConfig.m]"]
end

subgraph group_group_planning["Planning"]
  node_node_actions(("Action Counter<br/>budget<br/>[actionCounter.m]"))
  node_node_search["Search Core<br/>[searchAlgorithm.m]"]
  node_node_full["Full Expand<br/>expander<br/>[FullExpand.m]"]
  node_node_greedy["Greedy Expand<br/>expander<br/>[greedyExpand.m]"]
  node_node_cost(("Cost Eval<br/>[calculateCost.m]"))
  node_node_cost_alg(("Alg Cost"))
  node_node_heuristic(("Heuristic<br/>estimate<br/>[getHeuristic.m]"))
end

subgraph group_group_geometry["Geometry"]
  node_node_fk["Forward Kinematics"]
  node_node_fk_obs["FK w/ Obstacles<br/>kinematics"]
  node_node_rot(("Rodrigues Rot<br/>math"))
  node_node_collision["Collision Check<br/>feasibility<br/>[collisionCheck.m]"]
  node_node_nearby(("Nearby Obstacles<br/>geometry"))
  node_node_joint(("Joint Clearance<br/>geometry"))
  node_node_intersect(("Obstacle Intersect<br/>geometry"))
end

subgraph group_group_native["Native"]
  node_node_pdq["PDQ Native<br/>mex accel<br/>[PDQ.mexw64]"]
  node_node_pdq_cpp["PDQ C++<br/>native source<br/>[PDQ.cpp]"]
  node_node_pdq_hdr["PDQ Header<br/>native api<br/>[PDQquickMode.h]"]
end

subgraph group_group_output["Output"]
  node_node_viz["Animation<br/>visualization"]
end

node_node_motion -->|"loads"| node_node_config
node_node_motion -->|"starts"| node_node_search
node_node_motion -->|"renders"| node_node_viz
node_node_search -->|"budgets"| node_node_actions
node_node_search -->|"expands"| node_node_full
node_node_search -->|"expands"| node_node_greedy
node_node_search -->|"scores"| node_node_cost
node_node_search -->|"scores"| node_node_cost_alg
node_node_search -->|"guides"| node_node_heuristic
node_node_search -->|"filters"| node_node_collision
node_node_search -->|"queries"| node_node_fk
node_node_search -.->|"accelerates"| node_node_pdq
node_node_fk -->|"uses"| node_node_rot
node_node_fk_obs -->|"extends"| node_node_fk
node_node_collision -->|"queries"| node_node_nearby
node_node_collision -->|"checks"| node_node_joint
node_node_collision -->|"tests"| node_node_intersect
node_node_collision -->|"depends on"| node_node_fk_obs
node_node_pdq -.->|"implements"| node_node_pdq_cpp
node_node_pdq -.->|"declares"| node_node_pdq_hdr

click node_node_motion "https://github.com/rawadaltagiuri/mp_3d/blob/main/MotionPlannerSolution.m"
click node_node_config "https://github.com/rawadaltagiuri/mp_3d/blob/main/RobotConfig.m"
click node_node_actions "https://github.com/rawadaltagiuri/mp_3d/blob/main/actionCounter.m"
click node_node_search "https://github.com/rawadaltagiuri/mp_3d/blob/main/searchAlgorithm.m"
click node_node_full "https://github.com/rawadaltagiuri/mp_3d/blob/main/FullExpand.m"
click node_node_greedy "https://github.com/rawadaltagiuri/mp_3d/blob/main/greedyExpand.m"
click node_node_cost "https://github.com/rawadaltagiuri/mp_3d/blob/main/calculateCost.m"
click node_node_cost_alg "https://github.com/rawadaltagiuri/mp_3d/blob/main/calculateCostBasedOnAlgorithm.m"
click node_node_heuristic "https://github.com/rawadaltagiuri/mp_3d/blob/main/getHeuristic.m"
click node_node_fk "https://github.com/rawadaltagiuri/mp_3d/blob/main/solveForwardKinematics_3D.m"
click node_node_fk_obs "https://github.com/rawadaltagiuri/mp_3d/blob/main/solveForwardKinematicsWithObstacles_3D.m"
click node_node_rot "https://github.com/rawadaltagiuri/mp_3d/blob/main/GetRodriguesRotation.m"
click node_node_collision "https://github.com/rawadaltagiuri/mp_3d/blob/main/collisionCheck.m"
click node_node_nearby "https://github.com/rawadaltagiuri/mp_3d/blob/main/findNearbyObstacles.m"
click node_node_joint "https://github.com/rawadaltagiuri/mp_3d/blob/main/findJointHigherThanAllObstacles.m"
click node_node_intersect "https://github.com/rawadaltagiuri/mp_3d/blob/main/intersectObstacle_2D.m"
click node_node_pdq "https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQ.mexw64"
click node_node_pdq_cpp "https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQ.cpp"
click node_node_pdq_hdr "https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQquickMode.h"
click node_node_viz "https://github.com/rawadaltagiuri/mp_3d/blob/main/softRobot_animation.m"

classDef toneNeutral fill:#f8fafc,stroke:#334155,stroke-width:1.5px,color:#0f172a
classDef toneBlue fill:#dbeafe,stroke:#2563eb,stroke-width:1.5px,color:#172554
classDef toneAmber fill:#fef3c7,stroke:#d97706,stroke-width:1.5px,color:#78350f
classDef toneMint fill:#dcfce7,stroke:#16a34a,stroke-width:1.5px,color:#14532d
classDef toneRose fill:#ffe4e6,stroke:#e11d48,stroke-width:1.5px,color:#881337
classDef toneIndigo fill:#e0e7ff,stroke:#4f46e5,stroke-width:1.5px,color:#312e81
classDef toneTeal fill:#ccfbf1,stroke:#0f766e,stroke-width:1.5px,color:#134e4a
class node_node_motion,node_node_config toneBlue
class node_node_actions,node_node_search,node_node_full,node_node_greedy,node_node_cost,node_node_cost_alg,node_node_heuristic toneAmber
class node_node_fk,node_node_fk_obs,node_node_rot,node_node_collision,node_node_nearby,node_node_joint,node_node_intersect toneMint
class node_node_pdq,node_node_pdq_cpp,node_node_pdq_hdr toneRose
class node_node_viz toneIndigo
```

## Components

### Entry

Top-level driver and configuration

- [Motion Planner](https://github.com/rawadaltagiuri/mp_3d/blob/main/MotionPlannerSolution.m): driver · Top-level orchestration of planning and output · `MotionPlannerSolution.m`
- [Robot Config](https://github.com/rawadaltagiuri/mp_3d/blob/main/RobotConfig.m): params · Robot and planner setup parameters · `RobotConfig.m`

### Planning

Search, expansion, cost, and heuristic evaluation

- [Action Counter](https://github.com/rawadaltagiuri/mp_3d/blob/main/actionCounter.m): budget · Tracks expansion or search limits · `actionCounter.m`
- [Search Core](https://github.com/rawadaltagiuri/mp_3d/blob/main/searchAlgorithm.m): search · Main search controller · `searchAlgorithm.m`
- [Full Expand](https://github.com/rawadaltagiuri/mp_3d/blob/main/FullExpand.m): expander · Broad or exhaustive state expansion · `FullExpand.m`
- [Greedy Expand](https://github.com/rawadaltagiuri/mp_3d/blob/main/greedyExpand.m): expander · Directed state expansion strategy · `greedyExpand.m`
- [Cost Eval](https://github.com/rawadaltagiuri/mp_3d/blob/main/calculateCost.m): cost · Generic transition and path scoring · `calculateCost.m`
- [Alg Cost](https://github.com/rawadaltagiuri/mp_3d/blob/main/calculateCostBasedOnAlgorithm.m): cost · Algorithm-specific cost adjustment · `calculateCostBasedOnAlgorithm.m`
- [Heuristic](https://github.com/rawadaltagiuri/mp_3d/blob/main/getHeuristic.m): estimate · Goal-directed search estimate · `getHeuristic.m`

### Geometry

Kinematics and collision feasibility checks

- [Forward Kinematics](https://github.com/rawadaltagiuri/mp_3d/blob/main/solveForwardKinematics_3D.m): kinematics · Computes robot pose and shape in 3D · `solveForwardKinematics_3D.m`
- [FK w/ Obstacles](https://github.com/rawadaltagiuri/mp_3d/blob/main/solveForwardKinematicsWithObstacles_3D.m): kinematics · Forward kinematics variant aware of obstacles · `solveForwardKinematicsWithObstacles_3D.m`
- [Rodrigues Rot](https://github.com/rawadaltagiuri/mp_3d/blob/main/GetRodriguesRotation.m): math · Rigid-body rotation helper · `GetRodriguesRotation.m`
- [Collision Check](https://github.com/rawadaltagiuri/mp_3d/blob/main/collisionCheck.m): feasibility · Main feasibility gate for candidate motions · `collisionCheck.m`
- [Nearby Obstacles](https://github.com/rawadaltagiuri/mp_3d/blob/main/findNearbyObstacles.m): geometry · Finds relevant obstacles for a state · `findNearbyObstacles.m`
- [Joint Clearance](https://github.com/rawadaltagiuri/mp_3d/blob/main/findJointHigherThanAllObstacles.m): geometry · Checks joint height against obstacles · `findJointHigherThanAllObstacles.m`
- [Obstacle Intersect](https://github.com/rawadaltagiuri/mp_3d/blob/main/intersectObstacle_2D.m): geometry · 2D intersection test against obstacles · `intersectObstacle_2D.m`

### Native

Compiled performance boundary

- [PDQ Native](https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQ.mexw64): mex accel · Compiled C++ boundary for performance-critical work · `PDQ.mexw64`
- [PDQ C++](https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQ.cpp): native source · Native implementation behind the MEX binary · `PDQ.cpp`
- [PDQ Header](https://github.com/rawadaltagiuri/mp_3d/blob/main/PDQquickMode.h): native api · Native interface definitions · `PDQquickMode.h`

### Output

Visualization of the planned motion

- [Animation](https://github.com/rawadaltagiuri/mp_3d/blob/main/softRobot_animation.m): visualization · Renders the final robot motion through space · `softRobot_animation.m`

## Connections

- Motion Planner → Robot Config: loads · Reads robot and planner settings
- Motion Planner → Search Core: starts · Invokes the search pipeline
- Motion Planner → Animation: renders · Sends the resulting plan to animation
- Search Core → Action Counter: budgets · Tracks or limits search expansions
- Search Core → Full Expand: expands · Uses full expansion strategy
- Search Core → Greedy Expand: expands · Uses greedy expansion strategy
- Search Core → Cost Eval: scores · Evaluates candidate states and transitions
- Search Core → Alg Cost: scores · Applies algorithm-specific scoring
- Search Core → Heuristic: guides · Estimates distance to goal
- Search Core → Collision Check: filters · Rejects infeasible candidate motions
- Search Core → Forward Kinematics: queries · Uses kinematics to evaluate candidate geometry
- Search Core → PDQ Native: accelerates · Calls into compiled code for heavy work
- Forward Kinematics → Rodrigues Rot: uses · Builds pose using rotation math
- FK w/ Obstacles → Forward Kinematics: extends · Reuses core forward kinematics with obstacle context
- Collision Check → Nearby Obstacles: queries · Finds relevant obstacles to test
- Collision Check → Joint Clearance: checks · Validates clearance constraints
- Collision Check → Obstacle Intersect: tests · Performs intersection checks
- Collision Check → FK w/ Obstacles: depends on · Uses obstacle-aware kinematic states
- PDQ Native → PDQ C++: implements · Binary built from C++ source
- PDQ Native → PDQ Header: declares · Uses native header interface
