How rawadaltagiuri/mp_3d works
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 Search Core · collisionCheck.m Collision Check · MotionPlannerSolution.m Motion Planner · PDQ.mexw64 PDQ Native
Components 20
Entry
Top-level driver and configuration
- Motion Planner · Top-level orchestration of planning and output
MotionPlannerSolution.m - Robot Config · Robot and planner setup parameters
RobotConfig.m
Planning
Search, expansion, cost, and heuristic evaluation
- Action Counter · Tracks expansion or search limits
actionCounter.m - Search Core · Main search controller
searchAlgorithm.m - Full Expand · Broad or exhaustive state expansion
FullExpand.m - Greedy Expand · Directed state expansion strategy
greedyExpand.m - Cost Eval · Generic transition and path scoring
calculateCost.m - Alg Cost · Algorithm-specific cost adjustment
calculateCostBasedOnAlgorithm.m - Heuristic · Goal-directed search estimate
getHeuristic.m
Geometry
Kinematics and collision feasibility checks
- Forward Kinematics · Computes robot pose and shape in 3D
solveForwardKinematics_3D.m - FK w/ Obstacles · Forward kinematics variant aware of obstacles
solveForwardKinematicsWithObstacles_3D.m - Rodrigues Rot · Rigid-body rotation helper
GetRodriguesRotation.m - Collision Check · Main feasibility gate for candidate motions
collisionCheck.m - Nearby Obstacles · Finds relevant obstacles for a state
findNearbyObstacles.m - Joint Clearance · Checks joint height against obstacles
findJointHigherThanAllObstacles.m - Obstacle Intersect · 2D intersection test against obstacles
intersectObstacle_2D.m
Native
Compiled performance boundary
- PDQ Native · Compiled C++ boundary for performance-critical work
PDQ.mexw64 - PDQ C++ · Native implementation behind the MEX binary
PDQ.cpp - PDQ Header · Native interface definitions
PDQquickMode.h
Output
Visualization of the planned motion
- Animation · Renders the final robot motion through space
softRobot_animation.m
Connections 20
- 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