Skip to content

3D_Setup_Guide.md

Latest commit

 

History

History
556 lines (418 loc) · 21.2 KB

3D_Setup_Guide.md

File metadata and controls

556 lines (418 loc) · 21.2 KB
Raw

ME635 3D Warehouse — Setup & Manual-Fix Guide

Group 11 — ME 635 Modeling and Simulation, Spring 2026

This folder contains a hybrid 3D companion to the 2D MATLAB simulation Multirobot_Warehouse_Dynamic_2D.m. The 2D logic runs unchanged; its robot trajectories are captured and replayed in a Simscape Multibody world that visually mirrors the warehouse layout.

1. Files in this folder

File Stage Purpose
ME635_3D_Pose_Logger.m 1 Wraps the 2D simulation; logs poses + wheel angles + payload events to robot_trajectories.mat.
build_3D_warehouse_model.m 2 Programmatically builds ME635_3D_Warehouse.slx.
run_ME635_3D.m 3 One-button entry point: runs the logger, builds/loads the model, runs the simulation.
robot_trajectories.mat (output of Stage 1) Per-robot timeseries + map metadata.
ME635_3D_Warehouse.slx (output of Stage 2) The Simulink model.

2. Prerequisites

Required toolboxes (MATLAB R2025a or later recommended):

  • MATLAB
  • Simulink
  • Simscape
  • Simscape Multibody
  • Stateflow
  • Robotics System Toolbox
  • Navigation Toolbox

Optional (you said you have these — fine):

  • Robotics Playground (FX 67157)
  • Robotics Playground Expansion Pack (FX 80080)

The build script does not depend on the Playground toolboxes — it constructs robot bodies from Simscape Multibody primitives styled to look like TurtleBot3 Waffle Pi (1.5 m × 1.0 m × 0.4 m chassis, two side drive wheels, two casters, top "lidar" cap). If you later want to swap in the actual TurtleBot3 mesh from the Expansion Pack, see Section 6.

3. How to run (happy path)

In MATLAB, set the current folder to this directory and run:

run_ME635_3D

That's it. You'll see:

  1. The 2D simulation figure animating (with logging hooks running invisibly).
  2. After it finishes, robot_trajectories.mat is saved here.
  3. The Simulink model is built and opened.
  4. The simulation runs and Mechanics Explorer pops up with the 3D view.

You can also run each stage separately:

ME635_3D_Pose_Logger        % Stage 1 only
build_3D_warehouse_model    % Stage 2 only — needs .mat from Stage 1
sim('ME635_3D_Warehouse')   % Stage 3 only — needs .slx from Stage 2

4. What's in the .mat file

robot_trajectories.mat holds:

tsRobots     1×N cell, each is timeseries [x, y, theta]      — body pose
wheelTs      1×N cell, each is timeseries [phi_L, phi_R]    — wheel angles (rad)
payTs        1×N cell, each is timeseries scalar 0 or 1     — payload visible flag
NUM_R        scalar, number of robots
wheelR       0.15  (m)
trackW       1.0   (m)
BODY_LEN     1.5   (m)
BODY_WID     1.0   (m)
BODY_HGT     0.4   (m)
mapMeta      struct with shelf, station, barrier coordinates
robotColors  N×3 RGB matching the 2D neon palette

The build script reads this and pushes per-robot variables (pose_R1, wheels_R1, payload_R1, etc.) to the base workspace so that From Workspace blocks in the model can reference them by name.

5. Block-by-block layout of the Simulink model

Top-level (one-time foundation):

World Frame ─── Mechanism Configuration ─── Solver Configuration
   │
   ├──── Static World (Floor, 12 Shelves, Barrier, 5 Station markers)
   │
   ├──── Robot R1 (Subsystem)
   ├──── Robot R2 (Subsystem)
   ├──── Robot R3 (Subsystem)
   └──── Robot R4 (Subsystem)

Each robot subsystem:

