|
| 1 | +# Joint Space Control with Quintic Trajectory Tracking |
| 2 | + |
| 3 | +This MATLAB simulation is a **batch simulation** that generates smooth trajectories for a Universal Robots UR10e robotic arm using quintic (5th-order) polynomials in joint space. The robot smoothly moves from a starting joint configuration to a target configuration, following an optimally timed trajectory with continuous position, velocity, and acceleration profiles. |
| 4 | + |
| 5 | + |
| 6 | + |
| 7 | + |
| 8 | + |
| 9 | +- **Quintic Polynomial Trajectory Generation**: Ensures smooth motion with continuous position, velocity, and acceleration |
| 10 | +- **Proportional Feedback Control**: Tracks the desired trajectory using a simple P-controller |
| 11 | +- **Velocity Feedforward**: The controller adds the desired joint velocity to the command to reduce lag and tracking error during motion |
| 12 | +- **Real-time Visualization**: Displays robot motion, joint trajectories, and tracking errors |
| 13 | +- **Configurable Parameters**: Customize start/goal poses, duration, and time step |
| 14 | + |
| 15 | +## Requirements |
| 16 | + |
| 17 | +- MATLAB R2019b or later |
| 18 | +- Robotics System Toolbox |
| 19 | +- Universal Robots UR10e model (included in Robotics System Toolbox) |
| 20 | + |
| 21 | +## Usage |
| 22 | + |
| 23 | +### Input Parameters |
| 24 | + |
| 25 | +| Parameter | Type | Description | Default Value | |
| 26 | +| --------- | ---------- | ------------------------------------------ | ------------------------- | |
| 27 | +| `qs` | 6×1 vector | **Start joint configuration** [rad or deg] | `[0; -π/2; π/2; 0; 0; 0]` | |
| 28 | +| `qg` | 6×1 vector | **Goal joint configuration** [rad or deg] | `[π/2; 0; π/2; 0; π; π]` | |
| 29 | +| `tf` | scalar | **Total trajectory duration** [s] | `3` | |
| 30 | +| `showGui` | boolean | **Enable visualization** (true/false) | `true` | |
| 31 | +| `ts` | scalar | **Simulation time step** [s] | `0.005` | |
| 32 | + |
| 33 | +**Notes:** |
| 34 | + |
| 35 | +- Joint angles can be provided in **radians** or **degrees** (auto-detected) |
| 36 | +- All input vectors are automatically converted to column format |
| 37 | +- If values exceed 2π, they are assumed to be in degrees and converted to radians |
| 38 | + |
| 39 | +### Running the Simulation |
| 40 | + |
| 41 | +Run this simulation using the specified API payload: |
| 42 | + |
| 43 | +```yaml |
| 44 | +simulation: |
| 45 | + request_id: abcdef12345 |
| 46 | + client_id: dt |
| 47 | + simulator: matlab |
| 48 | + type: batch |
| 49 | + file: simulation.m |
| 50 | + timestamp: "2024-01-01T00:00:00Z" |
| 51 | + timeout: 60 |
| 52 | + inputs: |
| 53 | + qs: [1.5708, -1.2000, 1.2000, 0.0000, 0.0010, 0.0000] |
| 54 | + qg: [-1.5708, -1.2500, 1.2500, 3.1000, -0.0010, -3.1000] |
| 55 | + tf: 5 |
| 56 | + showGui: true |
| 57 | + ts: 0.005 |
| 58 | + outputs: |
| 59 | + outputs: outputs |
| 60 | +``` |
| 61 | +
|
| 62 | +Update `use.yaml` to reference `simulation.yaml` as the payload file and configure the required RabbitMQ credentials. |
| 63 | + |
| 64 | +Then run the batch client: |
| 65 | + |
| 66 | +```bash |
| 67 | +python .\use_matlab_agent_batch.py |
| 68 | +``` |
| 69 | + |
| 70 | +### Output Structure |
| 71 | + |
| 72 | +The function returns a structure `outputs` containing: |
| 73 | + |
| 74 | +| Field | Dimension | Description | |
| 75 | +| ------------------- | --------- | ------------------------------------------------------------ | |
| 76 | +| `desired_positions` | 6×N | Desired joint positions sampled at 10× time step [rad] | |
| 77 | +| `actual_positions` | 6×N | Actual joint positions sampled at 10× time step [rad] | |
| 78 | +| `time` | 1×N | Time vector corresponding to sampled data [s] | |
| 79 | +| `velocities` | 6×M | Commanded joint velocities at each control step [rad/s] | |
| 80 | +| `tracking_error` | 1×M | Euclidean norm of tracking error at each time step [rad] | |
| 81 | +| `final_error` | scalar | Final tracking error between goal and reached position [rad] | |
| 82 | +| `final_position` | 6×1 | Final joint configuration reached by the robot [rad] | |
| 83 | + |
| 84 | +_N = number of sampled points (every 10th time step)_ |
| 85 | +_M = total number of simulation time steps_ |
| 86 | + |
| 87 | +## How It Works |
| 88 | + |
| 89 | +### 1. Trajectory Generation |
| 90 | + |
| 91 | +For each joint, a **quintic polynomial** is computed to smoothly interpolate between start and goal positions: |
| 92 | + |
| 93 | +``` |
| 94 | +q(t) = a₀ + a₁t + a₂t² + a₃t³ + a₄t⁴ + a₅t⁵ |
| 95 | +``` |
| 96 | + |
| 97 | +Boundary conditions enforce: |
| 98 | + |
| 99 | +- Zero velocity at start and end: `q̇(0) = 0`, `q̇(tf) = 0` |
| 100 | +- Zero acceleration at start and end: `q̈(0) = 0`, `q̈(tf) = 0` |
| 101 | + |
| 102 | +### 2. Control Law |
| 103 | + |
| 104 | +The controller uses **proportional feedback with velocity feedforward**: |
| 105 | + |
| 106 | +``` |
| 107 | +u = dqd + K · (qd - q) |
| 108 | +``` |
| 109 | + |
| 110 | +Where: |
| 111 | + |
| 112 | +- `qd` and `dqd` are the desired joint position and velocity from the quintic trajectory; |
| 113 | +- `q` is the current joint position; |
| 114 | +- `K = 100·I₆` is the proportional gain matrix; |
| 115 | +- `u` is the commanded joint velocity. |
| 116 | + |
| 117 | +**Why feedforward?** |
| 118 | +The proportional term `K(qd - q)` corrects the error, but introduces delay (lag) when the trajectory is moving. Adding the **feedforward** term `dqd` provides the control chain with the "right" velocity that the trajectory requires at that instant, reducing: |
| 119 | + |
| 120 | +- lag during the tracking phase, |
| 121 | +- transient tracking error, |
| 122 | +- error peaks during accelerations/slope changes. |
| 123 | + |
| 124 | +### 3. Integration |
| 125 | + |
| 126 | +Joint positions are updated using Euler integration: |
| 127 | + |
| 128 | +``` |
| 129 | +q(t+Δt) = q(t) + u·Δt |
| 130 | +``` |
| 131 | + |
| 132 | +With `u = dqd + K(qd - q)`, the `dqd` part anticipates the movement required by the trajectory, while `K(qd - q)` cancels the residual error. |
| 133 | +The simulation continues until the trajectory time is reached **and** the final error is below threshold (`1e-5` rad). |
| 134 | + |
| 135 | +## Visualization |
| 136 | + |
| 137 | +When `showGui = true`, the simulation generates four figures: |
| 138 | + |
| 139 | +1. **Start Pose**: Initial robot configuration |
| 140 | +2. **Goal Pose**: Target robot configuration |
| 141 | +3. **Joint Trajectories**: Position vs. time for all 6 joints with desired waypoints |
| 142 | +4. **Tracking Error**: Euclidean norm of error over time |
| 143 | +5. **Animated Motion**: Real-time 3D visualization of robot movement |
| 144 | + |
| 145 | +## Configuration |
| 146 | + |
| 147 | +### Adjusting Control Performance |
| 148 | + |
| 149 | +Modify the proportional gain in the code: |
| 150 | + |
| 151 | +```matlab |
| 152 | +K = 100 * eye(6); % Increase for faster tracking, decrease if oscillations occur |
| 153 | +``` |
| 154 | + |
| 155 | +### Changing Sampling Rate |
| 156 | + |
| 157 | +Output data is sampled at 10× the simulation time step. To modify: |
| 158 | + |
| 159 | +```matlab |
| 160 | +sampling_rate = 10; % Change to desired sampling factor |
| 161 | +``` |
| 162 | + |
| 163 | +## Mathematical Background |
| 164 | + |
| 165 | +The quintic polynomial ensures **jerk-limited motion**, which is important for: |
| 166 | + |
| 167 | +- Reducing mechanical wear on joints |
| 168 | +- Minimizing vibrations and oscillations |
| 169 | +- Ensuring smooth, natural-looking robot motion |
| 170 | + |
| 171 | +The polynomial coefficients are computed using the `polynomialfit` function with boundary conditions for position, velocity, and acceleration at both endpoints. |
| 172 | + |
| 173 | +## Troubleshooting |
| 174 | + |
| 175 | +| Issue | Solution | |
| 176 | +| ------------------------ | ------------------------------------------------------------------------- | |
| 177 | +| Robot doesn't reach goal | Increase `tf` or proportional gain `K` | |
| 178 | +| Oscillations in motion | Decrease proportional gain `K` | |
| 179 | +| Slow visualization | Reduce animation sampling (change `1:10:length(time_sim)` to larger step) | |
| 180 | +| Input dimension errors | Ensure `qs` and `qg` are 6-element vectors | |
0 commit comments