Skip to content

Instantly share code, notes, and snippets.

@sash13

sash13/crane_modeliing.ipynb

Created December 17, 2020 13:15
Show Gist options
  • Save sash13/7f258530b88bcdc8e7dd27ac06b88022 to your computer and use it in GitHub Desktop.
Save sash13/7f258530b88bcdc8e7dd27ac06b88022 to your computer and use it in GitHub Desktop.
Download ZIP
Display the source blob
Display the rendered blob
Raw
crane_modeliing.ipynb
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"metadata": {},
"outputs": [],
"source": [
"#https://github.com/ARCRL/gym-PendulumCraneControl/blob/master/pendulumCrane/envs/cartpole_v4.py\n",
"#https://www.researchgate.net/publication/305229654_Three-Dimensional_Crane_Modelling_and_Control_Using_Euler-Lagrange_State-Space_Approach_and_Anti-Swing_Fuzzy_Logic\n",
"#https://digikogu.taltech.ee/en/Download/080842ed-f6e0-4fb2-9390-f6121c27011f"
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from scipy import signal"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"mc = 1.155; # Mass of the cart5.\n",
"ml = 1; # Mass of the load6.\n",
"mr = 2.2; # Mass of the rail\n",
"l = 0.6; # Length of lift-line8.\n",
"g = 9.81; # Gravitational acceleration"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"A_ss = np.array([[0, 1, 0, 0, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, ml*g/(mc+mr+ml), 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 1, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, ml*g/(mc+ml), 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 1, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, -g*(ml+mc+mr+ml)/((mc+mr+ml)*l), 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 1, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, -g*(ml+mc+ml)/((mc+ml)*l), 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 1],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]]\n",
" )\n",
"\n",
"B_ss = np.array([[0, 0, 0],\n",
" [1/(mc+mr+ml), 0, 0],\n",
" [0, 0, 0],\n",
" [0, 1/(mc+ml), 0],\n",
" [0, 0, 0],\n",
" [-1/((mc+mr+ml)*l), 0, 0],\n",
" [0, 0, 0],\n",
" [0, -1/((mc+ml)*l), 0],\n",
" [0, 0, 0],\n",
" [0, 0, 1/(ml+ml)]]\n",
" )\n",
"\n",
"C_ss = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 1, 0, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 1, 0]]\n",
" )\n",
"\n",
"D_ss = np.zeros((5, 3))"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"StateSpaceDiscrete(\n",
"array([[ 1.00000000e+00, 2.00000000e-02, 0.00000000e+00,\n",
" 0.00000000e+00, 4.50214818e-04, 3.00223690e-06,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 1.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 4.49913069e-02, 4.50214818e-04,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,\n",
" 2.00000000e-02, 0.00000000e+00, 0.00000000e+00,\n",
" 9.09714626e-04, 6.06670047e-06, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 9.08988649e-02, 9.09714626e-04, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 9.95981833e-01, 1.99732050e-02,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, -4.01547414e-01, 9.95981833e-01,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 9.95216417e-01, 1.99680993e-02, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -4.77976531e-01, 9.95216417e-01, 0.00000000e+00,\n",
" 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 1.00000000e+00,\n",
" 2.00000000e-02],\n",
" [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 1.00000000e+00]]),\n",
"array([[ 4.59184795e-05, 0.00000000e+00, 0.00000000e+00],\n",
" [ 4.59127354e-03, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, 9.27839611e-05, 0.00000000e+00],\n",
" [ 0.00000000e+00, 9.27605050e-03, 0.00000000e+00],\n",
" [-7.64890958e-05, 0.00000000e+00, 0.00000000e+00],\n",
" [-7.64378302e-03, 0.00000000e+00, 0.00000000e+00],\n",
" [ 0.00000000e+00, -1.54555662e-04, 0.00000000e+00],\n",
" [ 0.00000000e+00, -1.54432322e-02, 0.00000000e+00],\n",
" [ 0.00000000e+00, 0.00000000e+00, 1.00000000e-04],\n",
" [ 0.00000000e+00, 0.00000000e+00, 1.00000000e-02]]),\n",
"array([[1, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 1, 0, 0, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 1, 0, 0, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 1, 0, 0, 0],\n",
" [0, 0, 0, 0, 0, 0, 0, 0, 1, 0]]),\n",
"array([[0., 0., 0.],\n",
" [0., 0., 0.],\n",
" [0., 0., 0.],\n",
" [0., 0., 0.],\n",
" [0., 0., 0.]]),\n",
"dt: 0.02\n",
")\n"
]
}
],
"source": [
"tau = 0.02\n",
"system = signal.StateSpace(A_ss, B_ss, C_ss, D_ss)\n",
"system = system.to_discrete(tau)\n",
"\n",
"print(system)"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Python 3",
"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.8.5"
}
},
"nbformat": 4,
"nbformat_minor": 4
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment