mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-26 06:53:42 +08:00
DevTekVE
2197 lines
113 KiB
Plaintext
2197 lines
113 KiB
Plaintext
{
|
||
"cells": [
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "228a6736-de31-4255-9d72-a6ff391b968d",
|
||
"metadata": {
|
||
"jupyter": {
|
||
"is_executing": true
|
||
}
|
||
},
|
||
"outputs": [],
|
||
"source": [
|
||
"from opendbc.car import structs\n",
|
||
"from opendbc.car.hyundai.values import CAR, HyundaiFlags\n",
|
||
"from opendbc.car.hyundai.fingerprints import FW_VERSIONS\n",
|
||
"\n",
|
||
"TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) & set(CAR.with_flags(HyundaiFlags.CANFD_ANGLE_STEERING)) # CAN-FD electric vehicles only\n",
|
||
"#TEST_PLATFORMS = set(CAR.with_flags(HyundaiFlags.CANFD)) - set(CAR.with_flags(HyundaiFlags.EV)) # CAN-FD hybrid and ICE vehicles only\n",
|
||
"\n",
|
||
"print(f\"Found {len(TEST_PLATFORMS)} qualifying vehicles:\")\n",
|
||
"for platform in TEST_PLATFORMS:\n",
|
||
" print(f\" {platform}\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "ed1c8aec-c274-4c61-b83d-711ea194bf86",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"TEST_SEGMENTS = ['e1107f9d04dfb1e2/00000096--02ecca61a6']\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "0c75e8f2-4f5f-4f89-b8db-5223a6534a9f",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"import copy\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"import numpy as np\n",
|
||
"from opendbc.can.parser import CANParser\n",
|
||
"from opendbc.car.hyundai.values import DBC\n",
|
||
"from opendbc.car.hyundai.hyundaicanfd import CanBus\n",
|
||
"from openpilot.selfdrive.pandad import can_capnp_to_list\n",
|
||
"from openpilot.tools.lib.logreader import LogReader\n",
|
||
"\n",
|
||
"# Keep both messages we're interested in\n",
|
||
"message_names = [\"LKAS_ALT\", \"LFA_ALT\"]\n",
|
||
"\n",
|
||
"# You need to define this variable if it's not already defined\n",
|
||
"# TEST_SEGMENTS = [\"path/to/segment1\", \"path/to/segment2\"]\n",
|
||
"# And platform variable if not defined\n",
|
||
"# platform = \"CANFD\"\n",
|
||
"\n",
|
||
"# Select one segment for testing/debugging\n",
|
||
"segment = TEST_SEGMENTS[0] # Change index as needed"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "7724dd97-f62e-4fd3-9f64-63d49be669d2",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Load the segment\n",
|
||
"lr = LogReader(segment)\n",
|
||
"CP = lr.first(\"carParams\")\n",
|
||
"if CP is None:\n",
|
||
" print(f\"No carParams found in segment {segment}\")\n",
|
||
"else:\n",
|
||
" print(f\"Analyzing segment {segment} for {CP.carFingerprint}\")\n",
|
||
"\n",
|
||
" # Get CAN messages\n",
|
||
" can_msgs = [msg for msg in lr if msg.which() == \"can\"]\n",
|
||
" print(f\"Found {len(can_msgs)} CAN messages\")\n",
|
||
" \n",
|
||
" # Setup parser\n",
|
||
" parser_messages = []\n",
|
||
" for name in message_names:\n",
|
||
" parser_messages.append((name, 0))\n",
|
||
" \n",
|
||
" try:\n",
|
||
" cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n",
|
||
" print(\"Parser initialized successfully\")\n",
|
||
" except Exception as e:\n",
|
||
" print(f\"Error initializing parser: {e}\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "b9e0f5dd-4609-4fb2-8b36-28359256f6f6",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"parser_messages = [(\"LKAS_ALT\",0),(\"LFA_ALT\",0)]\n",
|
||
"# Reset parser\n",
|
||
"cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n",
|
||
"\n",
|
||
"# Check a single message\n",
|
||
"example_idx = 0\n",
|
||
"a = set()\n",
|
||
"while example_idx < len(can_msgs):\n",
|
||
" try:\n",
|
||
" cp.update_strings(can_capnp_to_list([can_msgs[example_idx].as_builder().to_bytes()]))\n",
|
||
" \n",
|
||
" #Print all available signals in both messages\n",
|
||
" #print(\"Available signals:\")\n",
|
||
" #if \"LKAS_ALT\" in cpacan.vl:\n",
|
||
" # print(\" LKAS_ALT:\", cpacan.vl[\"LKAS_ALT\"])\n",
|
||
" #if \"LFA_ALT\" in cp.vl:\n",
|
||
" # print(\" LFA_ALT:\", cp.vl[\"LFA_ALT\"])\n",
|
||
" \n",
|
||
" # Found at least one message with data\n",
|
||
" #if \"LKAS_ALT\" in cp.vl or \"LFA_ALT\" in cp.vl:\n",
|
||
" # break\n",
|
||
" #if cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_MAX_TORQUE\"]>0:\n",
|
||
" # a.add(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_MAX_TORQUE\"])\n",
|
||
" #print(\"AAAA\")\n",
|
||
" #break\n",
|
||
" if cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"]>0:\n",
|
||
" a.add(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"])\n",
|
||
" #print(\"AAAA\")\n",
|
||
" #break\n",
|
||
" \n",
|
||
" except Exception as e:\n",
|
||
" print(f\"Error examining message {example_idx}: {e}\")\n",
|
||
" \n",
|
||
" example_idx += 1\n",
|
||
" \n",
|
||
"\n",
|
||
"print(example_idx) # 72009\n",
|
||
"print(a)"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "87b5699c-10fa-456a-9dca-138eaace34e3",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Reset parser\n",
|
||
"cp = CANParser(DBC[platform][\"pt\"], parser_messages, CanBus(CP).ECAN)\n",
|
||
"\n",
|
||
"# Track timestamps and angle commands when active\n",
|
||
"timestamps = []\n",
|
||
"angle_cmds = []\n",
|
||
"active_flags = [] # For debugging\n",
|
||
"msg_indices = [] # Store original message indices for reference\n",
|
||
"\n",
|
||
"# Process all CAN messages\n",
|
||
"for i, msg in enumerate(can_msgs):\n",
|
||
" try:\n",
|
||
" cp.update_strings(can_capnp_to_list([msg.as_builder().to_bytes()]))\n",
|
||
" \n",
|
||
" # Check if both required messages and signals are present\n",
|
||
" if \"LFA_ALT\" in cp.vl and \"LKAS_ALT\" in cp.vl:\n",
|
||
" if \"LKAS_ANGLE_ACTIVE\" in cp.vl[\"LFA_ALT\"] and \"LKAS_ANGLE_CMD\" in cp.vl[\"LKAS_ALT\"]:\n",
|
||
" active_val = cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_ACTIVE\"]\n",
|
||
" active_flags.append(active_val)\n",
|
||
" \n",
|
||
" # Only collect data when active is 2\n",
|
||
" if active_val == 2:\n",
|
||
" timestamps.append(msg.logMonoTime / 1e9) # Convert to seconds\n",
|
||
" angle_cmds.append(cp.vl[\"LFA_ALT\"][\"LKAS_ANGLE_CMD\"])\n",
|
||
" msg_indices.append(i)\n",
|
||
" except Exception as e:\n",
|
||
" if i % 1000 == 0: # Only print errors occasionally to avoid flooding\n",
|
||
" print(f\"Error processing message {i}: {e}\")\n",
|
||
"\n",
|
||
"print(f\"Total messages processed: {len(can_msgs)}\")\n",
|
||
"print(f\"LKAS_ANGLE_ACTIVE values encountered: {set(active_flags)}\")\n",
|
||
"print(f\"Active steering points collected: {len(timestamps)}\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "f6194a2e-6bc9-49c8-befc-7ca930c07f29",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Find continuous active periods\n",
|
||
"active_periods = []\n",
|
||
"if msg_indices:\n",
|
||
" # Start with the first active message\n",
|
||
" current_period = [0, 0]\n",
|
||
" \n",
|
||
" for i in range(1, len(msg_indices)):\n",
|
||
" # Check if consecutive in the original message sequence\n",
|
||
" if msg_indices[i] == msg_indices[i-1] + 1:\n",
|
||
" # Part of the same period\n",
|
||
" current_period[1] = i\n",
|
||
" else:\n",
|
||
" # Gap in sequence, start new period\n",
|
||
" active_periods.append(current_period)\n",
|
||
" current_period = [i, i]\n",
|
||
" \n",
|
||
" # Add the last period\n",
|
||
" active_periods.append(current_period)\n",
|
||
"\n",
|
||
"print(f\"Found {len(active_periods)} active steering periods\")\n",
|
||
"for i, (start, end) in enumerate(active_periods):\n",
|
||
" duration = timestamps[end] - timestamps[start]\n",
|
||
" num_points = end - start + 1\n",
|
||
" print(f\" Period {i+1}: {num_points} points over {duration:.2f}s\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "2f5d4ca7-315e-417e-9e61-f490324e6a53",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"## Choose a period to analyze (if any exist)\n",
|
||
"if active_periods:\n",
|
||
" # Find the period with the most points\n",
|
||
" #longest_period_idx = max(range(len(active_periods)), \n",
|
||
" # key=lambda i: active_periods[i][1] - active_periods[i][0])\n",
|
||
" longest_period_idx = 1\n",
|
||
" \n",
|
||
" start_idx, end_idx = active_periods[longest_period_idx]\n",
|
||
" \n",
|
||
" # Get data for this period\n",
|
||
" period_timestamps = timestamps[start_idx:end_idx+1]\n",
|
||
" period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n",
|
||
" \n",
|
||
" # Normalize timestamps to start at 0\n",
|
||
" norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n",
|
||
" \n",
|
||
" # Calculate rate of change\n",
|
||
" angle_cmd_diffs = np.diff(period_angle_cmds)\n",
|
||
" \n",
|
||
" # Print statistics\n",
|
||
" print(f\"Analysis of Period {longest_period_idx+1}:\")\n",
|
||
" print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n",
|
||
" print(f\" Points: {len(period_angle_cmds)}\")\n",
|
||
" print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n",
|
||
" print(f\" Max absolute rate of change: {max(abs(angle_cmd_diffs)):.2f} units/frame\")\n",
|
||
" \n",
|
||
" # Plot the angle command evolution\n",
|
||
" plt.figure(figsize=(12, 8))\n",
|
||
" \n",
|
||
" # Plot angle commands\n",
|
||
" plt.subplot(2, 1, 1)\n",
|
||
" plt.plot(norm_timestamps, period_angle_cmds)\n",
|
||
" plt.title(f\"LKAS_ANGLE_CMD Evolution - Period {longest_period_idx+1}\")\n",
|
||
" plt.xlabel(\"Time (s)\")\n",
|
||
" plt.ylabel(\"LKAS_ANGLE_CMD\")\n",
|
||
" plt.grid(True)\n",
|
||
" \n",
|
||
" # Plot rate of change\n",
|
||
" plt.subplot(2, 1, 2)\n",
|
||
" plt.plot(norm_timestamps[1:], angle_cmd_diffs)\n",
|
||
" plt.title(\"Rate of Change\")\n",
|
||
" plt.xlabel(\"Time (s)\")\n",
|
||
" plt.ylabel(\"Delta LKAS_ANGLE_CMD per frame\")\n",
|
||
" plt.grid(True)\n",
|
||
" \n",
|
||
" plt.tight_layout()\n",
|
||
" plt.show()\n",
|
||
"else:\n",
|
||
" print(\"No active periods found to analyze\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "33c30ed5-e8aa-44e8-9e5c-dc993d83c299",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Analyze all active periods\n",
|
||
"if active_periods:\n",
|
||
" # Initialize variables to track overall statistics\n",
|
||
" all_max_rates = []\n",
|
||
" \n",
|
||
" # Create a figure with subplots for all periods\n",
|
||
" num_periods = len(active_periods)\n",
|
||
" fig, axes = plt.subplots(num_periods, 2, figsize=(14, 5*num_periods))\n",
|
||
" \n",
|
||
" # If there's only one period, make axes indexable as a 2D array\n",
|
||
" if num_periods == 1:\n",
|
||
" axes = np.array([axes])\n",
|
||
" \n",
|
||
" # Process each active period\n",
|
||
" for i, (start_idx, end_idx) in enumerate(active_periods):\n",
|
||
" # Get data for this period\n",
|
||
" period_timestamps = timestamps[start_idx:end_idx+1]\n",
|
||
" period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n",
|
||
" \n",
|
||
" # Normalize timestamps to start at 0\n",
|
||
" norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n",
|
||
" \n",
|
||
" # Calculate rate of change\n",
|
||
" angle_cmd_diffs = np.diff(period_angle_cmds)\n",
|
||
" max_rate = max(abs(angle_cmd_diffs))\n",
|
||
" all_max_rates.append(max_rate)\n",
|
||
" \n",
|
||
" # Print statistics for this period\n",
|
||
" print(f\"Analysis of Period {i+1}:\")\n",
|
||
" print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n",
|
||
" print(f\" Points: {len(period_angle_cmds)}\")\n",
|
||
" print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n",
|
||
" print(f\" Max absolute rate of change: {max_rate:.2f} units/frame\")\n",
|
||
" print()\n",
|
||
" \n",
|
||
" # Plot angle commands\n",
|
||
" axes[i, 0].plot(norm_timestamps, period_angle_cmds)\n",
|
||
" axes[i, 0].set_title(f\"LKAS_ANGLE_CMD Evolution - Period {i+1}\")\n",
|
||
" axes[i, 0].set_xlabel(\"Time (s)\")\n",
|
||
" axes[i, 0].set_ylabel(\"LKAS_ANGLE_CMD\")\n",
|
||
" axes[i, 0].grid(True)\n",
|
||
" \n",
|
||
" # Plot rate of change\n",
|
||
" axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs)\n",
|
||
" axes[i, 1].set_title(f\"Rate of Change - Period {i+1}\")\n",
|
||
" axes[i, 1].set_xlabel(\"Time (s)\")\n",
|
||
" axes[i, 1].set_ylabel(\"Delta LKAS_ANGLE_CMD per frame\")\n",
|
||
" axes[i, 1].grid(True)\n",
|
||
" \n",
|
||
" # Print average maximum rate of change\n",
|
||
" avg_max_rate = sum(all_max_rates) / len(all_max_rates)\n",
|
||
" print(f\"Average maximum rate of change across all periods: {avg_max_rate:.2f} units/frame\")\n",
|
||
" \n",
|
||
" # Add a summary section with all periods on one plot\n",
|
||
" plt.figure(figsize=(14, 8))\n",
|
||
" \n",
|
||
" # Plot all periods with different colors\n",
|
||
" for i, (start_idx, end_idx) in enumerate(active_periods):\n",
|
||
" period_timestamps = timestamps[start_idx:end_idx+1]\n",
|
||
" period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n",
|
||
" norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n",
|
||
" \n",
|
||
" plt.plot(norm_timestamps, period_angle_cmds, label=f\"Period {i+1}\")\n",
|
||
" \n",
|
||
" plt.title(\"LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)\")\n",
|
||
" plt.xlabel(\"Time (s)\")\n",
|
||
" plt.ylabel(\"LKAS_ANGLE_CMD\")\n",
|
||
" plt.grid(True)\n",
|
||
" plt.legend()\n",
|
||
" \n",
|
||
" plt.tight_layout()\n",
|
||
" plt.show()\n",
|
||
" \n",
|
||
" # Also display the original figure with individual period plots\n",
|
||
" fig.tight_layout()\n",
|
||
" plt.show()\n",
|
||
"else:\n",
|
||
" print(\"No active periods found to analyze\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "877702bf-0c3c-43ca-8900-c57b3c9237bc",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# Analyze all active periods with enhanced metrics\n",
|
||
"if active_periods:\n",
|
||
" # Initialize variables to track overall statistics\n",
|
||
" all_max_rates_per_frame = []\n",
|
||
" all_max_rates_per_second = []\n",
|
||
" all_min_to_max_times = []\n",
|
||
" all_min_to_max_rates = []\n",
|
||
" \n",
|
||
" # Create a figure with subplots for all periods\n",
|
||
" num_periods = len(active_periods)\n",
|
||
" fig, axes = plt.subplots(num_periods, 3, figsize=(18, 6*num_periods))\n",
|
||
" \n",
|
||
" # If there's only one period, make axes indexable as a 2D array\n",
|
||
" if num_periods == 1:\n",
|
||
" axes = np.array([axes]).reshape(1, 3)\n",
|
||
" \n",
|
||
" # Process each active period\n",
|
||
" for i, (start_idx, end_idx) in enumerate(active_periods):\n",
|
||
" # Get data for this period\n",
|
||
" period_timestamps = timestamps[start_idx:end_idx+1]\n",
|
||
" period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n",
|
||
" \n",
|
||
" # Normalize timestamps to start at 0\n",
|
||
" norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n",
|
||
" \n",
|
||
" # Calculate rate of change per frame\n",
|
||
" angle_cmd_diffs = np.diff(period_angle_cmds)\n",
|
||
" max_rate_per_frame = max(abs(angle_cmd_diffs))\n",
|
||
" all_max_rates_per_frame.append(max_rate_per_frame)\n",
|
||
" \n",
|
||
" # Calculate rate of change per second\n",
|
||
" time_diffs = np.diff(period_timestamps)\n",
|
||
" rates_per_second = [diff/t_diff if t_diff > 0 else 0 for diff, t_diff in zip(angle_cmd_diffs, time_diffs)]\n",
|
||
" max_rate_per_second = max(abs(rate) for rate in rates_per_second)\n",
|
||
" all_max_rates_per_second.append(max_rate_per_second)\n",
|
||
" \n",
|
||
" # Find local minima and maxima\n",
|
||
" from scipy.signal import find_peaks\n",
|
||
" \n",
|
||
" # Find local maxima\n",
|
||
" max_peaks, _ = find_peaks(period_angle_cmds)\n",
|
||
" # Find local minima (by finding maxima of negative values)\n",
|
||
" min_peaks, _ = find_peaks([-x for x in period_angle_cmds])\n",
|
||
" \n",
|
||
" min_to_max_times = []\n",
|
||
" min_to_max_rates = []\n",
|
||
" \n",
|
||
" # For each minimum, find the next maximum and calculate metrics\n",
|
||
" for min_idx in min_peaks:\n",
|
||
" # Find the next maximum after this minimum\n",
|
||
" next_max_indices = [idx for idx in max_peaks if idx > min_idx]\n",
|
||
" if next_max_indices: # If there's a maximum after this minimum\n",
|
||
" next_max_idx = next_max_indices[0]\n",
|
||
" \n",
|
||
" # Time from min to max\n",
|
||
" time_diff = period_timestamps[next_max_idx] - period_timestamps[min_idx]\n",
|
||
" \n",
|
||
" # Change in angle\n",
|
||
" angle_diff = period_angle_cmds[next_max_idx] - period_angle_cmds[min_idx]\n",
|
||
" \n",
|
||
" # Rate of change\n",
|
||
" rate = angle_diff / time_diff if time_diff > 0 else 0\n",
|
||
" \n",
|
||
" min_to_max_times.append(time_diff)\n",
|
||
" min_to_max_rates.append(rate)\n",
|
||
" \n",
|
||
" if min_to_max_times: # If we found min-to-max transitions\n",
|
||
" avg_min_to_max_time = sum(min_to_max_times) / len(min_to_max_times)\n",
|
||
" avg_min_to_max_rate = sum(min_to_max_rates) / len(min_to_max_rates)\n",
|
||
" max_min_to_max_rate = max(abs(rate) for rate in min_to_max_rates)\n",
|
||
" \n",
|
||
" all_min_to_max_times.extend(min_to_max_times)\n",
|
||
" all_min_to_max_rates.extend(min_to_max_rates)\n",
|
||
" else:\n",
|
||
" avg_min_to_max_time = 0\n",
|
||
" avg_min_to_max_rate = 0\n",
|
||
" max_min_to_max_rate = 0\n",
|
||
" \n",
|
||
" # Print statistics for this period\n",
|
||
" print(f\"Analysis of Period {i+1}:\")\n",
|
||
" print(f\" Duration: {period_timestamps[-1] - period_timestamps[0]:.2f}s\")\n",
|
||
" print(f\" Points: {len(period_angle_cmds)}\")\n",
|
||
" print(f\" Angle range: {min(period_angle_cmds):.2f} to {max(period_angle_cmds):.2f}\")\n",
|
||
" print(f\" Max absolute rate of change: {max_rate_per_frame:.2f} degrees/frame\")\n",
|
||
" print(f\" Max absolute rate of change: {max_rate_per_second:.2f} degrees/second\")\n",
|
||
" if min_to_max_times:\n",
|
||
" print(f\" Avg time from min to max: {avg_min_to_max_time:.2f}s\")\n",
|
||
" print(f\" Avg rate from min to max: {avg_min_to_max_rate:.2f} degrees/second\")\n",
|
||
" print(f\" Max rate from min to max: {max_min_to_max_rate:.2f} degrees/second\")\n",
|
||
" print()\n",
|
||
" \n",
|
||
" # Plot angle commands\n",
|
||
" axes[i, 0].plot(norm_timestamps, period_angle_cmds)\n",
|
||
" # Add markers for mins and maxs\n",
|
||
" for min_idx in min_peaks:\n",
|
||
" axes[i, 0].plot(norm_timestamps[min_idx], period_angle_cmds[min_idx], 'rv', markersize=8)\n",
|
||
" for max_idx in max_peaks:\n",
|
||
" axes[i, 0].plot(norm_timestamps[max_idx], period_angle_cmds[max_idx], 'g^', markersize=8)\n",
|
||
" \n",
|
||
" axes[i, 0].set_title(f\"LKAS_ANGLE_CMD Evolution - Period {i+1}\")\n",
|
||
" axes[i, 0].set_xlabel(\"Time (s)\")\n",
|
||
" axes[i, 0].set_ylabel(\"LKAS_ANGLE_CMD (degrees)\")\n",
|
||
" axes[i, 0].grid(True)\n",
|
||
" \n",
|
||
" # Plot rate of change per frame\n",
|
||
" axes[i, 1].plot(norm_timestamps[1:], angle_cmd_diffs)\n",
|
||
" axes[i, 1].set_title(f\"Rate of Change - Period {i+1}\")\n",
|
||
" axes[i, 1].set_xlabel(\"Time (s)\")\n",
|
||
" axes[i, 1].set_ylabel(\"Delta (degrees/frame)\")\n",
|
||
" axes[i, 1].grid(True)\n",
|
||
" \n",
|
||
" # Plot rate of change per second\n",
|
||
" axes[i, 2].plot([norm_timestamps[j] for j in range(1, len(norm_timestamps))], rates_per_second)\n",
|
||
" axes[i, 2].set_title(f\"Rate of Change - Period {i+1}\")\n",
|
||
" axes[i, 2].set_xlabel(\"Time (s)\")\n",
|
||
" axes[i, 2].set_ylabel(\"Delta (degrees/second)\")\n",
|
||
" axes[i, 2].grid(True)\n",
|
||
" \n",
|
||
" # Calculate overall statistics\n",
|
||
" avg_max_rate_per_frame = sum(all_max_rates_per_frame) / len(all_max_rates_per_frame)\n",
|
||
" avg_max_rate_per_second = sum(all_max_rates_per_second) / len(all_max_rates_per_second)\n",
|
||
" \n",
|
||
" print(\"\\nOverall Statistics:\")\n",
|
||
" print(f\"Average maximum rate of change: {avg_max_rate_per_frame:.2f} degrees/frame\")\n",
|
||
" print(f\"Average maximum rate of change: {avg_max_rate_per_second:.2f} degrees/second\")\n",
|
||
" \n",
|
||
" if all_min_to_max_times:\n",
|
||
" avg_min_to_max_time = sum(all_min_to_max_times) / len(all_min_to_max_times)\n",
|
||
" avg_min_to_max_rate = sum(all_min_to_max_rates) / len(all_min_to_max_rates)\n",
|
||
" max_min_to_max_rate = max(abs(rate) for rate in all_min_to_max_rates)\n",
|
||
" \n",
|
||
" print(f\"Average time from min to max: {avg_min_to_max_time:.2f}s\")\n",
|
||
" print(f\"Average rate from min to max: {avg_min_to_max_rate:.2f} degrees/second\")\n",
|
||
" print(f\"Maximum rate from min to max: {max_min_to_max_rate:.2f} degrees/second\")\n",
|
||
" \n",
|
||
" # Add a summary section with all periods on one plot\n",
|
||
" plt.figure(figsize=(14, 8))\n",
|
||
" \n",
|
||
" # Plot all periods with different colors\n",
|
||
" for i, (start_idx, end_idx) in enumerate(active_periods):\n",
|
||
" period_timestamps = timestamps[start_idx:end_idx+1]\n",
|
||
" period_angle_cmds = angle_cmds[start_idx:end_idx+1]\n",
|
||
" norm_timestamps = [t - period_timestamps[0] for t in period_timestamps]\n",
|
||
" \n",
|
||
" plt.plot(norm_timestamps, period_angle_cmds, label=f\"Period {i+1}\")\n",
|
||
" \n",
|
||
" plt.title(\"LKAS_ANGLE_CMD Evolution - All Periods (Time Normalized)\")\n",
|
||
" plt.xlabel(\"Time (s)\")\n",
|
||
" plt.ylabel(\"LKAS_ANGLE_CMD (degrees)\")\n",
|
||
" plt.grid(True)\n",
|
||
" plt.legend()\n",
|
||
" \n",
|
||
" # Create a histogram of min-to-max times\n",
|
||
" if all_min_to_max_times:\n",
|
||
" plt.figure(figsize=(10, 6))\n",
|
||
" plt.hist(all_min_to_max_times, bins=15)\n",
|
||
" plt.title(\"Histogram of Min-to-Max Transition Times\")\n",
|
||
" plt.xlabel(\"Time (s)\")\n",
|
||
" plt.ylabel(\"Frequency\")\n",
|
||
" plt.grid(True)\n",
|
||
" \n",
|
||
" # Create a histogram of min-to-max rates\n",
|
||
" plt.figure(figsize=(10, 6))\n",
|
||
" plt.hist(all_min_to_max_rates, bins=15)\n",
|
||
" plt.title(\"Histogram of Min-to-Max Transition Rates\")\n",
|
||
" plt.xlabel(\"Rate (degrees/second)\")\n",
|
||
" plt.ylabel(\"Frequency\")\n",
|
||
" plt.grid(True)\n",
|
||
" \n",
|
||
" plt.tight_layout()\n",
|
||
" plt.show()\n",
|
||
" \n",
|
||
" # Also display the original figure with individual period plots\n",
|
||
" fig.tight_layout()\n",
|
||
" plt.show()\n",
|
||
"else:\n",
|
||
" print(\"No active periods found to analyze\")"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "e201e356-5110-4603-9287-246fc7629ea9",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# torque_ramping.py\n",
|
||
"\n",
|
||
"import math\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"import matplotlib.ticker as ticker # For formatting the secondary x-axis\n",
|
||
"\n",
|
||
"# --- Ramping Functions (ema_torque_ramp, sigmoid_smootherstep_torque_ramp, linear_torque_ramp) ---\n",
|
||
"# These functions remain unchanged from the previous version provided in the document.\n",
|
||
"# For brevity, I'll represent them as comments here, but they are the same as in the document.\n",
|
||
"# def ema_torque_ramp(...): ...\n",
|
||
"# def sigmoid_smootherstep_torque_ramp(...): ...\n",
|
||
"# def linear_torque_ramp(...): ...\n",
|
||
"\n",
|
||
"def ema_torque_ramp(initial_torque: float, target_torque: float, alpha: float, completion_threshold: float = 0.999) -> list[float]:\n",
|
||
" \"\"\"\n",
|
||
" Calculates a torque ramp using Exponential Moving Average (EMA).\n",
|
||
" (Implementation as previously provided)\n",
|
||
" \"\"\"\n",
|
||
" if not (0 < alpha <= 1):\n",
|
||
" raise ValueError(\"Alpha must be between 0 (exclusive) and 1 (inclusive).\")\n",
|
||
" if not (0 < completion_threshold < 1):\n",
|
||
" raise ValueError(\"Completion threshold must be between 0 and 1 (exclusive).\")\n",
|
||
"\n",
|
||
" torque_values = [initial_torque]\n",
|
||
" current_ema_torque = initial_torque\n",
|
||
" total_change = target_torque - initial_torque\n",
|
||
" max_steps = 10000 \n",
|
||
"\n",
|
||
" if abs(total_change) < 1e-6:\n",
|
||
" return torque_values\n",
|
||
"\n",
|
||
" for _ in range(max_steps):\n",
|
||
" current_ema_torque = (target_torque * alpha) + (current_ema_torque * (1 - alpha))\n",
|
||
" torque_values.append(current_ema_torque)\n",
|
||
" current_progress = abs(current_ema_torque - initial_torque)\n",
|
||
" required_progress = abs(total_change) * completion_threshold\n",
|
||
" if current_progress >= required_progress:\n",
|
||
" break\n",
|
||
" if abs(current_ema_torque - target_torque) < 1e-6:\n",
|
||
" if len(torque_values) > 1 or initial_torque == target_torque:\n",
|
||
" break\n",
|
||
" \n",
|
||
" if abs(torque_values[-1] - target_torque) > 1e-6 :\n",
|
||
" threshold_met = (total_change > 0 and torque_values[-1] >= initial_torque + total_change * completion_threshold) or \\\n",
|
||
" (total_change < 0 and torque_values[-1] <= initial_torque + total_change * completion_threshold) or \\\n",
|
||
" (abs(current_ema_torque - target_torque) < 1e-5) # Check against the true EMA value\n",
|
||
" if threshold_met:\n",
|
||
" torque_values[-1] = target_torque\n",
|
||
" return torque_values\n",
|
||
"\n",
|
||
"def sigmoid_smootherstep_torque_ramp(initial_torque: float, target_torque: float, duration_steps: int) -> list[float]:\n",
|
||
" \"\"\"\n",
|
||
" Calculates a torque ramp using an S-curve (Smootherstep) profile.\n",
|
||
" s = 6x⁵ - 15x⁴ + 10x³\n",
|
||
" (Implementation as previously provided)\n",
|
||
" \"\"\"\n",
|
||
" if duration_steps < 0: \n",
|
||
" raise ValueError(\"Duration steps must be non-negative.\")\n",
|
||
" if duration_steps == 0:\n",
|
||
" return [initial_torque, target_torque] if initial_torque != target_torque else [initial_torque]\n",
|
||
"\n",
|
||
" torque_values = []\n",
|
||
" total_torque_change = target_torque - initial_torque\n",
|
||
" for i in range(duration_steps + 1):\n",
|
||
" x = i / duration_steps\n",
|
||
" if x == 0: s = 0.0\n",
|
||
" elif x == 1: s = 1.0\n",
|
||
" else:\n",
|
||
" x_3 = x * x * x\n",
|
||
" x_4 = x_3 * x\n",
|
||
" x_5 = x_4 * x\n",
|
||
" s = 6 * x_5 - 15 * x_4 + 10 * x_3\n",
|
||
" current_torque = initial_torque + total_torque_change * s\n",
|
||
" torque_values.append(current_torque)\n",
|
||
" return torque_values\n",
|
||
"\n",
|
||
"def linear_torque_ramp(initial_torque: float, target_torque: float, step_per_cycle: float = 1.0) -> list[float]:\n",
|
||
" \"\"\"\n",
|
||
" Calculates a torque ramp using a fixed linear step per cycle.\n",
|
||
" (Implementation as previously provided)\n",
|
||
" \"\"\"\n",
|
||
" if step_per_cycle <= 0:\n",
|
||
" raise ValueError(\"step_per_cycle must be positive.\")\n",
|
||
" torque_values = [initial_torque]\n",
|
||
" current_torque = initial_torque\n",
|
||
" if abs(initial_torque - target_torque) < 1e-6:\n",
|
||
" return torque_values\n",
|
||
"\n",
|
||
" actual_step = step_per_cycle if target_torque > initial_torque else -step_per_cycle\n",
|
||
" # Adjusted max_iterations to handle very small steps relative to total_change\n",
|
||
" if abs(actual_step) < 1e-9 : # Avoid division by zero or extremely small step\n",
|
||
" max_iterations = 20000 # A large fallback\n",
|
||
" else:\n",
|
||
" max_iterations = int(abs(target_torque - initial_torque) / abs(actual_step)) + 10 \n",
|
||
"\n",
|
||
"\n",
|
||
" for _ in range(max_iterations):\n",
|
||
" if (actual_step > 0 and current_torque >= target_torque) or \\\n",
|
||
" (actual_step < 0 and current_torque <= target_torque):\n",
|
||
" # Ensure the target is precisely hit if we just reached or passed it\n",
|
||
" if torque_values[-1] != target_torque:\n",
|
||
" # This logic can be complex due to floating point.\n",
|
||
" # The break condition itself should handle it, then the post-loop ensures target.\n",
|
||
" pass\n",
|
||
" break \n",
|
||
" \n",
|
||
" next_torque = current_torque + actual_step\n",
|
||
" if (actual_step > 0 and next_torque >= target_torque) or \\\n",
|
||
" (actual_step < 0 and next_torque <= target_torque):\n",
|
||
" current_torque = target_torque\n",
|
||
" torque_values.append(current_torque)\n",
|
||
" break\n",
|
||
" current_torque = next_torque\n",
|
||
" torque_values.append(current_torque)\n",
|
||
"\n",
|
||
" # Ensure the final value is exactly the target if the ramp intended to complete.\n",
|
||
" if abs(current_torque - target_torque) < 1e-6 : # If current_torque is essentially target\n",
|
||
" if not torque_values or torque_values[-1] != target_torque:\n",
|
||
" if torque_values and abs(torque_values[-1] - target_torque) < abs(actual_step) * 1.1 : # If last value is close\n",
|
||
" torque_values[-1] = target_torque\n",
|
||
" else: # If list is empty or last value is far, append target\n",
|
||
" torque_values.append(target_torque)\n",
|
||
" elif (actual_step > 0 and current_torque > target_torque) or \\\n",
|
||
" (actual_step < 0 and current_torque < target_torque) : # If overshot\n",
|
||
" if not torque_values or torque_values[-1] != target_torque:\n",
|
||
" torque_values[-1] = target_torque # Correct the overshoot to target\n",
|
||
"\n",
|
||
" # Remove duplicates at the end if any were formed by the logic\n",
|
||
" if len(torque_values) > 1 and torque_values[-1] == torque_values[-2]:\n",
|
||
" torque_values.pop()\n",
|
||
" \n",
|
||
" # If only initial torque is present and it's not the target, add target.\n",
|
||
" if len(torque_values) == 1 and initial_torque != target_torque:\n",
|
||
" torque_values.append(target_torque)\n",
|
||
"\n",
|
||
" return torque_values\n",
|
||
"\n",
|
||
"\n",
|
||
"def plot_ramps(ramp_data: list[dict], title: str, hz: float = 100.0):\n",
|
||
" \"\"\"\n",
|
||
" Plots multiple torque ramps on the same graph, with a secondary x-axis for time in seconds.\n",
|
||
"\n",
|
||
" Args:\n",
|
||
" ramp_data: A list of dictionaries, where each dictionary contains:\n",
|
||
" 'label': The label for the ramp (e.g., \"EMA Alpha 0.1\").\n",
|
||
" 'values': The list of torque values for the ramp.\n",
|
||
" 'style': (Optional) matplotlib line style (e.g., '--')\n",
|
||
" 'marker': (Optional) matplotlib marker style (e.g., '.')\n",
|
||
" title: The title for the plot.\n",
|
||
" hz: The frequency of the control loop in Hertz (for time calculation).\n",
|
||
" \"\"\"\n",
|
||
" fig, ax1 = plt.subplots(figsize=(14, 8)) \n",
|
||
" \n",
|
||
" max_steps = 0\n",
|
||
" for ramp_info in ramp_data:\n",
|
||
" steps_values = range(len(ramp_info['values']))\n",
|
||
" if len(steps_values) > max_steps:\n",
|
||
" max_steps = len(steps_values)\n",
|
||
" ax1.plot(steps_values, ramp_info['values'], \n",
|
||
" label=ramp_info['label'], \n",
|
||
" linestyle=ramp_info.get('style', '-'), \n",
|
||
" marker=ramp_info.get('marker', None),\n",
|
||
" markersize=ramp_info.get('markersize', 4)) \n",
|
||
"\n",
|
||
" ax1.set_title(title, fontsize=16)\n",
|
||
" ax1.set_xlabel(f\"Steps (Control Cycles @ {hz}Hz)\", fontsize=12)\n",
|
||
" ax1.set_ylabel(\"Torque\", fontsize=12)\n",
|
||
" ax1.legend(loc='best') \n",
|
||
" ax1.grid(True, which='both', linestyle='--', linewidth=0.5) \n",
|
||
"\n",
|
||
" ax2 = ax1.twiny() \n",
|
||
"\n",
|
||
" ax1_xlim = ax1.get_xlim()\n",
|
||
" # Ensure xlim[0] and xlim[1] are not identical before division, can happen if max_steps is 0 or 1\n",
|
||
" if ax1_xlim[1] > ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(ax1_xlim[0] / hz, ax1_xlim[1] / hz)\n",
|
||
" else: # Fallback if limits are problematic or hz is zero\n",
|
||
" ax2.set_xlim(ax1_xlim[0], ax1_xlim[1])\n",
|
||
"\n",
|
||
"\n",
|
||
" ax2.set_xlabel(\"Time (seconds)\", fontsize=12)\n",
|
||
" \n",
|
||
" # Format the ticks on the secondary x-axis to show 4 decimal places\n",
|
||
" ax2.xaxis.set_major_formatter(ticker.FormatStrFormatter('%.4f')) # Changed from %.2f\n",
|
||
"\n",
|
||
" if max_steps > 0 :\n",
|
||
" # Ensure xlim is at least 1 step wide for proper display if max_steps is 1 (len is 2)\n",
|
||
" current_xlim_max = max_steps -1 if max_steps > 1 else 1 # steps are 0-indexed\n",
|
||
" padding = int(current_xlim_max*0.05) if current_xlim_max > 20 else 1\n",
|
||
" ax1.set_xlim(0, current_xlim_max + padding ) \n",
|
||
" else: # Handle case with no steps or single point data\n",
|
||
" ax1.set_xlim(0,1)\n",
|
||
"\n",
|
||
" # Update secondary axis limits again after primary axis might have changed\n",
|
||
" if ax1.get_xlim()[1] > ax1.get_xlim()[0] and hz > 0:\n",
|
||
" ax2.set_xlim(ax1.get_xlim()[0] / hz, ax1.get_xlim()[1] / hz)\n",
|
||
" else:\n",
|
||
" ax2.set_xlim(ax1.get_xlim()[0], ax1.get_xlim()[1])\n",
|
||
"\n",
|
||
"\n",
|
||
" fig.tight_layout() \n",
|
||
" plt.show()\n",
|
||
"\n",
|
||
"# --- Example Usage ---\n",
|
||
"if __name__ == \"__main__\":\n",
|
||
" controller_hz = 100.0 \n",
|
||
"\n",
|
||
" initial_tq_up = 50.0\n",
|
||
" target_tq_up = 200.0\n",
|
||
" initial_tq_down = 200.0\n",
|
||
" target_tq_down = 25.0\n",
|
||
" linear_step_size = 1.0 \n",
|
||
"\n",
|
||
" alphas = {\n",
|
||
" \"Slow (α=0.05)\": 0.05, \n",
|
||
" \"Medium (α=0.2)\": 0.2, \n",
|
||
" \"Fast (α=0.5)\": 0.5\n",
|
||
" }\n",
|
||
" \n",
|
||
" durations = {\n",
|
||
" \"Short (10 steps)\": 10,\n",
|
||
" \"Medium (20 steps)\": 20,\n",
|
||
" \"Long (40 steps)\": 40,\n",
|
||
" \"Longer (60 steps)\": 60,\n",
|
||
" \"Very Long (100 steps)\": 100 \n",
|
||
" }\n",
|
||
"\n",
|
||
" # Ramp Up Data Collection\n",
|
||
" ramp_up_plot_data = []\n",
|
||
" print(f\"\\n--- Generating Ramp Up Data ({initial_tq_up} to {target_tq_up}) ---\")\n",
|
||
"\n",
|
||
" linear_ramp_up = linear_torque_ramp(initial_tq_up, target_tq_up, linear_step_size)\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Linear (+{linear_step_size}/step)\", \"values\": linear_ramp_up, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" # Number of steps is len(ramp_values) - 1 (since ramp_values includes the initial point)\n",
|
||
" print(f\"Linear Ramp Up: {max(0, len(linear_ramp_up)-1)} steps. First 5: {[round(t,1) for t in linear_ramp_up[:5]]}..., Last 5: {[round(t,1) for t in linear_ramp_up[-5:]]}\")\n",
|
||
"\n",
|
||
" for label_suffix, alpha_val in alphas.items():\n",
|
||
" ramp = ema_torque_ramp(initial_tq_up, target_tq_up, alpha_val)\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": ramp})\n",
|
||
" print(f\"EMA {label_suffix}: {max(0, len(ramp)-1)} steps. First 5: {[round(t,1) for t in ramp[:5]]}..., Last 5: {[round(t,1) for t in ramp[-5:]]}\")\n",
|
||
"\n",
|
||
" for label_suffix, dur_val in durations.items():\n",
|
||
" ramp = sigmoid_smootherstep_torque_ramp(initial_tq_up, target_tq_up, dur_val)\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": ramp, \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix}: {max(0, len(ramp)-1)} steps. First 5: {[round(t,1) for t in ramp[:5]]}..., Last 5: {[round(t,1) for t in ramp[-5:]]}\")\n",
|
||
" \n",
|
||
" # Ramp Down Data Collection\n",
|
||
" ramp_down_plot_data = []\n",
|
||
" print(f\"\\n--- Generating Ramp Down Data ({initial_tq_down} to {target_tq_down}) ---\")\n",
|
||
"\n",
|
||
" linear_ramp_down = linear_torque_ramp(initial_tq_down, target_tq_down, linear_step_size)\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Linear (-{linear_step_size}/step)\", \"values\": linear_ramp_down, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Down: {max(0, len(linear_ramp_down)-1)} steps. First 5: {[round(t,1) for t in linear_ramp_down[:5]]}..., Last 5: {[round(t,1) for t in linear_ramp_down[-5:]]}\")\n",
|
||
"\n",
|
||
" for label_suffix, alpha_val in alphas.items():\n",
|
||
" ramp = ema_torque_ramp(initial_tq_down, target_tq_down, alpha_val)\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": ramp})\n",
|
||
" print(f\"EMA {label_suffix}: {max(0, len(ramp)-1)} steps. First 5: {[round(t,1) for t in ramp[:5]]}..., Last 5: {[round(t,1) for t in ramp[-5:]]}\")\n",
|
||
"\n",
|
||
" for label_suffix, dur_val in durations.items():\n",
|
||
" ramp = sigmoid_smootherstep_torque_ramp(initial_tq_down, target_tq_down, dur_val)\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": ramp, \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix}: {max(0, len(ramp)-1)} steps. First 5: {[round(t,1) for t in ramp[:5]]}..., Last 5: {[round(t,1) for t in ramp[-5:]]}\")\n",
|
||
"\n",
|
||
" # Plotting\n",
|
||
" plot_ramps(ramp_up_plot_data, f\"Torque Ramp Up: {initial_tq_up} to {target_tq_up}\", hz=controller_hz)\n",
|
||
" plot_ramps(ramp_down_plot_data, f\"Torque Ramp Down: {initial_tq_down} to {target_tq_down}\", hz=controller_hz)\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "27f78aad-d201-4a63-979a-4644d5f76bb0",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# torque_ramping.py\n",
|
||
"\n",
|
||
"import math\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"import matplotlib.ticker as ticker # For formatting the secondary x-axis\n",
|
||
"\n",
|
||
"class EmaRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages EMA torque ramping, calculating one step at a time.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, alpha: float, \n",
|
||
" completion_threshold: float = 0.999): # Completion threshold is for internal 'done' state\n",
|
||
" if not (0 < alpha <= 1):\n",
|
||
" raise ValueError(\"Alpha must be between 0 (exclusive) and 1 (inclusive).\")\n",
|
||
" \n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.alpha = alpha\n",
|
||
" \n",
|
||
" # Internal state to know if the ramp considers itself \"done\"\n",
|
||
" # This doesn't stop step() from being called, but stops further changes.\n",
|
||
" self._is_effectively_done = False \n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self._is_effectively_done = True\n",
|
||
"\n",
|
||
" # For monitoring progress if needed, not strictly used by step() to halt\n",
|
||
" self._initial_torque_for_threshold = initial_torque\n",
|
||
" self._total_change_for_threshold = target_torque - initial_torque\n",
|
||
" self._completion_threshold = completion_threshold\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_effectively_done:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" # Calculate next EMA value\n",
|
||
" next_torque = (self.target_torque * self.alpha) + (self.current_torque * (1 - self.alpha))\n",
|
||
" self.current_torque = next_torque\n",
|
||
" \n",
|
||
" # Check if the ramp is now effectively complete\n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self.current_torque = self.target_torque # Snap to target\n",
|
||
" self._is_effectively_done = True\n",
|
||
" elif abs(self._total_change_for_threshold) > 1e-7: # Check threshold if there's change\n",
|
||
" current_progress_fraction = abs(self.current_torque - self._initial_torque_for_threshold) / abs(self._total_change_for_threshold)\n",
|
||
" if current_progress_fraction >= self._completion_threshold:\n",
|
||
" # If threshold met, snap to target for subsequent calls if not already there\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_effectively_done = True\n",
|
||
" \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class SigmoidRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Sigmoid (Smootherstep) torque ramping, calculating one step at a time.\n",
|
||
" Formula: s = 6x⁵ - 15x⁴ + 10x³\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, duration_steps: int):\n",
|
||
" if duration_steps < 0: \n",
|
||
" raise ValueError(\"Duration steps must be non-negative.\")\n",
|
||
" \n",
|
||
" self.initial_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.duration_steps = duration_steps # This is the number of steps to reach the target\n",
|
||
" self.total_torque_change = target_torque - initial_torque\n",
|
||
" \n",
|
||
" self.current_simulation_step = 0 # Tracks how many times step() has been called effectively\n",
|
||
" self.current_torque = initial_torque\n",
|
||
"\n",
|
||
" if self.duration_steps == 0: # If duration is 0, it's immediately at target or initial\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self.duration_steps == 0: # Already at target if duration is 0\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" # current_ramp_step is the step *within the defined sigmoid duration*\n",
|
||
" current_ramp_step = self.current_simulation_step \n",
|
||
" self.current_simulation_step +=1 # Increment for next call\n",
|
||
"\n",
|
||
" if current_ramp_step >= self.duration_steps:\n",
|
||
" # If we've completed the defined duration, output target torque\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" x = current_ramp_step / self.duration_steps # Normalized time for the ramp segment\n",
|
||
" s = 0.0\n",
|
||
" if x == 0: s = 0.0\n",
|
||
" elif x >= 1.0: s = 1.0 # Should be caught by current_ramp_step >= duration_steps\n",
|
||
" else:\n",
|
||
" x_3 = x * x * x \n",
|
||
" x_4 = x_3 * x \n",
|
||
" x_5 = x_4 * x \n",
|
||
" s = 6 * x_5 - 15 * x_4 + 10 * x_3\n",
|
||
" \n",
|
||
" self.current_torque = self.initial_torque + self.total_torque_change * s\n",
|
||
" \n",
|
||
" # If this step is the last one in the defined duration, ensure it's exactly target\n",
|
||
" if current_ramp_step == self.duration_steps -1 : # Last calculation before it's considered \"done\"\n",
|
||
" # The next call will have current_ramp_step == self.duration_steps\n",
|
||
" # So, this calculation is for x = (D-1)/D. The one for x=1 is when current_ramp_step == D\n",
|
||
" pass # The logic for current_ramp_step >= self.duration_steps will handle snapping to target.\n",
|
||
" \n",
|
||
" # Ensure if we are at the exact duration_steps point, torque is target\n",
|
||
" if current_ramp_step == self.duration_steps :\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class LinearRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Linear torque ramping, calculating one step at a time.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, step_per_cycle: float = 1.0):\n",
|
||
" if step_per_cycle <= 0:\n",
|
||
" raise ValueError(\"step_per_cycle must be positive.\")\n",
|
||
"\n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" \n",
|
||
" self.actual_step_value = 0 # Default to 0 if no change needed\n",
|
||
" if abs(initial_torque - target_torque) >= 1e-7: # Only set step if change is needed\n",
|
||
" self.actual_step_value = step_per_cycle if target_torque > initial_torque else -step_per_cycle\n",
|
||
" \n",
|
||
" self._is_done = abs(initial_torque - target_torque) < 1e-7\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_done or self.actual_step_value == 0:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" next_torque_candidate = self.current_torque + self.actual_step_value\n",
|
||
"\n",
|
||
" if (self.actual_step_value > 0 and next_torque_candidate >= self.target_torque) or \\\n",
|
||
" (self.actual_step_value < 0 and next_torque_candidate <= self.target_torque):\n",
|
||
" self.current_torque = self.target_torque # Snap to target\n",
|
||
" self._is_done = True\n",
|
||
" else:\n",
|
||
" self.current_torque = next_torque_candidate\n",
|
||
" \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"\n",
|
||
"def plot_ramps(ramp_data: list[dict], title: str, hz: float = 100.0):\n",
|
||
" \"\"\"\n",
|
||
" Plots multiple torque ramps on the same graph, with a secondary x-axis for time in seconds.\n",
|
||
" (Implementation remains largely the same)\n",
|
||
" \"\"\"\n",
|
||
" fig, ax1 = plt.subplots(figsize=(14, 8)) \n",
|
||
" max_len = 0 \n",
|
||
" for ramp_info in ramp_data:\n",
|
||
" num_points = len(ramp_info['values'])\n",
|
||
" if num_points > max_len: max_len = num_points\n",
|
||
" steps_values = range(num_points) \n",
|
||
" ax1.plot(steps_values, ramp_info['values'], \n",
|
||
" label=ramp_info['label'], \n",
|
||
" linestyle=ramp_info.get('style', '-'), \n",
|
||
" marker=ramp_info.get('marker', None),\n",
|
||
" markersize=ramp_info.get('markersize', 4)) \n",
|
||
" ax1.set_title(title, fontsize=16)\n",
|
||
" ax1.set_xlabel(f\"Simulation Cycles (Steps @ {hz}Hz)\", fontsize=12) # Changed label\n",
|
||
" ax1.set_ylabel(\"Torque\", fontsize=12)\n",
|
||
" ax1.legend(loc='best') \n",
|
||
" ax1.grid(True, which='both', linestyle='--', linewidth=0.5) \n",
|
||
" ax2 = ax1.twiny() \n",
|
||
" ax1_xlim = ax1.get_xlim()\n",
|
||
" if ax1_xlim[1] > ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(ax1_xlim[0] / hz, ax1_xlim[1] / hz)\n",
|
||
" else: \n",
|
||
" ax2.set_xlim(ax1_xlim[0], ax1_xlim[1])\n",
|
||
" ax2.set_xlabel(\"Time (seconds)\", fontsize=12)\n",
|
||
" ax2.xaxis.set_major_formatter(ticker.FormatStrFormatter('%.4f'))\n",
|
||
" if max_len > 0:\n",
|
||
" xlim_max_steps = max_len -1 \n",
|
||
" padding = int(xlim_max_steps * 0.05) if xlim_max_steps > 20 else 1\n",
|
||
" ax1.set_xlim(-0.5, xlim_max_steps + padding + 0.5) \n",
|
||
" else: \n",
|
||
" ax1.set_xlim(-0.5, 1.5)\n",
|
||
" new_ax1_xlim = ax1.get_xlim()\n",
|
||
" if new_ax1_xlim[1] > new_ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0] / hz, new_ax1_xlim[1] / hz)\n",
|
||
" else:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0], new_ax1_xlim[1])\n",
|
||
" fig.tight_layout() \n",
|
||
" plt.show()\n",
|
||
"\n",
|
||
"# --- Example Usage ---\n",
|
||
"if __name__ == \"__main__\":\n",
|
||
" controller_hz = 100.0 \n",
|
||
" simulation_cycles = 160 # Arbitrary number of cycles to run the simulation\n",
|
||
"\n",
|
||
" initial_tq_up = 50.0\n",
|
||
" target_tq_up = 200.0\n",
|
||
" initial_tq_down = 200.0\n",
|
||
" target_tq_down = 25.0\n",
|
||
" linear_step_size = 1.0 \n",
|
||
"\n",
|
||
" alphas_config = {\n",
|
||
" \"Slow (α=0.05)\": 0.05, \n",
|
||
" \"Medium (α=0.2)\": 0.2, \n",
|
||
" \"Fast (α=0.5)\": 0.5\n",
|
||
" }\n",
|
||
" \n",
|
||
" # For Sigmoid, duration_steps is the number of steps *to complete* the ramp.\n",
|
||
" # The ramp will output target_torque after these many steps.\n",
|
||
" durations_config = { \n",
|
||
" \"Short (10 steps completion)\": 10, \n",
|
||
" \"Medium (20 steps completion)\": 20,\n",
|
||
" \"Long (40 steps completion)\": 40 \n",
|
||
" }\n",
|
||
"\n",
|
||
" # --- Ramp Up Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Up ({initial_tq_up} to {target_tq_up}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_up_plot_data = []\n",
|
||
"\n",
|
||
" # Linear Ramp Up\n",
|
||
" linear_ramper_up = LinearRamp(initial_tq_up, target_tq_up, linear_step_size)\n",
|
||
" linear_ramp_up_values = [initial_tq_up] # Start with initial value\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" linear_ramp_up_values.append(linear_ramper_up.step())\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Linear (+{linear_step_size}/step)\", \"values\": linear_ramp_up_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Up after {simulation_cycles} cycles: final torque {linear_ramp_up_values[-1]:.1f}\")\n",
|
||
"\n",
|
||
" # EMA Ramps Up\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ema_ramper_up = EmaRamp(initial_tq_up, target_tq_up, alpha_val)\n",
|
||
" ema_ramp_up_values = [initial_tq_up]\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" ema_ramp_up_values.append(ema_ramper_up.step())\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": ema_ramp_up_values})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {ema_ramp_up_values[-1]:.1f}\")\n",
|
||
"\n",
|
||
" # Sigmoid Ramps Up\n",
|
||
" for label_suffix, dur_val in durations_config.items():\n",
|
||
" sigmoid_ramper_up = SigmoidRamp(initial_tq_up, target_tq_up, dur_val)\n",
|
||
" sigmoid_ramp_up_values = [initial_tq_up]\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" sigmoid_ramp_up_values.append(sigmoid_ramper_up.step())\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": sigmoid_ramp_up_values, \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix} after {simulation_cycles} cycles: final torque {sigmoid_ramp_up_values[-1]:.1f}\")\n",
|
||
" \n",
|
||
" plot_ramps(ramp_up_plot_data, f\"Torque Ramp Up: {initial_tq_up} to {target_tq_up} (Simulated for {simulation_cycles} cycles)\", hz=controller_hz)\n",
|
||
"\n",
|
||
" # --- Ramp Down Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Down ({initial_tq_down} to {target_tq_down}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_down_plot_data = []\n",
|
||
"\n",
|
||
" # Linear Ramp Down\n",
|
||
" linear_ramper_down = LinearRamp(initial_tq_down, target_tq_down, linear_step_size)\n",
|
||
" linear_ramp_down_values = [initial_tq_down]\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" linear_ramp_down_values.append(linear_ramper_down.step())\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Linear (-{linear_step_size}/step)\", \"values\": linear_ramp_down_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Down after {simulation_cycles} cycles: final torque {linear_ramp_down_values[-1]:.1f}\")\n",
|
||
" \n",
|
||
" # EMA Ramps Down\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ema_ramper_down = EmaRamp(initial_tq_down, target_tq_down, alpha_val)\n",
|
||
" ema_ramp_down_values = [initial_tq_down]\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" ema_ramp_down_values.append(ema_ramper_down.step())\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": ema_ramp_down_values})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {ema_ramp_down_values[-1]:.1f}\")\n",
|
||
"\n",
|
||
" # Sigmoid Ramps Down\n",
|
||
" for label_suffix, dur_val in durations_config.items():\n",
|
||
" sigmoid_ramper_down = SigmoidRamp(initial_tq_down, target_tq_down, dur_val)\n",
|
||
" sigmoid_ramp_down_values = [initial_tq_down]\n",
|
||
" for _ in range(simulation_cycles):\n",
|
||
" sigmoid_ramp_down_values.append(sigmoid_ramper_down.step())\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": sigmoid_ramp_down_values, \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix} after {simulation_cycles} cycles: final torque {sigmoid_ramp_down_values[-1]:.1f}\")\n",
|
||
"\n",
|
||
" plot_ramps(ramp_down_plot_data, f\"Torque Ramp Down: {initial_tq_down} to {target_tq_down} (Simulated for {simulation_cycles} cycles)\", hz=controller_hz)\n",
|
||
"\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "05211d6c-598f-4171-964b-727c14c6244e",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# torque_ramping.py\n",
|
||
"\n",
|
||
"import math\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"import matplotlib.ticker as ticker # For formatting the secondary x-axis\n",
|
||
"\n",
|
||
"class EmaRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages EMA torque ramping, calculating one step at a time.\n",
|
||
" Can handle dynamic target torque changes.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, alpha: float, \n",
|
||
" completion_threshold: float = 0.999):\n",
|
||
" if not (0 < alpha <= 1):\n",
|
||
" raise ValueError(\"Alpha must be between 0 (exclusive) and 1 (inclusive).\")\n",
|
||
" \n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.alpha = alpha\n",
|
||
" \n",
|
||
" self._is_effectively_done = False \n",
|
||
" self._initial_torque_for_threshold = initial_torque # Used for current segment\n",
|
||
" self._total_change_for_threshold = target_torque - initial_torque # For current segment\n",
|
||
" self._completion_threshold = completion_threshold\n",
|
||
" self._update_done_status()\n",
|
||
"\n",
|
||
" def _update_done_status(self):\n",
|
||
" \"\"\"Checks if the current torque is at the target torque.\"\"\"\n",
|
||
" self._is_effectively_done = abs(self.current_torque - self.target_torque) < 1e-7\n",
|
||
" if not self._is_effectively_done:\n",
|
||
" # Recalculate threshold parameters for the new segment if target changed\n",
|
||
" self._initial_torque_for_threshold = self.current_torque \n",
|
||
" self._total_change_for_threshold = self.target_torque - self.current_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque and resets ramp state if necessary.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7: # If target actually changed\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self._update_done_status() # Reset done status and threshold params\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_effectively_done:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" next_torque = (self.target_torque * self.alpha) + (self.current_torque * (1 - self.alpha))\n",
|
||
" self.current_torque = next_torque\n",
|
||
" \n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_effectively_done = True\n",
|
||
" elif abs(self._total_change_for_threshold) > 1e-7:\n",
|
||
" # Check progress only if there's a change to be made for current segment\n",
|
||
" current_progress_fraction = abs(self.current_torque - self._initial_torque_for_threshold) / abs(self._total_change_for_threshold)\n",
|
||
" if current_progress_fraction >= self._completion_threshold:\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_effectively_done = True\n",
|
||
" elif abs(self._total_change_for_threshold) <= 1e-7: # No change needed for this segment\n",
|
||
" self._is_effectively_done = True\n",
|
||
"\n",
|
||
"\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class SigmoidRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Sigmoid (Smootherstep) torque ramping, calculating one step at a time.\n",
|
||
" Can handle dynamic target torque changes by starting a new S-curve segment.\n",
|
||
" Formula: s = 6x⁵ - 15x⁴ + 10x³\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, duration_steps: int):\n",
|
||
" if duration_steps < 0: \n",
|
||
" raise ValueError(\"Duration steps must be non-negative (0 means immediate).\")\n",
|
||
" \n",
|
||
" self.initial_segment_torque = initial_torque # Torque at the start of the current S-curve segment\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.original_duration_steps = duration_steps # Fixed duration for any segment\n",
|
||
" \n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.current_segment_step = 0 # Steps taken *within the current S-curve segment*\n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
"\n",
|
||
" if self.original_duration_steps == 0:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
" def _recalculate_segment_parameters(self):\n",
|
||
" \"\"\"Recalculates parameters for the current S-curve segment.\"\"\"\n",
|
||
" self.current_segment_total_change = self.target_torque - self.initial_segment_torque\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque. Starts a new S-curve segment from current torque.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7:\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self.initial_segment_torque = self.current_torque # New segment starts from current torque\n",
|
||
" self.current_segment_step = 0 # Reset step count for the new segment\n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
" if self.original_duration_steps == 0: # If duration is 0, snap to new target\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self.original_duration_steps == 0:\n",
|
||
" # self.current_torque is already set to target_torque in init or set_target_torque\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" if self.current_segment_step >= self.original_duration_steps:\n",
|
||
" # If current segment duration is complete, output target torque of this segment\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" # We don't increment current_segment_step beyond duration_steps here,\n",
|
||
" # it will just keep returning target_torque.\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" # Calculate S-curve factor for the current step in the current segment\n",
|
||
" # Note: x should go from 0 to 1 over original_duration_steps.\n",
|
||
" # If original_duration_steps is 1, x will be 0/1 then 1/1.\n",
|
||
" x = 0.0\n",
|
||
" if self.original_duration_steps > 0 : # Avoid division by zero if duration is 0 (though handled above)\n",
|
||
" x = self.current_segment_step / self.original_duration_steps\n",
|
||
" \n",
|
||
" s = 0.0\n",
|
||
" if x <= 0: s = 0.0 # Handles x=0\n",
|
||
" elif x >= 1.0: s = 1.0 # Handles x=1\n",
|
||
" else:\n",
|
||
" x_3 = x * x * x \n",
|
||
" x_4 = x_3 * x \n",
|
||
" x_5 = x_4 * x \n",
|
||
" s = 6 * x_5 - 15 * x_4 + 10 * x_3\n",
|
||
" \n",
|
||
" self.current_torque = self.initial_segment_torque + self.current_segment_total_change * s\n",
|
||
" self.current_segment_step += 1 # Increment for the next call\n",
|
||
" \n",
|
||
" # If this was the last step to calculate for the segment, ensure it's precisely the target\n",
|
||
" if self.current_segment_step == self.original_duration_steps:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class LinearRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Linear torque ramping, calculating one step at a time.\n",
|
||
" Can handle dynamic target torque changes.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, step_per_cycle: float = 1.0):\n",
|
||
" if step_per_cycle <= 0:\n",
|
||
" raise ValueError(\"step_per_cycle must be positive.\")\n",
|
||
"\n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.step_per_cycle_magnitude = step_per_cycle # Store the magnitude\n",
|
||
" \n",
|
||
" self._actual_step_value = 0\n",
|
||
" self._is_done = False\n",
|
||
" self._update_step_direction_and_done_status()\n",
|
||
"\n",
|
||
" def _update_step_direction_and_done_status(self):\n",
|
||
" \"\"\"Updates step direction based on current and target torque, and done status.\"\"\"\n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self._actual_step_value = 0\n",
|
||
" self._is_done = True\n",
|
||
" else:\n",
|
||
" self._actual_step_value = self.step_per_cycle_magnitude if self.target_torque > self.current_torque else -self.step_per_cycle_magnitude\n",
|
||
" self._is_done = False\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque and re-evaluates step direction and done status.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7:\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self._update_step_direction_and_done_status()\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_done or self._actual_step_value == 0:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" next_torque_candidate = self.current_torque + self._actual_step_value\n",
|
||
"\n",
|
||
" if (self._actual_step_value > 0 and next_torque_candidate >= self.target_torque) or \\\n",
|
||
" (self._actual_step_value < 0 and next_torque_candidate <= self.target_torque):\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_done = True # Mark as done for this target\n",
|
||
" self._actual_step_value = 0 # Stop stepping until target changes again\n",
|
||
" else:\n",
|
||
" self.current_torque = next_torque_candidate\n",
|
||
" \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"\n",
|
||
"def plot_ramps(ramp_data: list[dict], title: str, hz: float = 100.0, events: list[dict] = None):\n",
|
||
" \"\"\"\n",
|
||
" Plots multiple torque ramps on the same graph, with a secondary x-axis for time in seconds.\n",
|
||
" Can also plot vertical lines for events like target changes.\n",
|
||
" \"\"\"\n",
|
||
" fig, ax1 = plt.subplots(figsize=(14, 8)) \n",
|
||
" max_len = 0 \n",
|
||
" for ramp_info in ramp_data:\n",
|
||
" num_points = len(ramp_info['values'])\n",
|
||
" if num_points > max_len: max_len = num_points\n",
|
||
" steps_values = range(num_points) \n",
|
||
" ax1.plot(steps_values, ramp_info['values'], \n",
|
||
" label=ramp_info['label'], \n",
|
||
" linestyle=ramp_info.get('style', '-'), \n",
|
||
" marker=ramp_info.get('marker', None),\n",
|
||
" markersize=ramp_info.get('markersize', 4)) \n",
|
||
"\n",
|
||
" if events:\n",
|
||
" for event in events:\n",
|
||
" ax1.axvline(x=event['cycle'], color=event.get('color', 'r'), linestyle=event.get('linestyle', '--'), label=event['label'])\n",
|
||
"\n",
|
||
"\n",
|
||
" ax1.set_title(title, fontsize=16)\n",
|
||
" ax1.set_xlabel(f\"Simulation Cycles (Steps @ {hz}Hz)\", fontsize=12)\n",
|
||
" ax1.set_ylabel(\"Torque\", fontsize=12)\n",
|
||
" ax1.legend(loc='best') \n",
|
||
" ax1.grid(True, which='both', linestyle='--', linewidth=0.5) \n",
|
||
" ax2 = ax1.twiny() \n",
|
||
" ax1_xlim = ax1.get_xlim()\n",
|
||
" if ax1_xlim[1] > ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(ax1_xlim[0] / hz, ax1_xlim[1] / hz)\n",
|
||
" else: \n",
|
||
" ax2.set_xlim(ax1_xlim[0], ax1_xlim[1])\n",
|
||
" ax2.set_xlabel(\"Time (seconds)\", fontsize=12)\n",
|
||
" ax2.xaxis.set_major_formatter(ticker.FormatStrFormatter('%.4f'))\n",
|
||
" if max_len > 0:\n",
|
||
" xlim_max_steps = max_len -1 \n",
|
||
" padding = int(xlim_max_steps * 0.05) if xlim_max_steps > 20 else 1\n",
|
||
" ax1.set_xlim(-0.5, xlim_max_steps + padding + 0.5) \n",
|
||
" else: \n",
|
||
" ax1.set_xlim(-0.5, 1.5)\n",
|
||
" new_ax1_xlim = ax1.get_xlim()\n",
|
||
" if new_ax1_xlim[1] > new_ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0] / hz, new_ax1_xlim[1] / hz)\n",
|
||
" else:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0], new_ax1_xlim[1])\n",
|
||
" fig.tight_layout() \n",
|
||
" plt.show()\n",
|
||
"\n",
|
||
"# --- Example Usage ---\n",
|
||
"if __name__ == \"__main__\":\n",
|
||
" controller_hz = 100.0 \n",
|
||
" simulation_cycles = 200 # Increased cycles to see effect of change\n",
|
||
" change_target_cycle = 50 # Cycle at which target torque changes\n",
|
||
"\n",
|
||
" initial_tq_up = 50.0\n",
|
||
" target_tq_up_initial = 200.0\n",
|
||
" target_tq_up_new = 120.0 # New target for ramp up scenario\n",
|
||
"\n",
|
||
" initial_tq_down = 200.0\n",
|
||
" target_tq_down_initial = 50.0\n",
|
||
" target_tq_down_new = 65.0 # New target for ramp down scenario\n",
|
||
" \n",
|
||
" linear_step_size = 1.0 \n",
|
||
"\n",
|
||
" alphas_config = {\n",
|
||
" #\"Slow (α=0.05)\": 0.05, \n",
|
||
" \"Medium (α=0.2)\": 0.2, \n",
|
||
" #\"Fast (α=0.5)\": 0.5\n",
|
||
" }\n",
|
||
" \n",
|
||
" durations_config = { \n",
|
||
" \"Short (10 steps completion)\": 10, \n",
|
||
" \"Medium (25 steps completion)\": 25,\n",
|
||
" \"Long (40 steps completion)\": 40 \n",
|
||
" }\n",
|
||
"\n",
|
||
" # Store all rampers for dynamic target change\n",
|
||
" all_rampers_up = []\n",
|
||
" all_rampers_down = []\n",
|
||
"\n",
|
||
" # --- Ramp Up Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Up ({initial_tq_up} to {target_tq_up_initial}, then to {target_tq_up_new}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_up_plot_data = []\n",
|
||
"\n",
|
||
" # Linear Ramp Up\n",
|
||
" linear_ramper_up = LinearRamp(initial_tq_up, target_tq_up_initial, linear_step_size)\n",
|
||
" all_rampers_up.append(linear_ramper_up)\n",
|
||
" linear_ramp_up_values = [initial_tq_up] \n",
|
||
" \n",
|
||
" # EMA Ramps Up\n",
|
||
" ema_rampers_up_dict = {}\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ramper = EmaRamp(initial_tq_up, target_tq_up_initial, alpha_val)\n",
|
||
" ema_rampers_up_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_up]}\n",
|
||
" all_rampers_up.append(ramper)\n",
|
||
"\n",
|
||
" # Sigmoid Ramps Up\n",
|
||
" sigmoid_rampers_up_dict = {}\n",
|
||
" for label_suffix, dur_val in durations_config.items():\n",
|
||
" ramper = SigmoidRamp(initial_tq_up, target_tq_up_initial, dur_val)\n",
|
||
" sigmoid_rampers_up_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_up]}\n",
|
||
" all_rampers_up.append(ramper)\n",
|
||
"\n",
|
||
" # Main simulation loop for RAMP UP\n",
|
||
" for cycle in range(simulation_cycles):\n",
|
||
" if cycle == change_target_cycle:\n",
|
||
" print(f\"Cycle {cycle}: Changing ramp UP target to {target_tq_up_new}\")\n",
|
||
" for ramper in all_rampers_up:\n",
|
||
" ramper.set_target_torque(target_tq_up_new)\n",
|
||
" \n",
|
||
" linear_ramp_up_values.append(linear_ramper_up.step())\n",
|
||
" for config in ema_rampers_up_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
" for config in sigmoid_rampers_up_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
"\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Linear (+{linear_step_size}/step)\", \"values\": linear_ramp_up_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Up after {simulation_cycles} cycles: final torque {linear_ramp_up_values[-1]:.1f}\")\n",
|
||
" for label_suffix, config in ema_rampers_up_dict.items():\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": config[\"values\"]})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
" for label_suffix, config in sigmoid_rampers_up_dict.items():\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": config[\"values\"], \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
" \n",
|
||
" plot_events_up = [{'cycle': change_target_cycle, 'label': f'Target -> {target_tq_up_new}'}]\n",
|
||
" plot_ramps(ramp_up_plot_data, f\"Torque Ramp Up with Dynamic Target (Simulated for {simulation_cycles} cycles)\", hz=controller_hz, events=plot_events_up)\n",
|
||
"\n",
|
||
"\n",
|
||
" # --- Ramp Down Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Down ({initial_tq_down} to {target_tq_down_initial}, then to {target_tq_down_new}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_down_plot_data = []\n",
|
||
"\n",
|
||
" # Linear Ramp Down\n",
|
||
" linear_ramper_down = LinearRamp(initial_tq_down, target_tq_down_initial, linear_step_size)\n",
|
||
" all_rampers_down.append(linear_ramper_down)\n",
|
||
" linear_ramp_down_values = [initial_tq_down]\n",
|
||
"\n",
|
||
" # EMA Ramps Down\n",
|
||
" ema_rampers_down_dict = {}\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ramper = EmaRamp(initial_tq_down, target_tq_down_initial, alpha_val)\n",
|
||
" ema_rampers_down_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_down]}\n",
|
||
" all_rampers_down.append(ramper)\n",
|
||
"\n",
|
||
" # Sigmoid Ramps Down\n",
|
||
" sigmoid_rampers_down_dict = {}\n",
|
||
" for label_suffix, dur_val in durations_config.items():\n",
|
||
" ramper = SigmoidRamp(initial_tq_down, target_tq_down_initial, dur_val)\n",
|
||
" sigmoid_rampers_down_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_down]}\n",
|
||
" all_rampers_down.append(ramper)\n",
|
||
"\n",
|
||
" # Main simulation loop for RAMP DOWN\n",
|
||
" for cycle in range(simulation_cycles):\n",
|
||
" if cycle == change_target_cycle:\n",
|
||
" print(f\"Cycle {cycle}: Changing ramp DOWN target to {target_tq_down_new}\")\n",
|
||
" for ramper in all_rampers_down:\n",
|
||
" ramper.set_target_torque(target_tq_down_new)\n",
|
||
"\n",
|
||
" linear_ramp_down_values.append(linear_ramper_down.step())\n",
|
||
" for config in ema_rampers_down_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
" for config in sigmoid_rampers_down_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
"\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Linear (-{linear_step_size}/step)\", \"values\": linear_ramp_down_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Down after {simulation_cycles} cycles: final torque {linear_ramp_down_values[-1]:.1f}\")\n",
|
||
" for label_suffix, config in ema_rampers_down_dict.items():\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": config[\"values\"]})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
" for label_suffix, config in sigmoid_rampers_down_dict.items():\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Sigmoid {label_suffix}\", \"values\": config[\"values\"], \"style\": \"--\"})\n",
|
||
" print(f\"Sigmoid {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
"\n",
|
||
" plot_events_down = [{'cycle': change_target_cycle, 'label': f'Target -> {target_tq_down_new}'}]\n",
|
||
" plot_ramps(ramp_down_plot_data, f\"Torque Ramp Down with Dynamic Target (Simulated for {simulation_cycles} cycles)\", hz=controller_hz, events=plot_events_down)\n",
|
||
"\n",
|
||
" print(\"\\nPlotting complete. Close plot windows to exit.\")\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "24c01cba-9b02-4ae1-be5c-559fc2447e4c",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# torque_ramping.py\n",
|
||
"\n",
|
||
"import math\n",
|
||
"import matplotlib.pyplot as plt\n",
|
||
"import matplotlib.ticker as ticker # For formatting the secondary x-axis\n",
|
||
"\n",
|
||
"class EmaRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages EMA torque ramping, calculating one step at a time.\n",
|
||
" Can handle dynamic target torque changes.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, alpha: float, \n",
|
||
" completion_threshold: float = 0.999):\n",
|
||
" if not (0 < alpha <= 1):\n",
|
||
" raise ValueError(\"Alpha must be between 0 (exclusive) and 1 (inclusive).\")\n",
|
||
" \n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.alpha = alpha\n",
|
||
" \n",
|
||
" self._is_effectively_done = False \n",
|
||
" self._initial_torque_for_threshold = initial_torque \n",
|
||
" self._total_change_for_threshold = target_torque - initial_torque \n",
|
||
" self._completion_threshold = completion_threshold\n",
|
||
" self._update_done_status()\n",
|
||
"\n",
|
||
" def _update_done_status(self):\n",
|
||
" \"\"\"Checks if the current torque is at the target torque.\"\"\"\n",
|
||
" self._is_effectively_done = abs(self.current_torque - self.target_torque) < 1e-7\n",
|
||
" if not self._is_effectively_done:\n",
|
||
" self._initial_torque_for_threshold = self.current_torque \n",
|
||
" self._total_change_for_threshold = self.target_torque - self.current_torque\n",
|
||
" else: # If done, ensure threshold params reflect no change needed\n",
|
||
" self._initial_torque_for_threshold = self.current_torque\n",
|
||
" self._total_change_for_threshold = 0\n",
|
||
"\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque and resets ramp state if necessary.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7: \n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self._update_done_status() \n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_effectively_done:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" next_torque = (self.target_torque * self.alpha) + (self.current_torque * (1 - self.alpha))\n",
|
||
" self.current_torque = next_torque\n",
|
||
" \n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_effectively_done = True\n",
|
||
" elif abs(self._total_change_for_threshold) > 1e-7:\n",
|
||
" current_progress_fraction = abs(self.current_torque - self._initial_torque_for_threshold) / abs(self._total_change_for_threshold)\n",
|
||
" if current_progress_fraction >= self._completion_threshold:\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_effectively_done = True\n",
|
||
" elif abs(self._total_change_for_threshold) <= 1e-7: \n",
|
||
" self._is_effectively_done = True\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class SigmoidRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Sigmoid (Smootherstep) torque ramping with dynamic duration.\n",
|
||
" Duration is based on torque delta, clamped by min/max steps.\n",
|
||
" Formula: s = 6x⁵ - 15x⁴ + 10x³\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, config: dict):\n",
|
||
" self.config = { # Default config values\n",
|
||
" \"steps_per_unit_torque\": 0.2,\n",
|
||
" \"min_duration_steps\": 3, # S-curve needs at least a few steps\n",
|
||
" \"max_duration_steps\": 50,\n",
|
||
" **config # User provided config overrides defaults\n",
|
||
" }\n",
|
||
" if self.config[\"min_duration_steps\"] < 1:\n",
|
||
" raise ValueError(\"min_duration_steps must be at least 1 for Sigmoid.\")\n",
|
||
" if self.config[\"max_duration_steps\"] < self.config[\"min_duration_steps\"]:\n",
|
||
" raise ValueError(\"max_duration_steps must be >= min_duration_steps.\")\n",
|
||
"\n",
|
||
"\n",
|
||
" self.initial_segment_torque = initial_torque \n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.current_torque = initial_torque\n",
|
||
" \n",
|
||
" self.current_segment_step = 0 \n",
|
||
" self.current_segment_duration_steps = 0 # Will be calculated\n",
|
||
" self.current_segment_total_change = 0 # Will be calculated\n",
|
||
" \n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
"\n",
|
||
"\n",
|
||
" def _recalculate_segment_parameters(self):\n",
|
||
" \"\"\"Recalculates parameters for the current S-curve segment, including dynamic duration.\"\"\"\n",
|
||
" self.current_segment_total_change = self.target_torque - self.initial_segment_torque\n",
|
||
" abs_delta = abs(self.current_segment_total_change)\n",
|
||
"\n",
|
||
" if abs_delta < 1e-7: # Effectively no change needed\n",
|
||
" self.current_segment_duration_steps = 0\n",
|
||
" self.current_torque = self.target_torque # Snap to target\n",
|
||
" else:\n",
|
||
" calculated_duration = round(abs_delta * self.config[\"steps_per_unit_torque\"])\n",
|
||
" self.current_segment_duration_steps = max(self.config[\"min_duration_steps\"], \n",
|
||
" min(self.config[\"max_duration_steps\"], calculated_duration))\n",
|
||
" \n",
|
||
" # If duration is 0 (either by calculation for zero delta, or if min_duration was 0 - though disallowed),\n",
|
||
" # ensure current_torque is target.\n",
|
||
" if self.current_segment_duration_steps == 0:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque. Starts a new S-curve segment from current torque with re-calculated dynamic duration.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7:\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self.initial_segment_torque = self.current_torque \n",
|
||
" self.current_segment_step = 0 \n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self.current_segment_duration_steps == 0: \n",
|
||
" # This means target is already reached or delta was zero.\n",
|
||
" # self.current_torque should already be self.target_torque from _recalculate_segment_parameters\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" if self.current_segment_step >= self.current_segment_duration_steps:\n",
|
||
" # Segment complete, hold target torque of this segment\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" x = 0.0\n",
|
||
" # Ensure current_segment_duration_steps is positive before division\n",
|
||
" if self.current_segment_duration_steps > 0:\n",
|
||
" x = self.current_segment_step / self.current_segment_duration_steps\n",
|
||
" \n",
|
||
" s = 0.0\n",
|
||
" if x <= 0: s = 0.0\n",
|
||
" elif x >= 1.0: s = 1.0 \n",
|
||
" else:\n",
|
||
" x_3 = x * x * x \n",
|
||
" x_4 = x_3 * x \n",
|
||
" x_5 = x_4 * x \n",
|
||
" s = 6 * x_5 - 15 * x_4 + 10 * x_3\n",
|
||
" \n",
|
||
" self.current_torque = self.initial_segment_torque + self.current_segment_total_change * s\n",
|
||
" \n",
|
||
" # If this calculation makes it the last step of the segment, ensure it's precisely the target\n",
|
||
" if self.current_segment_step == self.current_segment_duration_steps -1 : # This is the step that calculates for x=(D-1)/D\n",
|
||
" # The next increment of current_segment_step will make it equal to duration\n",
|
||
" pass\n",
|
||
"\n",
|
||
" self.current_segment_step += 1 \n",
|
||
"\n",
|
||
" # After incrementing, if current_segment_step IS NOW duration_steps, it means the s-curve for x=1 has been effectively applied.\n",
|
||
" # Or, if x was already >= 1.0, snap to target.\n",
|
||
" if self.current_segment_step >= self.current_segment_duration_steps or x >= 1.0:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"class LinearRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Linear torque ramping, calculating one step at a time.\n",
|
||
" Can handle dynamic target torque changes.\n",
|
||
" \"\"\"\n",
|
||
" def __init__(self, initial_torque: float, target_torque: float, step_per_cycle: float = 1.0):\n",
|
||
" if step_per_cycle <= 0:\n",
|
||
" raise ValueError(\"step_per_cycle must be positive.\")\n",
|
||
"\n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = target_torque\n",
|
||
" self.step_per_cycle_magnitude = step_per_cycle \n",
|
||
" \n",
|
||
" self._actual_step_value = 0\n",
|
||
" self._is_done = False\n",
|
||
" self._update_step_direction_and_done_status()\n",
|
||
"\n",
|
||
" def _update_step_direction_and_done_status(self):\n",
|
||
" \"\"\"Updates step direction based on current and target torque, and done status.\"\"\"\n",
|
||
" if abs(self.current_torque - self.target_torque) < 1e-7:\n",
|
||
" self._actual_step_value = 0\n",
|
||
" self._is_done = True\n",
|
||
" else:\n",
|
||
" self._actual_step_value = self.step_per_cycle_magnitude if self.target_torque > self.current_torque else -self.step_per_cycle_magnitude\n",
|
||
" self._is_done = False\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"Updates the target torque and re-evaluates step direction and done status.\"\"\"\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7:\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" self._update_step_direction_and_done_status()\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"Calculates and returns the next torque value for this cycle.\"\"\"\n",
|
||
" if self._is_done or self._actual_step_value == 0:\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" next_torque_candidate = self.current_torque + self._actual_step_value\n",
|
||
"\n",
|
||
" if (self._actual_step_value > 0 and next_torque_candidate >= self.target_torque) or \\\n",
|
||
" (self._actual_step_value < 0 and next_torque_candidate <= self.target_torque):\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" self._is_done = True \n",
|
||
" self._actual_step_value = 0 \n",
|
||
" else:\n",
|
||
" self.current_torque = next_torque_candidate\n",
|
||
" \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"\n",
|
||
"def plot_ramps(ramp_data: list[dict], title: str, hz: float = 100.0, events: list[dict] = None):\n",
|
||
" \"\"\"\n",
|
||
" Plots multiple torque ramps on the same graph, with a secondary x-axis for time in seconds.\n",
|
||
" Can also plot vertical lines for events like target changes.\n",
|
||
" \"\"\"\n",
|
||
" fig, ax1 = plt.subplots(figsize=(14, 8)) \n",
|
||
" max_len = 0 \n",
|
||
" for ramp_info in ramp_data:\n",
|
||
" num_points = len(ramp_info['values'])\n",
|
||
" if num_points > max_len: max_len = num_points\n",
|
||
" steps_values = range(num_points) \n",
|
||
" ax1.plot(steps_values, ramp_info['values'], \n",
|
||
" label=ramp_info['label'], \n",
|
||
" linestyle=ramp_info.get('style', '-'), \n",
|
||
" marker=ramp_info.get('marker', None),\n",
|
||
" markersize=ramp_info.get('markersize', 4)) \n",
|
||
"\n",
|
||
" if events:\n",
|
||
" for event in events:\n",
|
||
" ax1.axvline(x=event['cycle'], color=event.get('color', 'r'), linestyle=event.get('linestyle', '--'), label=event['label'])\n",
|
||
"\n",
|
||
"\n",
|
||
" ax1.set_title(title, fontsize=16)\n",
|
||
" ax1.set_xlabel(f\"Simulation Cycles (Steps @ {hz}Hz)\", fontsize=12)\n",
|
||
" ax1.set_ylabel(\"Torque\", fontsize=12)\n",
|
||
" # Filter out event labels from main legend if they were added by axvline\n",
|
||
" handles, labels = ax1.get_legend_handles_labels()\n",
|
||
" filtered_handles_labels = [(h, l) for h, l in zip(handles, labels) if not any(event.get('label') == l for event in (events or []))]\n",
|
||
" if filtered_handles_labels: # Only show legend if there are actual lines\n",
|
||
" ax1.legend([h for h,l in filtered_handles_labels], [l for h,l in filtered_handles_labels], loc='best')\n",
|
||
" elif events : # if only events have labels, create a legend for them\n",
|
||
" event_handles = [plt.Line2D([0], [0], color=event.get('color', 'r'), linestyle=event.get('linestyle', '--')) for event in events if event.get('label')]\n",
|
||
" event_labels = [event['label'] for event in events if event.get('label')]\n",
|
||
" if event_handles:\n",
|
||
" ax1.legend(event_handles, event_labels, loc='best')\n",
|
||
"\n",
|
||
"\n",
|
||
" ax1.grid(True, which='both', linestyle='--', linewidth=0.5) \n",
|
||
" ax2 = ax1.twiny() \n",
|
||
" ax1_xlim = ax1.get_xlim()\n",
|
||
" if ax1_xlim[1] > ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(ax1_xlim[0] / hz, ax1_xlim[1] / hz)\n",
|
||
" else: \n",
|
||
" ax2.set_xlim(ax1_xlim[0], ax1_xlim[1])\n",
|
||
" ax2.set_xlabel(\"Time (seconds)\", fontsize=12)\n",
|
||
" ax2.xaxis.set_major_formatter(ticker.FormatStrFormatter('%.4f'))\n",
|
||
" if max_len > 0:\n",
|
||
" xlim_max_steps = max_len -1 \n",
|
||
" padding = int(xlim_max_steps * 0.05) if xlim_max_steps > 20 else 1\n",
|
||
" ax1.set_xlim(-0.5, xlim_max_steps + padding + 0.5) \n",
|
||
" else: \n",
|
||
" ax1.set_xlim(-0.5, 1.5)\n",
|
||
" new_ax1_xlim = ax1.get_xlim()\n",
|
||
" if new_ax1_xlim[1] > new_ax1_xlim[0] and hz > 0:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0] / hz, new_ax1_xlim[1] / hz)\n",
|
||
" else:\n",
|
||
" ax2.set_xlim(new_ax1_xlim[0], new_ax1_xlim[1])\n",
|
||
" fig.tight_layout() \n",
|
||
" plt.show()\n",
|
||
"\n",
|
||
"# --- Example Usage ---\n",
|
||
"if __name__ == \"__main__\":\n",
|
||
" controller_hz = 100.0 \n",
|
||
" simulation_cycles = 200 \n",
|
||
" change_target_cycle = 100 \n",
|
||
"\n",
|
||
" initial_tq_up = 50.0\n",
|
||
" target_tq_up_initial = 200.0\n",
|
||
" target_tq_up_new = 120.0 \n",
|
||
"\n",
|
||
" initial_tq_down = 200.0\n",
|
||
" target_tq_down_initial = 25.0\n",
|
||
" target_tq_down_new = 180.0 \n",
|
||
" \n",
|
||
" linear_step_size = 1.0 \n",
|
||
"\n",
|
||
" alphas_config = {\n",
|
||
" #\"Slowest (α=0.05)\": 0.05, \n",
|
||
" \"Slower (α=0.07)\": 0.07,\n",
|
||
" \"Slow (α=0.1)\": 0.1, \n",
|
||
" #\"Medium (α=0.2)\": 0.2, \n",
|
||
" #\"Fast (α=0.5)\": 0.5\n",
|
||
" }\n",
|
||
" \n",
|
||
" # Configuration for dynamic sigmoid duration\n",
|
||
" sigmoid_dynamic_configs = { \n",
|
||
" #\"Sigmoid Responsive (0.1 steps/unit, min 3, max 30)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.1, \"min_duration_steps\": 3, \"max_duration_steps\": 30\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.25 steps/unit, min 5, max 50)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.25, \"min_duration_steps\": 5, \"max_duration_steps\": 50\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.25 steps/unit, min 5, max 100)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.25, \"min_duration_steps\": 5, \"max_duration_steps\": 100\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.5 steps/unit, min 25, max 100)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.5, \"min_duration_steps\": 25, \"max_duration_steps\": 100\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.5 steps/unit, min 40, max 75)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.5, \"min_duration_steps\": 40, \"max_duration_steps\": 75\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.5 steps/unit, min 5, max 75)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.5, \"min_duration_steps\": 5, \"max_duration_steps\": 75\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.1 steps/unit, min 40, max 75)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.1, \"min_duration_steps\": 40, \"max_duration_steps\": 75\n",
|
||
" #},\n",
|
||
" #\"Sigmoid Smooth (0.7 steps/unit, min 5, max 200)\": {\n",
|
||
" # \"steps_per_unit_torque\": 0.7, \"min_duration_steps\": 5, \"max_duration_steps\": 200\n",
|
||
" #},\n",
|
||
" \"Sigmoid Smooth (0.7 steps/unit, min 5, max 75)\": {\n",
|
||
" \"steps_per_unit_torque\": 0.7, \"min_duration_steps\": 5, \"max_duration_steps\": 75\n",
|
||
" }\n",
|
||
" }\n",
|
||
"\n",
|
||
" all_rampers_up = []\n",
|
||
" all_rampers_down = []\n",
|
||
"\n",
|
||
" # --- Ramp Up Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Up ({initial_tq_up} to {target_tq_up_initial}, then to {target_tq_up_new}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_up_plot_data = []\n",
|
||
"\n",
|
||
" linear_ramper_up = LinearRamp(initial_tq_up, target_tq_up_initial, linear_step_size)\n",
|
||
" all_rampers_up.append(linear_ramper_up)\n",
|
||
" linear_ramp_up_values = [initial_tq_up] \n",
|
||
" \n",
|
||
" ema_rampers_up_dict = {}\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ramper = EmaRamp(initial_tq_up, target_tq_up_initial, alpha_val)\n",
|
||
" ema_rampers_up_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_up]}\n",
|
||
" all_rampers_up.append(ramper)\n",
|
||
"\n",
|
||
" sigmoid_rampers_up_dict = {}\n",
|
||
" for label_suffix, sig_config in sigmoid_dynamic_configs.items(): # Use new sigmoid_dynamic_configs\n",
|
||
" ramper = SigmoidRamp(initial_tq_up, target_tq_up_initial, config=sig_config)\n",
|
||
" sigmoid_rampers_up_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_up]}\n",
|
||
" all_rampers_up.append(ramper)\n",
|
||
"\n",
|
||
" for cycle in range(simulation_cycles):\n",
|
||
" if cycle == change_target_cycle:\n",
|
||
" print(f\"Cycle {cycle}: Changing ramp UP target to {target_tq_up_new}\")\n",
|
||
" for ramper in all_rampers_up:\n",
|
||
" ramper.set_target_torque(target_tq_up_new)\n",
|
||
" \n",
|
||
" linear_ramp_up_values.append(linear_ramper_up.step())\n",
|
||
" for config in ema_rampers_up_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
" for config_label, config_data in sigmoid_rampers_up_dict.items(): # Iterate through new dict structure\n",
|
||
" config_data[\"values\"].append(config_data[\"ramper\"].step())\n",
|
||
"\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"Linear (+{linear_step_size}/step)\", \"values\": linear_ramp_up_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Up after {simulation_cycles} cycles: final torque {linear_ramp_up_values[-1]:.1f}\")\n",
|
||
" for label_suffix, config in ema_rampers_up_dict.items():\n",
|
||
" ramp_up_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": config[\"values\"]})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
" for label_suffix, config_data in sigmoid_rampers_up_dict.items(): # Use new dict structure\n",
|
||
" ramp_up_plot_data.append({\"label\": label_suffix, \"values\": config_data[\"values\"], \"style\": \"--\"})\n",
|
||
" print(f\"{label_suffix} after {simulation_cycles} cycles: final torque {config_data['values'][-1]:.1f}\")\n",
|
||
" \n",
|
||
" plot_events_up = [{'cycle': change_target_cycle, 'label': f'Target -> {target_tq_up_new:.0f}'}]\n",
|
||
" plot_ramps(ramp_up_plot_data, f\"Torque Ramp Up with Dynamic Target (Simulated for {simulation_cycles} cycles)\", hz=controller_hz, events=plot_events_up)\n",
|
||
"\n",
|
||
"\n",
|
||
" # --- Ramp Down Simulation ---\n",
|
||
" print(f\"\\n--- Simulating Ramp Down ({initial_tq_down} to {target_tq_down_initial}, then to {target_tq_down_new}) for {simulation_cycles} cycles ---\")\n",
|
||
" ramp_down_plot_data = []\n",
|
||
"\n",
|
||
" linear_ramper_down = LinearRamp(initial_tq_down, target_tq_down_initial, linear_step_size)\n",
|
||
" all_rampers_down.append(linear_ramper_down)\n",
|
||
" linear_ramp_down_values = [initial_tq_down]\n",
|
||
"\n",
|
||
" ema_rampers_down_dict = {}\n",
|
||
" for label_suffix, alpha_val in alphas_config.items():\n",
|
||
" ramper = EmaRamp(initial_tq_down, target_tq_down_initial, alpha_val)\n",
|
||
" ema_rampers_down_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_down]}\n",
|
||
" all_rampers_down.append(ramper)\n",
|
||
"\n",
|
||
" sigmoid_rampers_down_dict = {}\n",
|
||
" for label_suffix, sig_config in sigmoid_dynamic_configs.items(): # Use new sigmoid_dynamic_configs\n",
|
||
" ramper = SigmoidRamp(initial_tq_down, target_tq_down_initial, config=sig_config)\n",
|
||
" sigmoid_rampers_down_dict[label_suffix] = {\"ramper\": ramper, \"values\": [initial_tq_down]}\n",
|
||
" all_rampers_down.append(ramper)\n",
|
||
"\n",
|
||
" for cycle in range(simulation_cycles):\n",
|
||
" if cycle == change_target_cycle:\n",
|
||
" print(f\"Cycle {cycle}: Changing ramp DOWN target to {target_tq_down_new}\")\n",
|
||
" for ramper in all_rampers_down:\n",
|
||
" ramper.set_target_torque(target_tq_down_new)\n",
|
||
"\n",
|
||
" linear_ramp_down_values.append(linear_ramper_down.step())\n",
|
||
" for config in ema_rampers_down_dict.values():\n",
|
||
" config[\"values\"].append(config[\"ramper\"].step())\n",
|
||
" for config_label, config_data in sigmoid_rampers_down_dict.items(): # Iterate through new dict structure\n",
|
||
" config_data[\"values\"].append(config_data[\"ramper\"].step())\n",
|
||
"\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"Linear (-{linear_step_size}/step)\", \"values\": linear_ramp_down_values, \"style\": \":\", \"marker\":\".\"})\n",
|
||
" print(f\"Linear Ramp Down after {simulation_cycles} cycles: final torque {linear_ramp_down_values[-1]:.1f}\")\n",
|
||
" for label_suffix, config in ema_rampers_down_dict.items():\n",
|
||
" ramp_down_plot_data.append({\"label\": f\"EMA {label_suffix}\", \"values\": config[\"values\"]})\n",
|
||
" print(f\"EMA {label_suffix} after {simulation_cycles} cycles: final torque {config['values'][-1]:.1f}\")\n",
|
||
" for label_suffix, config_data in sigmoid_rampers_down_dict.items(): # Use new dict structure\n",
|
||
" ramp_down_plot_data.append({\"label\": label_suffix, \"values\": config_data[\"values\"], \"style\": \"--\"})\n",
|
||
" print(f\"{label_suffix} after {simulation_cycles} cycles: final torque {config_data['values'][-1]:.1f}\")\n",
|
||
"\n",
|
||
" plot_events_down = [{'cycle': change_target_cycle, 'label': f'Target -> {target_tq_down_new:.0f}'}]\n",
|
||
" plot_ramps(ramp_down_plot_data, f\"Torque Ramp Down with Dynamic Target (Simulated for {simulation_cycles} cycles)\", hz=controller_hz, events=plot_events_down)\n",
|
||
"\n",
|
||
" print(\"\\nPlotting complete. Close plot windows to exit.\")\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "cfcb2326-6e77-4e80-9822-09e8b5b293ea",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": [
|
||
"# live_sigmoid_torque_ramp.py\n",
|
||
"import math\n",
|
||
"\n",
|
||
"class LiveSigmoidTorqueRamp:\n",
|
||
" \"\"\"\n",
|
||
" Manages Sigmoid (Smootherstep) torque ramping for a live environment,\n",
|
||
" calculating one step at a time with dynamic duration based on configuration.\n",
|
||
"\n",
|
||
" The S-curve profile is based on the Smootherstep function: s = 6x⁵ - 15x⁴ + 10x³\n",
|
||
" The duration of each ramp segment is dynamically calculated based on the\n",
|
||
" magnitude of the torque change, clamped by min_duration_steps and max_duration_steps.\n",
|
||
" \"\"\"\n",
|
||
"\n",
|
||
" def __init__(self, initial_torque: float, config: dict = None):\n",
|
||
" \"\"\"\n",
|
||
" Initializes the LiveSigmoidTorqueRamp.\n",
|
||
"\n",
|
||
" Args:\n",
|
||
" initial_torque (float): The starting torque value.\n",
|
||
" config (dict, optional): Configuration for the sigmoid ramp.\n",
|
||
" Defaults to:\n",
|
||
" {\n",
|
||
" \"steps_per_unit_torque\": 0.7,\n",
|
||
" \"min_duration_steps\": 5,\n",
|
||
" \"max_duration_steps\": 75\n",
|
||
" }\n",
|
||
" \"\"\"\n",
|
||
" default_config = {\n",
|
||
" \"steps_per_unit_torque\": 0.7,\n",
|
||
" \"min_duration_steps\": 5,\n",
|
||
" \"max_duration_steps\": 75\n",
|
||
" }\n",
|
||
" self.config = {**default_config, **(config or {})} # User config overrides defaults\n",
|
||
"\n",
|
||
" if self.config[\"min_duration_steps\"] < 1:\n",
|
||
" raise ValueError(\"min_duration_steps must be at least 1 for Sigmoid.\")\n",
|
||
" if self.config[\"max_duration_steps\"] < self.config[\"min_duration_steps\"]:\n",
|
||
" raise ValueError(\"max_duration_steps must be >= min_duration_steps.\")\n",
|
||
"\n",
|
||
" self.current_torque = initial_torque\n",
|
||
" self.target_torque = initial_torque # Initially, target is the current torque\n",
|
||
"\n",
|
||
" # Parameters for the current S-curve segment\n",
|
||
" self.initial_segment_torque = initial_torque\n",
|
||
" self.current_segment_step = 0\n",
|
||
" self.current_segment_duration_steps = 0\n",
|
||
" self.current_segment_total_change = 0\n",
|
||
" \n",
|
||
" # Initialize segment parameters based on initial state (no change)\n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
"\n",
|
||
" def _recalculate_segment_parameters(self):\n",
|
||
" \"\"\"\n",
|
||
" Recalculates parameters for the current S-curve segment,\n",
|
||
" including its dynamic duration based on the torque delta.\n",
|
||
" This is called when the target torque changes or at initialization.\n",
|
||
" \"\"\"\n",
|
||
" self.current_segment_total_change = self.target_torque - self.initial_segment_torque\n",
|
||
" abs_delta = abs(self.current_segment_total_change)\n",
|
||
"\n",
|
||
" if abs_delta < 1e-7: # Effectively no change needed for this segment\n",
|
||
" self.current_segment_duration_steps = 0\n",
|
||
" # self.current_torque is already self.initial_segment_torque.\n",
|
||
" # If initial_segment_torque is already target_torque, then current_torque is target.\n",
|
||
" # This ensures if a new target is set that is the same as current_torque, duration is 0.\n",
|
||
" if abs(self.current_torque - self.target_torque) > 1e-7 : # If current is not yet target\n",
|
||
" self.current_torque = self.target_torque # Snap to target if delta is zero\n",
|
||
" else:\n",
|
||
" calculated_duration = round(abs_delta * self.config[\"steps_per_unit_torque\"])\n",
|
||
" self.current_segment_duration_steps = max(\n",
|
||
" self.config[\"min_duration_steps\"],\n",
|
||
" min(self.config[\"max_duration_steps\"], calculated_duration)\n",
|
||
" )\n",
|
||
" \n",
|
||
" # If, after calculation, duration is 0, it means we should be at the target.\n",
|
||
" # This can happen if abs_delta was very small leading to calculated_duration < min_duration_steps,\n",
|
||
" # and min_duration_steps was then clamped to 0 (if min_duration_steps was allowed to be 0, which it isn't by the check).\n",
|
||
" # More relevantly, if abs_delta was ~0.\n",
|
||
" if self.current_segment_duration_steps == 0:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
"\n",
|
||
" def set_target_torque(self, new_target_torque: float):\n",
|
||
" \"\"\"\n",
|
||
" Updates the target torque. This will start a new S-curve ramp segment\n",
|
||
" from the current actual torque to the new target torque.\n",
|
||
" The duration of this new segment is dynamically calculated.\n",
|
||
"\n",
|
||
" Args:\n",
|
||
" new_target_torque (float): The new desired target torque.\n",
|
||
" \"\"\"\n",
|
||
" # Check if the new target is meaningfully different from the current target\n",
|
||
" if abs(self.target_torque - new_target_torque) > 1e-7:\n",
|
||
" self.target_torque = new_target_torque\n",
|
||
" # The new S-curve segment starts from the current actual torque\n",
|
||
" self.initial_segment_torque = self.current_torque \n",
|
||
" self.current_segment_step = 0 # Reset step count for the new segment\n",
|
||
" self._recalculate_segment_parameters()\n",
|
||
" # If new target is same as old target, do nothing, continue current ramp.\n",
|
||
"\n",
|
||
"\n",
|
||
" def step(self) -> float:\n",
|
||
" \"\"\"\n",
|
||
" Calculates and returns the torque value for the current control cycle.\n",
|
||
" This method should be called once per cycle in your live environment.\n",
|
||
"\n",
|
||
" Returns:\n",
|
||
" float: The calculated torque for the current cycle.\n",
|
||
" \"\"\"\n",
|
||
" # If the duration for the current segment is 0, it means:\n",
|
||
" # a) The initial torque and target torque for this segment were the same.\n",
|
||
" # b) Or, the calculated duration (based on delta and config) was clamped to 0.\n",
|
||
" # In such cases, current_torque should already be at the target_torque of this segment.\n",
|
||
" if self.current_segment_duration_steps == 0:\n",
|
||
" # self.current_torque should have been set to self.target_torque\n",
|
||
" # by _recalculate_segment_parameters when duration became 0.\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" # If the number of steps taken in the current segment has reached or exceeded its planned duration\n",
|
||
" if self.current_segment_step >= self.current_segment_duration_steps:\n",
|
||
" # The S-curve segment is complete. Hold the target torque of this segment.\n",
|
||
" # Ensure current_torque is precisely the target_torque for this completed segment.\n",
|
||
" self.current_torque = self.target_torque \n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
" # Calculate the S-curve factor 's' (normalized progress from 0 to 1)\n",
|
||
" # x is the normalized time within the current segment's duration\n",
|
||
" x = 0.0\n",
|
||
" if self.current_segment_duration_steps > 0: # Avoid division by zero\n",
|
||
" x = self.current_segment_step / self.current_segment_duration_steps\n",
|
||
" \n",
|
||
" s = 0.0\n",
|
||
" if x <= 0: # At or before the start of the segment\n",
|
||
" s = 0.0\n",
|
||
" elif x >= 1.0: # At or after the end of the segment\n",
|
||
" s = 1.0\n",
|
||
" else: # Within the segment, apply Smootherstep\n",
|
||
" x_3 = x * x * x\n",
|
||
" x_4 = x_3 * x\n",
|
||
" x_5 = x_4 * x\n",
|
||
" s = 6 * x_5 - 15 * x_4 + 10 * x_3\n",
|
||
" \n",
|
||
" # Calculate the torque based on the S-curve factor\n",
|
||
" self.current_torque = self.initial_segment_torque + self.current_segment_total_change * s\n",
|
||
" \n",
|
||
" # Increment the step count for the current segment\n",
|
||
" self.current_segment_step += 1\n",
|
||
"\n",
|
||
" # If this increment just completed the duration, ensure torque is exactly target\n",
|
||
" if self.current_segment_step >= self.current_segment_duration_steps:\n",
|
||
" self.current_torque = self.target_torque\n",
|
||
"\n",
|
||
" return self.current_torque\n",
|
||
"\n",
|
||
"# --- Example Usage (for testing the live class) ---\n",
|
||
"if __name__ == \"__main__\":\n",
|
||
" # User-specified configuration\n",
|
||
" live_config = {\n",
|
||
" \"steps_per_unit_torque\": 0.7,\n",
|
||
" \"min_duration_steps\": 5,\n",
|
||
" \"max_duration_steps\": 75\n",
|
||
" }\n",
|
||
"\n",
|
||
" # Initialize the ramp controller\n",
|
||
" initial_car_torque = 0.0\n",
|
||
" torque_controller = LiveSigmoidTorqueRamp(initial_torque=initial_car_torque, config=live_config)\n",
|
||
"\n",
|
||
" print(f\"Initial Torque: {torque_controller.current_torque:.2f}\")\n",
|
||
"\n",
|
||
" # Simulate some controller cycles\n",
|
||
" simulation_total_cycles = 150\n",
|
||
" target_change_cycle_1 = 10\n",
|
||
" new_target_1 = 100.0\n",
|
||
" target_change_cycle_2 = 70\n",
|
||
" new_target_2 = 30.0\n",
|
||
" target_change_cycle_3 = 120\n",
|
||
" new_target_3 = 50.0\n",
|
||
"\n",
|
||
"\n",
|
||
" print(f\"\\nSimulating {simulation_total_cycles} cycles at 100Hz (example):\")\n",
|
||
" print(\"Cycle | Target | Current Segment Duration | Segment Step | Output Torque\")\n",
|
||
" print(\"------|--------|--------------------------|--------------|--------------\")\n",
|
||
"\n",
|
||
" # Store values for plotting if desired (basic plot example)\n",
|
||
" output_torques = [torque_controller.current_torque]\n",
|
||
" cycle_numbers = [0]\n",
|
||
"\n",
|
||
" for cycle in range(1, simulation_total_cycles + 1):\n",
|
||
" # Potentially change target at specific cycles\n",
|
||
" if cycle == target_change_cycle_1:\n",
|
||
" print(f\"----- Cycle {cycle}: Setting new target to {new_target_1:.2f} -----\")\n",
|
||
" torque_controller.set_target_torque(new_target_1)\n",
|
||
" elif cycle == target_change_cycle_2:\n",
|
||
" print(f\"----- Cycle {cycle}: Setting new target to {new_target_2:.2f} -----\")\n",
|
||
" torque_controller.set_target_torque(new_target_2)\n",
|
||
" elif cycle == target_change_cycle_3:\n",
|
||
" print(f\"----- Cycle {cycle}: Setting new target to {new_target_3:.2f} -----\")\n",
|
||
" torque_controller.set_target_torque(new_target_3)\n",
|
||
"\n",
|
||
" # Get the torque for the current cycle\n",
|
||
" current_cycle_torque = torque_controller.step()\n",
|
||
" output_torques.append(current_cycle_torque)\n",
|
||
" cycle_numbers.append(cycle)\n",
|
||
"\n",
|
||
" if cycle <= 15 or cycle >= simulation_total_cycles - 5 or \\\n",
|
||
" cycle == target_change_cycle_1 or cycle == target_change_cycle_1 + 1 or \\\n",
|
||
" cycle == target_change_cycle_2 or cycle == target_change_cycle_2 + 1 or \\\n",
|
||
" cycle == target_change_cycle_3 or cycle == target_change_cycle_3 + 1:\n",
|
||
" print(f\"{cycle:<5} | {torque_controller.target_torque:<6.1f} | \"\n",
|
||
" f\"{torque_controller.current_segment_duration_steps:<24} | \"\n",
|
||
" f\"{torque_controller.current_segment_step:<12} | \"\n",
|
||
" f\"{current_cycle_torque:<13.2f}\")\n",
|
||
"\n",
|
||
" print(f\"\\nFinal Torque after {simulation_total_cycles} cycles: {torque_controller.current_torque:.2f}\")\n",
|
||
"\n",
|
||
" # Basic plotting example (requires matplotlib)\n",
|
||
" try:\n",
|
||
" import matplotlib.pyplot as plt\n",
|
||
" plt.figure(figsize=(12, 6))\n",
|
||
" plt.plot(cycle_numbers, output_torques, marker='.', linestyle='-')\n",
|
||
" plt.title(\"Live Sigmoid Torque Ramp Simulation\")\n",
|
||
" plt.xlabel(\"Simulation Cycle\")\n",
|
||
" plt.ylabel(\"Torque\")\n",
|
||
" \n",
|
||
" # Add vertical lines for target changes\n",
|
||
" if target_change_cycle_1 < simulation_total_cycles:\n",
|
||
" plt.axvline(x=target_change_cycle_1, color='r', linestyle='--', label=f'Target -> {new_target_1:.0f}')\n",
|
||
" if target_change_cycle_2 < simulation_total_cycles:\n",
|
||
" plt.axvline(x=target_change_cycle_2, color='g', linestyle='--', label=f'Target -> {new_target_2:.0f}')\n",
|
||
" if target_change_cycle_3 < simulation_total_cycles:\n",
|
||
" plt.axvline(x=target_change_cycle_3, color='purple', linestyle='--', label=f'Target -> {new_target_3:.0f}')\n",
|
||
" \n",
|
||
" plt.legend()\n",
|
||
" plt.grid(True)\n",
|
||
" plt.tight_layout()\n",
|
||
" plt.show()\n",
|
||
" except ImportError:\n",
|
||
" print(\"\\nMatplotlib not installed. Skipping plot.\")\n"
|
||
]
|
||
},
|
||
{
|
||
"cell_type": "code",
|
||
"execution_count": null,
|
||
"id": "65cd4855-6f4f-4029-b728-75c09391bce6",
|
||
"metadata": {},
|
||
"outputs": [],
|
||
"source": []
|
||
}
|
||
],
|
||
"metadata": {
|
||
"kernelspec": {
|
||
"display_name": "Python 3 (ipykernel)",
|
||
"language": "python",
|
||
"name": "python3"
|
||
},
|
||
"language_info": {
|
||
"codemirror_mode": {
|
||
"name": "ipython",
|
||
"version": 3
|
||
},
|
||
"file_extension": ".py",
|
||
"mimetype": "text/x-python",
|
||
"name": "python",
|
||
"nbconvert_exporter": "python",
|
||
"pygments_lexer": "ipython3",
|
||
"version": "3.11.12"
|
||
}
|
||
},
|
||
"nbformat": 4,
|
||
"nbformat_minor": 5
|
||
}
|