PMC inport ─── Planar Joint (Px, Py, Rz) ─── Rigid Transform ─── Body (Brick Solid)
                  ▲   ▲    ▲                                       │
                q_x q_y q_th  (from FW_pose_R<n> via Demux)       ├── Drive wheel L (Revolute Joint, motion in)
                                                                   ├── Drive wheel R (Revolute Joint, motion in)
                                                                   ├── Caster F     (rigid sphere)
                                                                   ├── Caster B     (rigid sphere)
                                                                   ├── Lidar cap    (rigid cylinder, robot color)
                                                                   └── Prismatic-Z ─── Payload (Brick Solid)

External signal flow into each subsystem:

FW_pose_R<n>     ─── Demux (3) ──► q_x, q_y, q_th
FW_wheels_R<n>   ─── Demux (2) ──► q_phiL, q_phiR
FW_payload_R<n>  ─── Gain(5.4) ─── Bias(-5) ──► q_payZ

The Gain+Bias maps the 0/1 payload flag to z-position: p=1 → z=0.4 m (visible), p=0 → z=-5 m (hidden underground).

6. Common build-script failures and how to fix them

The build script is wrapped in extensive try/catch. When something fails, you'll see warnings like:

[BUILD warn] set_param Body.GeometryLengthCartesian = [1.5 1 0.4] failed: Invalid parameter

That tells you exactly which block parameter to fix. Below are the most likely culprits.

6a. Wrong library path for Simscape Multibody blocks

If the very first add_block call fails (World Frame), your library path differs from sm_lib/.... Check with:

sm_lib    % opens the library browser at the right place

If MATLAB tells you sm_lib doesn't exist, your install is older or named differently. Try mech_lib (legacy name pre-R2019a). Find/replace sm_lib → <your library> in build_3D_warehouse_model.m.

6b. Wrong parameter name on solid blocks

Block parameter names changed at various releases. The script uses these (R2024+ convention):

Block Parameter Value
Brick Solid GeometryLengthCartesian [lx ly lz]
Cylindrical Solid GeometryRadius, GeometryLength scalars
Solid (any) ColorVector [r g b]

If set_param warns on these, open the block in the GUI, look at the mask parameter names, and update the script. Right-click → Mask → Edit Mask shows internal names. Or use:

get_param('ME635_3D_Warehouse/Floor', 'ObjectParameters')

…to list valid parameter names for any specific block.

6c. "Cannot connect frame ports" / wrong child offsets

If wheels/casters/lidar/payload all stack at the body's center instead of branching out to their correct positions, the issue is the offset between the body and each child — not a missing reference frame on the brick.

R2025b note: the Edit Frame dialog on a Solid block does NOT have a numeric offset (XYZ translation) field anymore. The only options are "At Reference Frame Origin" and "Based on Geometric Feature". Older guides describing a Translation [x y z] text input are obsolete.

The fix is to put each offset into the Rigid Transform block already in the chain, not to bake new frames into the brick. The build script inserts RT_WL, RT_WR, RT_CF, RT_CB, RT_lidar, RT_PrismPay between the body and each child precisely for this purpose. See §11.3 for the offset values to enter.

This is the single most likely manual-fix step.

6d. Planar Joint motion-input port names

The script expects the joint to expose ports named q_Px, q_Py, q_Rz. If your release names them differently (e.g. Px, Py, Rz or numbered ports 1, 2, 3), the add_line calls will warn.

Fix: open R<n>/PlanarJ block, look at the actual port labels, and search/replace q_Px, q_Py, q_Rz, q_Pz accordingly in the script.

6e. From Workspace block "Variable not found"

If the simulation errors with Variable 'pose_R1' not found in base workspace:

  1. Re-run ME635_3D_Pose_Logger.m — it must complete and call assignin('base', ...) for these variables.
  2. Or load manually: 
    S = load('robot_trajectories.mat');
for ri = 1:S.NUM_R
    assignin('base', sprintf('pose_R%d', ri),    S.tsRobots{ri});
    assignin('base', sprintf('wheels_R%d', ri),  S.wheelTs{ri});
    assignin('base', sprintf('payload_R%d', ri), S.payTs{ri});
end

6f. Solver complaints about position-input differentiation

Driving a joint's position from a From Workspace timeseries means Simscape Multibody must compute velocity and acceleration internally. For dt = 0.2 s data this can cause solver chatter.

