Skip to content

Commit

Permalink
feat: update example plots
Browse files Browse the repository at this point in the history
  • Loading branch information
@incebellipipo
incebellipipo committed Dec 13, 2024
1 parent 4ce160f commit a591f3c
Show file tree
Hide file tree
Showing 19 changed files with 2,801 additions and 93 deletions.
1 change: 1 addition & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
### Project

notebooks/data/*
notebooks/plots/*
!notebooks/data/.gitkeep

### CMake ###
Expand Down
161 changes: 144 additions & 17 deletions examples/cybership_jonny.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,11 @@
"outputs": [],
"source": [
"import skadipy\n",
"import numpy as np"
"import numpy as np\n",
"import ipywidgets as widgets\n",
"import matplotlib.pyplot as plt\n",
"# Use interactive backend\n",
"%matplotlib qt5"
]
},
{
Expand All @@ -64,15 +68,15 @@
" orientation=skadipy.toolbox.Quaternion(\n",
" axis=(0.0, 0.0, 1.0), radians=np.pi / 2.0),\n",
" extra_attributes={\n",
" \"rate_limit\": 0.1,\n",
" \"rate_limit\": 1.0,\n",
" \"saturation_limit\": 1.0,\n",
" \"name\": \"tunnel\",\n",
" }\n",
")\n",
"port_azimuth = skadipy.actuator.Azimuth(\n",
" position=skadipy.toolbox.Point([-0.4574, -0.055, 0.0]),\n",
" extra_attributes={\n",
" \"rate_limit\": 0.1,\n",
" \"rate_limit\": 1.0,\n",
" \"saturation_limit\": 1.0,\n",
" \"reference_angle\": np.pi / 4.0,\n",
" \"name\": \"port_azimuth\",\n",
Expand All @@ -81,7 +85,7 @@
"starboard_azimuth = skadipy.actuator.Azimuth(\n",
" position=skadipy.toolbox.Point([-0.4547, 0.055, 0.0]),\n",
" extra_attributes={\n",
" \"rate_limit\": 0.1,\n",
" \"rate_limit\": 1.0,\n",
" \"saturation_limit\": 1.0,\n",
" \"reference_angle\": - np.pi / 4.0,\n",
" \"name\": \"starboard_azimuth\",\n",
Expand Down Expand Up @@ -145,10 +149,10 @@
"allocator = skadipy.allocator.reference_filters.MinimumMagnitudeAndAzimuth(\n",
" actuators=actuators,\n",
" force_torque_components=dofs,\n",
" gamma=0.001,\n",
" mu=0.01,\n",
" rho=10,\n",
" time_step=0.01,\n",
" gamma=0.1,\n",
" mu=0.1,\n",
" rho=100,\n",
" time_step=0.1,\n",
" control_barrier_function=skadipy.safety.ControlBarrierFunctionType.ABSOLUTE\n",
")"
]
Expand Down Expand Up @@ -197,12 +201,12 @@
"name": "stdout",
"output_type": "stream",
"text": [
"Allocated [[1.00166667e+00]\n",
" [2.11758237e-20]\n",
"Allocated [[1.16666667e+00]\n",
" [8.67361738e-19]\n",
" [0.00000000e+00]\n",
" [0.00000000e+00]\n",
" [0.00000000e+00]\n",
" [7.43993201e-18]]\n"
" [0.00000000e+00]]\n"
]
}
],
Expand Down Expand Up @@ -238,18 +242,141 @@
"name": "stdout",
"output_type": "stream",
"text": [
"tunnel: [[-6.38889204e-08]]\n",
"port_azimuth: [[5.00833979e-01]\n",
" [6.38005511e-06]]\n",
"starboard_azimuth: [[ 5.00832688e-01]\n",
" [-6.31616619e-06]]\n"
"tunnel: [[2.01128555e-05]]\n",
"port_azimuth: [[0.5833353 ]\n",
" [0.00635386]]\n",
"starboard_azimuth: [[ 0.58333137]\n",
" [-0.00637397]]\n"
]
}
],
"source": [
"for actuator in actuators:\n",
" print(f\"{actuator.extra_attributes['name']}: {actuator.force}\")"
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "f497810500104c98b9c355c4a2638630",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"FloatSlider(value=1.0, description='F_x:', max=10.0, min=-10.0)"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "afa5ded7777144119790d099d1b718b7",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"FloatSlider(value=0.0, description='F_y:', max=10.0, min=-10.0)"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "f96abe416f354abe9806eed5249e081d",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"FloatSlider(value=1.0, description='M_z:', max=10.0, min=-10.0)"
]
},
"metadata": {},
"output_type": "display_data"
},
{
"data": {
"application/vnd.jupyter.widget-view+json": {
"model_id": "0a48b8c2b3454267bea7c6f5b57edc18",
"version_major": 2,
"version_minor": 0
},
"text/plain": [
"Button(description='Allocate', style=ButtonStyle())"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# Create sliders for F_x, F_y, and M_z\n",
"F_x_slider = widgets.FloatSlider(value=1.0, min=-10.0, max=10.0, step=0.1, description='F_x:')\n",
"F_y_slider = widgets.FloatSlider(value=0.0, min=-10.0, max=10.0, step=0.1, description='F_y:')\n",
"M_z_slider = widgets.FloatSlider(value=1.0, min=-10.0, max=10.0, step=0.1, description='M_z:')\n",
"\n",
"# Create a button to trigger the allocator\n",
"allocate_button = widgets.Button(description='Allocate')\n",
"\n",
"plt.ion()\n",
"fig, ax = plt.subplots(figsize=(10, 10))\n",
"\n",
"# Function to update tau_cmd and allocate forces\n",
"def allocate_forces(button):\n",
" global tau_cmd\n",
" tau_cmd[0, 0] = F_x_slider.value\n",
" tau_cmd[1, 0] = F_y_slider.value\n",
" tau_cmd[5, 0] = M_z_slider.value\n",
" allocator.allocate(tau=tau_cmd)\n",
"\n",
" positions = [actuator.position for actuator in actuators]\n",
" forces = [actuator.force for actuator in actuators]\n",
" # Correct size for tunnel thruster to be size of two\n",
"\n",
" # Find tunnel thrusters and correct the size of the forces\n",
" for i, actuator in enumerate(actuators):\n",
" if forces[i].shape[0] == 1:\n",
" forces[i] = np.concatenate((forces[i], np.zeros((1,1))), axis=0)\n",
" break\n",
"\n",
" # Extract X and Y components of positions and forces\n",
" x_positions = [pos.x for pos in positions]\n",
" y_positions = [pos.y for pos in positions]\n",
" x_forces = [force[0] for force in forces]\n",
" y_forces = [force[1] for force in forces]\n",
" # Normalize the forces for better visualization\n",
" scale_factor = 0.2\n",
" x_forces = [force * scale_factor for force in x_forces]\n",
" y_forces = [force * scale_factor for force in y_forces]\n",
"\n",
" # Create the plot with predefined limits\n",
" ax.cla()\n",
" ax.set_xlim(-0.5, 0.5)\n",
" ax.set_ylim(-0.5, 0.5)\n",
" ax.grid()\n",
" ax.set_xlabel('X Position')\n",
" ax.set_ylabel('Y Position')\n",
" ax.set_title('Thrust Directions and Magnitudes in X-Y Plane')\n",
"\n",
" ax.quiver(x_positions, y_positions, x_forces, y_forces, angles='xy', scale_units='xy', scale=1, color='r')\n",
" ax.scatter(x_positions, y_positions, color='b')\n",
" fig.canvas.draw_idle()\n",
"\n",
"\n",
"# Attach the function to the button\n",
"allocate_button.on_click(allocate_forces)\n",
"\n",
"# Display the widgets\n",
"display(F_x_slider, F_y_slider, M_z_slider, allocate_button)"
]
}
],
"metadata": {
Expand All @@ -268,7 +395,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.11.2"
"version": "3.11.5"
}
},
"nbformat": 4,
Expand Down
6 changes: 4 additions & 2 deletions examples/milliampere1.ipynb → examples/milliampere1_qp.ipynb
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,9 @@
"cell_type": "markdown",
"metadata": {},
"source": [
"# Control allocation example for MilliAmpere1"
"# Control allocation example for MilliAmpere1\n",
"\n",
"## Quadratic Programming based Control Allocation"
]
},
{
Expand Down Expand Up @@ -271,7 +273,7 @@
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.11.2"
"version": "3.11.5"
}
},
"nbformat": 4,
Expand Down
Loading

0 comments on commit a591f3c

Please sign in to comment.