From 13935940b440b7cf61bf7b578e35b87b5b066486 Mon Sep 17 00:00:00 2001 From: mcarfagno Date: Mon, 13 Jan 2020 12:25:26 +0000 Subject: [PATCH] WIP: plot based simulation --- husky_mpc/CMakeLists.txt | 208 ------------------ husky_mpc/package.xml | 74 ------- husky_mpc/scripts/mpc_node.py | 84 ------- husky_mpc/scripts/utils.pyc | Bin 1529 -> 0 bytes mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc | Bin 0 -> 4325 bytes mpc_demo/__pycache__/utils.cpython-35.pyc | Bin 0 -> 1332 bytes {husky_mpc/scripts => mpc_demo}/cvxpy_mpc.py | 36 +-- mpc_demo/main.py | 91 ++++++++ {husky_mpc/scripts => mpc_demo}/utils.py | 12 +- .../MPC_cvxpy-checkpoint.ipynb | 7 - notebooks/MPC_cvxpy.ipynb | 7 - 11 files changed, 115 insertions(+), 404 deletions(-) delete mode 100644 husky_mpc/CMakeLists.txt delete mode 100644 husky_mpc/package.xml delete mode 100755 husky_mpc/scripts/mpc_node.py delete mode 100644 husky_mpc/scripts/utils.pyc create mode 100644 mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc create mode 100644 mpc_demo/__pycache__/utils.cpython-35.pyc rename {husky_mpc/scripts => mpc_demo}/cvxpy_mpc.py (94%) create mode 100755 mpc_demo/main.py rename {husky_mpc/scripts => mpc_demo}/utils.py (92%) diff --git a/husky_mpc/CMakeLists.txt b/husky_mpc/CMakeLists.txt deleted file mode 100644 index d3ea737..0000000 --- a/husky_mpc/CMakeLists.txt +++ /dev/null @@ -1,208 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(husky_mpc) - -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# geometry_msgs# navigation_msgs# std_msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES husky_mpc -# CATKIN_DEPENDS geometry_msgs navigation_msgs roscpp rospy std_msgs -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( -# include - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/husky_mpc.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/husky_mpc_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_husky_mpc.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/husky_mpc/package.xml b/husky_mpc/package.xml deleted file mode 100644 index a4c6bc6..0000000 --- a/husky_mpc/package.xml +++ /dev/null @@ -1,74 +0,0 @@ - - - husky_mpc - 0.0.0 - The husky_mpc package - - - - - marcello - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - geometry_msgs - nav_msgs - roscpp - rospy - std_msgs - - - - - - - - diff --git a/husky_mpc/scripts/mpc_node.py b/husky_mpc/scripts/mpc_node.py deleted file mode 100755 index f60dedb..0000000 --- a/husky_mpc/scripts/mpc_node.py +++ /dev/null @@ -1,84 +0,0 @@ -#! /usr/bin/env python - -import rospy -import numpy as np - -from nav_msgs.msg import Odometry - -from geometry_msgs.msg import Twist - -from utils import compute_path_from_wp -from cvxpy_mpc import optimize - -# classes -class Node(): - - def __init__(self): - - rospy.init_node('mpc_node') - - N = 5 #number of state variables - M = 2 #number of control variables - T = 20 #Prediction Horizon - dt = 0.25 #discretization step - - # State for the robot mathematical model - self.state = None - - # starting guess output - self.opt_u = np.zeros((M,T)) - self.opt_u[0,:] = 1 #m/s - self.opt_u[1,:] = np.radians(0) #rad/s - - # Interpolated Path to follow given waypoints - self.path = compute_path_from_wp([0,20,30,30],[0,0,10,20]) - - self._cmd_pub = rospy.Publisher(rospy.get_namespace() + 'husky_velocity_controller/cmd_vel', Twist, queue_size=10) - self._odom_sub = rospy.Subscriber(rospy.get_namespace() +'husky_velocity_controller/odom', Odometry, self._odom_cb, queue_size=1) - - def run(self): - while 1: - if self.state is not None: - - #optimization loop - self.opt_u = optimize(self.state, - self.opt_u, - self.path) - - msg = Twist() - msg.linear.x=self.opt_u[0,1] - msg.angular.z=self.opt_u[0,1] - - self._cmd_pub(msg) - - def _odom_cb(self,odom): - ''' - Updates state with latest odometry. - - :param odom: nav_msgs.msg.Odometry - ''' - - state = np.zeros(3) - - # Update current position - state[0] = odom.pose.pose.position.x - state[1] = odom.pose.pose.position.y - - # Update current orientation - _, _, state[2] = euler_from_quaternion( - [odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z, - odom.pose.pose.orientation.w]) - - self.state = state - -def main(): - ros_node=Node() - try: - ros_node.run() - except rospy.exceptions.ROSException as e: - sys.exit(e) - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/husky_mpc/scripts/utils.pyc b/husky_mpc/scripts/utils.pyc deleted file mode 100644 index 15af75ad231d0e8ecb4131bbb0cc298bef2b411b..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1529 zcmbtUPmdcl6n~ycve|43R4Qs=L2_@YwAu)ItEy574shASazIL1jm(TEad*b!+Ri2; zB^Q=2QN9UZi7Oufe)c5YU7+HCGk*5_>)(6NPWs2a(Ww3XBB$L*u)f4}-+~nQ7*#|w z7WWBN3DqghQer`uQq`lnPqTiQ9Wb3B%S0zA2>A zzT<~Muf4S9i(>xk+c19nDo-K(0CR--HKzL+&U2z|8flL<32pnde#|^#GTIPb11O_M zSiZS9WZMDxgkGnS_9E>^ItaEK&;b<*J$xM861qUP;^XQmvscl=DL8k~=RAe^pqDW&VQ1AC8b6f?PR|6>PqsJXk*ShKvw#t)$VR4A&G@Bl24wVoIQ!iRd^>Vyhepd zz6Wxtba6I03Tz)FmfD9$Qb~=z5@{1zx2&VRdt-gHG|fudC@NH0;&#GCX=g+(&)>aA zVT{y;KY&u$94+~B5*}Z)-Z$VD)0*xKG%ZR?l)XZaZZR+kI15>M5rL^Eg+$7A%%^PQ0rpE zpDvc}vdwCfM-E-7+^K2(1NiMOX=a39WTkEDY-PfKllv5eMgxXl@;Uo4`6QXLY4V7D zmK?KVHaUbgg3-&`wC5p9{~?IWm1$2yI&+Ilcn%Ty`*#%9gsZcwdvMIJ)<6>y%+2{R emYdxEUxGu#dR{lhvXU=Cmt7Ey*+=+|lfMAFXliQ! diff --git a/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc b/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..968bddefcc00d02ea44098f2d41bcef388a3fb02 GIT binary patch literal 4325 zcmcIn&2t-B5$`u2`u4YMOL3B&C9rF6j7=7*0GEr9O}tfGOR~#mYZGP(Vxk$#vZayo zX5>gN2@YZZ82$tJFDQ<~g(@z%xNzVAdx2l~NU|Jfk4T!Qe*OCO>+aWIvo<$ZNWQrK zD{GnPZ#45U0RI$E^lJ=0eu`Q|t`gXwmO*WkT$2>9nbfkVZIf%KbI@kU&601CZ`1Fv zFGqfc{H(xv@-fW|TwunJw+iPtg&u9GB-8POW*7s?;V;nLJAR#I;m-=59G?4lqQ5E; zYGjaa(8y#VMmWx*kxfG^*;E~61j+!)3S!BxXmLuG9?h#ukO-$OPRVn`J-7skng*y}~6di7HFN7zT&2pydn!Xez8Y)*xcK85bv;d96HJrr|sd z7iqXmhbH`D`OGdj_?+9g@^kis^D>489(?|ESHlVDTn3XN3&WNnSj}4iXYn_I^9-{G zO=x)Mgk}u$HraIkUwp90^nw5^41_S37-2WD&jQ_v5;JOsiCJeT7zBy!Y3&V?tPY|h zuNx$$--)+Y61!h}?&-wt$)9?ZsE3LAJTd$@QT;>>5<5N$VlOefQ4`$aAW^4@dYPzu zu6?i8!V&5N*ZzPZ*E+2Ad#-(-C%-5CEy(!w-tJMS9qhI}T@PBV&hBxqJ3s{0UPim^ zZoP)dQ7078589pG`f0yAs9|NNJ4hBALCm}YPuJQALBua&><1VqlTq?D3Tpgi$XHbs zwayQq6?}QE4E$sDfuX;Rebca=!-PXu!Zv~RI6M_qIp*+_0RBostXe@h^QxJy!Z|Ll znimm*NM9L(0DtRnM8rP37PeawsWuJ%D6uT@1%~Sg;qj3O0iIM?00gYf<8)aNvfO}6 z!+SHr*pzsMt17y|Sdo3qXspi#KRGH@qrzOW!UC}d&lhip-X|96vH}NWq!0-xl>(C| z;cc?w44o86|4b>QDRqXlB!o|Xks1WZ-{k3OsOaf9pdb(ONlp}F6f&$u8*CY#A=P~| z0ZW)>z$pjT$byBC4|zKzmwZr#!Er_vJB9@bc_arURESqf1J*wKS_1aNX6Q!_9NF(j&5tzT}lccg?`XK(7L1bJR^*5Q*+pC%_X+jBe7Uw?KG&z`Z}oe4IVgKq{A)VnX7xPdM(iU zyO>3r7$|>J-BcTD!7$ZZ>N{!;F}`6`RNk03mQ~ewTfJ*mEXUCAgSO6@mp?4Vv^;-; zJPI%8!VLF9VP4_Fj2esJfomZokoV#3=n3G=>|_X32`~0V?1h`b$MCn$vwS+^LYulK zuUV*Y;)z^G4f-WIfFhxAbD4%?f`ThBCV<8@R}O*8f+0sdCk_-GoH);Q6+a{*oQb9V z{y!;(^a!1l949<@_7@tVbj=WRRn1d{BOV1E`e6o2lvj?p3Cq{0K$udYRwA}QyEIW? znMSZwSuB!f9^?#^#YHAyGhwWpSWs=)u|l-!46D#Sp!!0#h>X3 zG#{;Tbseo!oRhSHRCmc*mB|9+5IeC6tJMFo0b>D!>4Rst4Ym?2lan;}TZ6^Ba%A=5 z+OEmXA&tQLQ=46rWHziY8{8yr1}xdomC*(b*F+H9+y=KlY?&fc;rucz^+V_ZY6x^< zkS6p0EI``1y;%Tuj}rvkm*$1c)fqL#fztAG87-=!nM-v;?U>bVs{YkIIPgtd*wfx{ z2baB+&xH9nlqQ}yTqnMR>9x$r3%|U)!rRO2pI4~*s)4#5#N$gdbjl6{{VvyTu1-_ literal 0 HcmV?d00001 diff --git a/mpc_demo/__pycache__/utils.cpython-35.pyc b/mpc_demo/__pycache__/utils.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8a9d396a5a85d91990d21b754879e907d42f27ba GIT binary patch literal 1332 zcmah}Pj3`A6o2;2Y?jRiwFnAnQpv}z6tW^n91vV-l7+v_&^<6Lb;|1Ag&qVpCkGuS&zVAThQ6SHq@ zA$sz8-are6qGWaL3y6Q$aETbi1lM4RaZ&a|Mc4uI6B6SNQZ%@-pTg@AJYU%n;1HgeoxF|w}P@vE8c@V}7H?YPlJjvv!jMODbA z9V3yoJ=C4Qd2yj~&D}IFP21#ijlU|e`zT=0Blyto$V<9QkH`_dL-y$&*&{co*+cs% w^O>48x-)(JntW}GN_U5s=eg!8y2d)Mh;I?#`;VGd%xd{VI1wB24#`*aZ@ha~hX4Qo literal 0 HcmV?d00001 diff --git a/husky_mpc/scripts/cvxpy_mpc.py b/mpc_demo/cvxpy_mpc.py similarity index 94% rename from husky_mpc/scripts/cvxpy_mpc.py rename to mpc_demo/cvxpy_mpc.py index 666f7a9..abdbd9b 100755 --- a/husky_mpc/scripts/cvxpy_mpc.py +++ b/mpc_demo/cvxpy_mpc.py @@ -19,16 +19,16 @@ def get_linear_model(x_bar,u_bar): theta = x_bar[2] psi = x_bar[3] cte = x_bar[4] - + v = u_bar[0] w = u_bar[1] - + A = np.zeros((N,N)) A[0,2]=-v*np.sin(theta) A[1,2]=v*np.cos(theta) A[4,3]=v*np.cos(-psi) A_lin=np.eye(N)+dt*A - + B = np.zeros((N,M)) B[0,0]=np.cos(theta) B[1,0]=np.sin(theta) @@ -36,10 +36,10 @@ def get_linear_model(x_bar,u_bar): B[3,1]=-1 B[4,0]=np.sin(-psi) B_lin=dt*B - + f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1) C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1))) - + return A_lin,B_lin,C_lin def calc_err(state,path): @@ -58,7 +58,7 @@ def calc_err(state,path): try: v = [path[0,nn_idx+1] - path[0,nn_idx], - path[1,nn_idx+1] - path[1,nn_idx]] + path[1,nn_idx+1] - path[1,nn_idx]] v /= np.linalg.norm(v) d = [path[0,nn_idx] - state[0], @@ -74,17 +74,17 @@ def calc_err(state,path): path_ref_vect = [np.cos(path[2,target_idx] + np.pi / 2), np.sin(path[2,target_idx] + np.pi / 2)] - + #heading error w.r.t path frame psi = path[2,target_idx] - state[2] - + # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor #cte = np.dot([dx[target_idx], dy[target_idx]],front_axle_vect) cte = np.dot([dx[target_idx], dy[target_idx]],path_ref_vect) return target_idx,psi,cte -def optimize(starting_state,u_bar,track); +def optimize(starting_state,u_bar,track): ''' :param starting_state: :param u_bar: @@ -100,7 +100,7 @@ def optimize(starting_state,u_bar,track); M = 2 #number of control variables T = 20 #Prediction Horizon dt = 0.25 #discretization step - + #Starting Condition x0 = np.zeros(N) x0[0] = starting_state[0] @@ -141,7 +141,7 @@ def optimize(starting_state,u_bar,track); idx,_,_ = calc_err(x_bar[:,t],track) delta_x = track[:,idx]-x[0:3,t] cost+= cp.quad_form(delta_x,10*np.eye(3)) - + # Tracking last time step if t == T: idx,_,_ = calc_err(x_bar[:,t],track) @@ -151,26 +151,26 @@ def optimize(starting_state,u_bar,track); # Actuation rate of change if t < (T - 1): cost += cp.quad_form(u[:, t + 1] - u[:, t], 25*np.eye(M)) - + # Actuation effort cost += cp.quad_form( u[:, t],1*np.eye(M)) - + # Constrains A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t]) constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()] # sums problem objectives and concatenates constraints. - constr += [x[:,0] == x_sim[:,sim_time]] # starting condition + constr += [x[:,0] == x0] # starting condition constr += [u[0, :] <= MAX_SPEED] constr += [u[0, :] >= MIN_SPEED] constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED] - + # Solve prob = cp.Problem(cp.Minimize(cost), constr) solution = prob.solve(solver=cp.ECOS, verbose=False) - + #retrieved optimized U and assign to u_bar to linearize in next step u_bar=np.vstack((np.array(u.value[0, :]).flatten(), (np.array(u.value[1, :]).flatten()))) - - return u_bar \ No newline at end of file + + return u_bar diff --git a/mpc_demo/main.py b/mpc_demo/main.py new file mode 100755 index 0000000..1413014 --- /dev/null +++ b/mpc_demo/main.py @@ -0,0 +1,91 @@ +#! /usr/bin/env python + +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import animation + +from utils import compute_path_from_wp +from cvxpy_mpc import optimize + +import sys +import time + +# classes +class MPC(): + + def __init__(self): + + # State for the robot mathematical model [x,y,heading] + self.state = np.zeros(3) + # Sim step + self.dt = 0.25 + + # starting guess output + N = 5 #number of state variables + M = 2 #number of control variables + T = 20 #Prediction Horizon + self.opt_u = np.zeros((M,T)) + self.opt_u[0,:] = 1 #m/s + self.opt_u[1,:] = np.radians(0) #rad/s + + # Interpolated Path to follow given waypoints + self.path = compute_path_from_wp([0,20,30,30],[0,0,10,20]) + + #Initialise plot + # First set up the figure, the axis, and the plot element we want to animate + plt.style.use("ggplot") + self.fig = plt.figure() + plt.ion() + plt.show() + + def run(self): + ''' + ''' + + while 1: + if self.state is not None: + + #optimization loop + start=time.time() + self.opt_u = optimize(self.state, + self.opt_u, + self.path) + + print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start)) + + self.update_sim(self.opt_u[0,1],self.opt_u[1,1]) + + self.plot_sim() + + def update_sim(self,lin_v,ang_v): + ''' + Updates state. + + :param lin_v: float + :param ang_v: float + ''' + + self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt + self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt + self.state[2] = self.state[2] +ang_v*self.dt + + def plot_sim(self): + plt.clf() + self.ax = plt.axes(xlim=(np.min(self.path[0,:])-1, np.max(self.path[0,:])+1), + ylim=(np.min(self.path[1,:])-1, np.max(self.path[1,:])+1)) + + self.track, = self.ax.plot(self.path[0,:],self.path[1,:], "g-", label="reference track") + self.vehicle, = self.ax.plot([self.state[0]], [self.state[1]], "r*", label="vehicle path") + plt.legend() + plt.draw() + plt.pause(0.1) + +def do_sim(): + sim=MPC() + try: + sim.run() + except Exception as e: + sys.exit(e) + +if __name__ == '__main__': + do_sim() diff --git a/husky_mpc/scripts/utils.py b/mpc_demo/utils.py similarity index 92% rename from husky_mpc/scripts/utils.py rename to mpc_demo/utils.py index c978f42..d9916bd 100755 --- a/husky_mpc/scripts/utils.py +++ b/mpc_demo/utils.py @@ -1,12 +1,10 @@ import numpy as np -from scipy.integrate import odeint from scipy.interpolate import interp1d -import cvxpy as cp def compute_path_from_wp(start_xp, start_yp, step = 0.1): """ Interpolation range is computed to assure one point every fixed distance step [m]. - + :param start_xp: array_like, list of starting x coordinates :param start_yp: array_like, list of starting y coordinates :param step: float, interpolation distance [m] between consecutive waypoints @@ -21,13 +19,15 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1): section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2))) interp_range = np.linspace(0,1,section_len/delta) - + fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1) fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1) - + final_xp=np.append(final_xp,fx(interp_range)) final_yp=np.append(final_yp,fy(interp_range)) - + dx = np.append(0, np.diff(final_xp)) dy = np.append(0, np.diff(final_yp)) theta = np.arctan2(dy, dx) + + return np.vstack((final_xp,final_yp,theta)) diff --git a/notebooks/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb b/notebooks/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb index db4ff85..cbdf593 100644 --- a/notebooks/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb +++ b/notebooks/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb @@ -1158,13 +1158,6 @@ "plt.tight_layout()\n", "plt.show()" ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] } ], "metadata": { diff --git a/notebooks/MPC_cvxpy.ipynb b/notebooks/MPC_cvxpy.ipynb index db4ff85..cbdf593 100644 --- a/notebooks/MPC_cvxpy.ipynb +++ b/notebooks/MPC_cvxpy.ipynb @@ -1158,13 +1158,6 @@ "plt.tight_layout()\n", "plt.show()" ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] } ], "metadata": {