Minor folder structure arranging

husky_mpc/CMakeLists.txt

@@ -0,0 +1,208 @@
+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)

husky_mpc/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>husky_mpc</name>
+  <version>0.0.0</version>
+  <description>The husky_mpc package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="marcello@todo.todo">marcello</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/husky_mpc</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

husky_mpc/scripts/cvxpy_mpc.py

@@ -0,0 +1,176 @@
+import numpy as np
+from scipy.integrate import odeint
+from scipy.interpolate import interp1d
+import cvxpy as cp
+
+def get_linear_model(x_bar,u_bar):
+    """
+    """
+
+    # Control problem statement.
+
+    N = 5 #number of state variables
+    M = 2 #number of control variables
+    T = 20 #Prediction Horizon
+    dt = 0.25 #discretization step
+
+    x = x_bar[0]
+    y = x_bar[1]
+    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)
+    B[2,1]=1
+    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):
+    """
+    Finds psi and cte w.r.t. the closest waypoint.
+
+    :param state: array_like, state of the vehicle [x_pos, y_pos, theta]
+    :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...), (th1 ,th2, ...)]
+    :returns: (float,float)
+    """
+
+    dx = state[0]-path[0,:]
+    dy = state[1]-path[1,:]
+    dist = np.sqrt(dx**2 + dy**2)
+    nn_idx = np.argmin(dist)
+
+    try:
+        v = [path[0,nn_idx+1] - path[0,nn_idx],
+             path[1,nn_idx+1] - path[1,nn_idx]]   
+        v /= np.linalg.norm(v)
+
+        d = [path[0,nn_idx] - state[0],
+             path[1,nn_idx] - state[1]]
+
+        if np.dot(d,v) > 0:
+            target_idx = nn_idx
+        else:
+            target_idx = nn_idx+1
+
+    except IndexError as e:
+        target_idx = nn_idx
+
+    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);
+    '''
+    :param starting_state:
+    :param u_bar:
+    :param track:
+    :returns:
+    '''
+
+    MAX_SPEED = 1.25
+    MIN_SPEED = 0.75
+    MAX_STEER_SPEED = 1.57/2
+
+    N = 5 #number of state variables
+    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]
+    x0[1] = starting_state[1]
+    x0[2] = starting_state[2]
+    _,psi,cte = calc_err(x0,track)
+    x0[3]=psi
+    x0[4]=cte
+
+    # Prediction
+    x_bar=np.zeros((N,T+1))
+    x_bar[:,0]=x0
+
+    for t in range (1,T+1):
+        xt=x_bar[:,t-1].reshape(5,1)
+        ut=u_bar[:,t-1].reshape(2,1)
+
+        A,B,C=get_linear_model(xt,ut)
+
+        xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)
+
+        _,psi,cte = calc_err(xt_plus_one,track)
+        xt_plus_one[3]=psi
+        xt_plus_one[4]=cte
+
+        x_bar[:,t]= xt_plus_one
+
+    #CVXPY Linear MPC problem statement
+    cost = 0
+    constr = []
+    x = cp.Variable((N, T+1))
+    u = cp.Variable((M, T))
+
+    for t in range(T):
+
+        # Tracking
+        if t > 0:
+            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)
+            delta_x = track[:,idx]-x[0:3,t]
+            cost+= cp.quad_form(delta_x,100*np.eye(3))
+
+        # 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 += [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

husky_mpc/scripts/mpc_node.py

@@ -0,0 +1,84 @@
+#! /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()

husky_mpc/scripts/utils.py

@@ -0,0 +1,33 @@
+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
+    :returns: array_like, of shape (3,N)
+    """
+
+    final_xp=[]
+    final_yp=[]
+    delta = step #[m]
+
+    for idx in range(len(start_xp)-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)