Fix (recommended): in each Planar Joint and Revolute Joint block, set:

  • Position Targets → Specify → "Provided by Input"
  • Velocity Targets → Specify → "Calculated by Differentiation"
  • Acceleration Targets → Specify → "Calculated by Differentiation"

If you still see jitter, increase the timeseries sample rate by linear interpolation in the logger:

% In ME635_3D_Pose_Logger.m, just before save():
fineT = (0:0.02:t(end))';                      % 50 Hz
tsRobots{ri_} = resample(tsRobots{ri_}, fineT);
wheelTs{ri_}  = resample(wheelTs{ri_},  fineT);
payTs{ri_}    = resample(payTs{ri_},    fineT);

6g. Mechanics Explorer view + camera

After the model opens, the camera defaults to a generic view. To get the isometric front-right preset:

  1. Open Mechanics Explorer (it opens automatically on first sim run).
  2. Right-click the 3D viewport → View Convention → "Y up".
  3. Left-click + drag to rotate to your preferred angle.
  4. Mechanics Explorer → File → Save Working State → save as ME635_3D_Warehouse_view.mat.
  5. The next time you open the model, Mechanics Explorer remembers it.

7. Swapping in the real TurtleBot3 Waffle Pi mesh (optional)

If you want the actual TurtleBot3 Waffle Pi from Robotics Playground Expansion Pack:

  1. In the Library Browser, find the TurtleBot3 Waffle Pi mechanism block (path varies by release; typically Robotics Playground Expansion Pack / Robots / TurtleBot3 / Waffle Pi).
  2. Drag a copy into each R<n> subsystem, replacing the primitive Body Brick Solid + wheels.
  3. Connect the TurtleBot3 block's frame port to your Planar Joint's follower frame.
  4. The TurtleBot3 block has its own internal wheel actuators — you'll need to either disable them (set wheel command inputs to 0) or, if you want the TurtleBot3's own wheels to spin from your wheel timeseries, route phi_L/phi_R to its wheel-velocity inputs after differentiating: omega_L = du/dt(phi_L).

The primitive build is recommended for the report demo because it gives you full control over chassis colour matching to the 2D palette (R1 yellow, R2 cyan, R3 magenta, R4 orange). The Expansion Pack TurtleBot3 has a baked-in white/grey shell.

8. Recording an MP4 of the 3D run

After running the simulation, record Mechanics Explorer:

smwritevideo('ME635_3D_Warehouse', 'ME635_3D_Warehouse.mp4', ...
    'FrameRate', 30, 'PlaybackSpeedRatio', 1);

If smwritevideo doesn't exist in your install, use the Mechanics Explorer GUI: Tools → Record video.

9. If you really get stuck

The pose logger (Stage 1) is the bulletproof part — it just runs the 2D sim with a few extra assignin calls. If robot_trajectories.mat is not produced, that's the only file you need to fix.

The build script (Stage 2) is the fragile part. If it cannot produce a working .slx, you can build the model manually:

  1. Create a blank Simulink model.
  2. Drop in: World Frame, Mechanism Configuration, Solver Configuration. Connect them.
  3. Drop one Brick Solid (50×50×0.05) connected via Rigid Transform to World, with translation [25 25 -0.025]. That's your floor.
  4. For each shelf in mapMeta.shelfOrigins: drop a Rigid Transform + Brick Solid, with translation = [sx + 1, sy + 2.5, 1.5] and geometry = [2, 5, 3].
  5. For each robot: build a Subsystem as described in Section 5.
  6. Add From Workspace blocks reading pose_R<n>, wheels_R<n>, payload_R<n>.

A reasonable minimum is one robot driven by pose_R1 — once that works, copy/paste it 3 more times and rename.

10. Memory & limits

  • 4 robots × ~120 sample/s for a typical 60–120 s run = a few thousand samples per robot. Negligible for the .mat file.
  • The 12 shelves × 4 robots × ~10 blocks each = ~150 blocks total in the model. Compiles in well under a minute on a typical laptop.
  • Mechanics Explorer rendering of 50×50 m world at 1920×1080 runs at ~30 FPS on integrated graphics; smoother with discrete GPU.

11. Manual completion checklist (after the build script runs)

