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 15af75a..0000000 Binary files a/husky_mpc/scripts/utils.pyc and /dev/null differ 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 0000000..968bdde Binary files /dev/null and b/mpc_demo/__pycache__/cvxpy_mpc.cpython-35.pyc differ diff --git a/mpc_demo/__pycache__/utils.cpython-35.pyc b/mpc_demo/__pycache__/utils.cpython-35.pyc new file mode 100644 index 0000000..8a9d396 Binary files /dev/null and b/mpc_demo/__pycache__/utils.cpython-35.pyc differ 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": {