The build script gets you ~80% of the way there. The remaining ~20% is joint motion-input enabling and reference-frame addition that has to be done in the Simulink GUI because the underlying mask logic only exposes extra ports/frames after a checkbox toggle. This section is a strict step-by-step recipe — follow it once for R1, then copy the pattern to R2, R3, R4.

The build script's last console block (Diagnostic — DialogParameters available on key blocks) tells you the exact mask parameter names your release uses — keep that output handy.

11.1 Verify the foundation

Open ME635_3D_Warehouse.slx. At top-left you should see:

  • World (World Frame)
  • MechCfg (Mechanism Configuration)
  • SolverCfg (Solver Configuration)

These three need to share one physical-modeling network. If your release auto-connected them, fine. If they're floating, draw three lines:

  1. From World right port to MechCfg left port.
  2. Branch from that line over to SolverCfg left port.

You can also delete MechCfg and SolverCfg and re-drag fresh ones from the library — Simulink usually auto-attaches them when dropped near World.

11.2 Verify the static world

The 12 shelves + barrier + 5 station markers should be present along the top of the diagram. If any shelf has wrong dimensions (e.g. it's a default 1×1×1 cube instead of 2×5×3 m):

  1. Double-click the offending Shelf_<n> block.
  2. Geometry tab → set the dimensions field (likely named Length, LengthCartesian, or Side in your release) to [2 5 3].
  3. Inertia tab → set Density (or rely on default ~1000 kg/m³).
  4. Graphics → Visual Properties → Color → set to the dark blue [0.12 0.26 0.50].

Same drill for Floor ([50 50 0.05]), Barrier ([4.4 1.2 0.6]), and ShelfC4_<n> ([1 5 3]).

For the station cylinder markers (Stn_Charging, etc.) — verify radius 1.5 m, height 0.05 m.

11.3 Set per-child offsets via Rigid Transform blocks (R2025b workflow)

Each robot's Body has six children that must branch off it at specific offsets: WL, WR, CF, CB, Lidar, PrismPay. The build script already inserts a Rigid Transform block in each chain — that's where the offset belongs in modern Simscape Multibody.

Why we don't add frames to the brick in R2025b: the "Edit Frame" dialog on a Brick Solid only offers "At Reference Frame Origin" and "Based on Geometric Feature". There is no numeric offset field anymore. So we leave the brick with its default R frame and put all offsets in the existing Rigid Transforms.

For each robot R1…R4, open each Rigid Transform inside the subsystem and set its translation:

Block Translation (m) Meaning
RT_WL [0, 0.55, 0] Left wheel mount
RT_WR [0, -0.55, 0] Right wheel mount
RT_CF [ 0.55, 0, -0.20] Front caster
RT_CB [-0.55, 0, -0.20] Back caster
RT_lidar [0, 0, 0.20] Lidar cap on top
RT_PrismPay [0, 0, 0.20] Payload anchor on top

In each Rigid Transform dialog:

  1. Double-click the block.
  2. Translation → Method = Cartesian.
  3. Translation → Offset = the vector from the table above (paste it as [0 0.55 0] etc., space-separated, units m).
  4. Rotation = None (default).
  5. OK / Apply.

The connection chain stays exactly as the script built it: Body/R → RT_<child>/B → ... → child block. No need to touch the brick's frame editor.

Tip: do this for R1 first, then right-click the entire R1 subsystem → Copy → Paste to replace R2/R3/R4 with renamed copies. The Rigid Transform offsets carry through the copy.

11.4 Enable joint motion inputs — R2025b (the critical step)

This is what exposes the px, py, qz, q, and pz signal-input triangles on the joints that the recorded poses drive.

Where the option lives in R2025b: the "Provided by Input" setting is in the Actuation → Motion dropdown, NOT in the State Targets section. State Targets only seeds the solver's initial guess at t=0 and does not drive the joint over time. Older guides describing a "Provided by Input" checkbox inside State Targets are obsolete — ignore that section (or leave it unchecked).

For each Planar Joint (R1/PlanarJ, R2/PlanarJ, R3/PlanarJ, R4/PlanarJ):

  1. Double-click the Planar Joint block.
  2. Expand X Prismatic Primitive (Px) in the parameter tree.
  3. Scroll to the Actuation group → click the Motion dropdown (default value: "Automatically Computed") → select "Provided by Input".
  4. Repeat for Y Prismatic Primitive (Py): Actuation → Motion → "Provided by Input".
  5. Repeat for Z Revolute Primitive (Rz): Actuation → Motion → "Provided by Input".
  6. Click OK / Apply. The block now shows three signal-input triangles on the left side, labelled px, py, qz (one per primitive).
  7. Wire the subsystem inports to those ports:
    • q_x → PlanarJ px
    • q_y → PlanarJ py
    • q_z → PlanarJ qz (this is the body rotation about Z)

For each Revolute Joint at the wheels (RJ_WL, RJ_WR × 4 robots):

  1. Double-click.
  2. Z Revolute Primitive (Rz) → Actuation → Motion → "Provided by Input".
  3. OK. The joint now has a q signal input port. Wire q_phiL → RJ_WL q; q_phiR → RJ_WR q.

For each Prismatic Joint at the payload (PrismPay × 4 robots):

  1. Double-click.
  2. Z Prismatic Primitive (Pz) → Actuation → Motion → "Provided by Input".
  3. OK. The joint now has a pz signal input port. Wire q_payZ → PrismPay pz.

Connecting signal lines: troubleshooting

If you drag from a subsystem inport (e.g. q_x) toward the joint's input port and Simulink leaves a dashed red line that won't commit:

  • Click the broken red segment, hit Delete — don't try to fix it by drawing more.
  • Drag from the source's output edge directly to the destination triangle. Release the mouse exactly on the triangle (Simulink highlights it green when you're hovered on it). Releasing on empty space — even close to it — leaves a stub.
  • If the autorouter keeps colliding with RT_CB or other blocks between the inports and the joint, hold Shift while dragging to force a straight segment, or temporarily move the obstructing block aside, draw the line, then move it back.
  • Verify you clicked OK / Apply on the joint dialog before drawing. The triangle ports don't accept connections until the Motion dropdown change is committed.

11.5 Velocity and acceleration: differentiation (R2025b is automatic)

In R2025b, when you set Actuation → Motion = "Provided by Input" with the default sub-options, the joint takes position only and the solver computes velocity and acceleration by differentiation under the hood. You don't need to do anything in this section in R2025b — this step is essentially a no-op.

If you ever see an "Underspecified motion at primitive Px" warning at simulation time, open the joint, look near the Motion dropdown for an "Input Handling" sub-option, and confirm it's set to "Position only" (or "Provide position; compute velocity and acceleration"). Don't re-touch State Targets — that section is unrelated to motion drive.

11.6 Run the simulation

  1. Click the green ▶ Run button (or sim('ME635_3D_Warehouse') in the command window).
  2. Mechanics Explorer pops up.
  3. Robots should immediately appear at their start poses (corners of the warehouse) and begin moving.

If the model errors with "Cannot resolve variable 'pose_R1' in base workspace": re-run ME635_3D_Pose_Logger.m (which calls assignin to push the pose timeseries to the base workspace) before running the 3D model.

11.7 Camera setup

In Mechanics Explorer:

  1. View menu → Convention → "Y up".
  2. Drag the 3D viewport with the mouse to rotate to a front-right isometric view.
  3. File → Save Working State → name it ME635_3D_Warehouse_view.mat. Loading the model next time restores the camera.

11.8 Estimated time for manual completion

  • Foundation verification (§11.1–§11.2): ~5 minutes.
  • One-robot reference frames + joint enables (§11.3–§11.5 for R1): ~15 minutes.
  • Copy-paste R2, R3, R4 + verify wheel offsets: ~10 minutes.
  • Camera + run + first error round-trip: ~10 minutes.

Realistic total: 30–45 minutes once you have the script-built model open. The bulk of the model topology — 50×50 floor, 12 shelves, 4 robots' chassis + wheels + casters + lidar + payload boxes, all the From Workspace + Demux signal wiring — that's done by the script.

11.9 If something is structurally wrong with a robot subsystem

Easiest fix: delete the broken subsystem entirely, copy a working one, rename, and update its FW_pose_R* / FW_wheels_R* / FW_payload_R* VariableName parameters to match the robot index. The rest of the top-level signal wiring is identical between robots.