├── .gitignore ├── CMakeLists.txt ├── FindACADO.cmake ├── README.md ├── action ├── move.action └── traj.action ├── config └── predictive_config_parameter.yaml ├── include └── predictive_control │ ├── mpcc_controller.h │ ├── predictive_configuration.h │ ├── predictive_controller.h │ └── predictive_trajectory_generator.h ├── launch └── predictive_controller.launch ├── matlab ├── relative_motion.m └── relative_motion2.m ├── package.xml ├── predictive_control.rosinstall ├── rviz_config └── rviz_config.rviz ├── src ├── mpcc_controller.cpp ├── predictive_configuration.cpp ├── predictive_control_node.cpp └── predictive_trajectory_generator.cpp └── srv ├── GetFrameTrackingInfo.srv ├── StaticCollisionObject.srv └── StaticObstacle.srv /.gitignore: -------------------------------------------------------------------------------- 1 | logs/ 2 | devel/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNOREdevel/ 52 | logs/ 53 | build/ 54 | bin/ 55 | lib/ 56 | msg_gen/ 57 | srv_gen/ 58 | msg/*Action.msg 59 | msg/*ActionFeedback.msg 60 | msg/*ActionGoal.msg 61 | msg/*ActionResult.msg 62 | msg/*Feedback.msg 63 | msg/*Goal.msg 64 | msg/*Result.msg 65 | msg/_*.py 66 | build_isolated/ 67 | devel_isolated/ 68 | 69 | # Generated by dynamic reconfigure 70 | *.cfgc 71 | /cfg/cpp/ 72 | /cfg/*.py 73 | 74 | # Ignore generated docs 75 | *.dox 76 | *.wikidoc 77 | 78 | # eclipse stuff 79 | .project 80 | .cproject 81 | 82 | # qcreator stuff 83 | CMakeLists.txt.user 84 | 85 | srv/_*.py 86 | *.pcd 87 | *.pyc 88 | qtcreator-* 89 | *.user 90 | 91 | /planning/cfg 92 | /planning/docs 93 | /planning/src 94 | 95 | *~ 96 | 97 | # Emacs 98 | .#* 99 | 100 | # Catkin custom files 101 | CATKIN_IGNORE 102 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(predictive_control) 3 | 4 | add_compile_options(-std=c++11) 5 | find_package(catkin REQUIRED COMPONENTS 6 | actionlib 7 | actionlib_msgs 8 | cmake_modules 9 | control_msgs 10 | eigen_conversions 11 | geometry_msgs 12 | kdl_conversions 13 | kdl_parser 14 | moveit_core 15 | nav_msgs 16 | roscpp 17 | roslint 18 | sensor_msgs 19 | std_msgs 20 | tf 21 | tf_conversions 22 | trajectory_msgs 23 | urdf 24 | visualization_msgs 25 | shape_msgs 26 | moveit_msgs 27 | obstacle_feed 28 | ) 29 | 30 | find_package(Boost REQUIRED COMPONENTS thread) 31 | find_package(Eigen REQUIRED) 32 | 33 | find_package(ACADO REQUIRED) 34 | 35 | add_definitions(${EIGEN_DEFINITIONS}) 36 | 37 | find_package(orocos_kdl REQUIRED) 38 | 39 | ## Generate messages in the 'action' folder 40 | add_action_files( 41 | DIRECTORY 42 | action 43 | FILES 44 | traj.action 45 | move.action 46 | ) 47 | 48 | ## Generate messages in the 'msg' folder 49 | #add_message_files( 50 | # DIRECTORY 51 | # msg 52 | # FILES 53 | #) 54 | 55 | ## Generate services in the 'srv' folder 56 | add_service_files( 57 | DIRECTORY 58 | srv 59 | FILES 60 | ) 61 | 62 | # Generate added messages and services with any dependencies listed here 63 | generate_messages( 64 | DEPENDENCIES 65 | actionlib_msgs 66 | geometry_msgs 67 | sensor_msgs 68 | std_msgs 69 | shape_msgs 70 | visualization_msgs 71 | moveit_msgs 72 | obstacle_feed 73 | ) 74 | 75 | catkin_package( 76 | CATKIN_DEPENDS actionlib_msgs dynamic_reconfigure eigen_conversions geometry_msgs kdl_conversions kdl_parser nav_msgs roscpp sensor_msgs std_msgs tf tf_conversions urdf visualization_msgs shape_msgs moveit_core obstacle_feed 77 | DEPENDS Boost ACADO 78 | INCLUDE_DIRS include ${ACADO_INCLUDE_DIRS} include/${PROJECT_NAME} 79 | LIBRARIES predictive_configuration predictive_trajectory_generator predictive_controller 80 | ) 81 | 82 | ### BUILD ### 83 | include_directories(include 84 | ${catkin_INCLUDE_DIRS} 85 | ${EIGEN_INCLUDE_DIRS} 86 | ${orocos_kdl_INCLUDE_DIRS} 87 | ${ACADO_INCLUDE_DIRS} 88 | include/${PROJECT_NAME} 89 | #${ACADO_INCLUDE_PACKAGES} 90 | ) 91 | 92 | FIND_LIBRARY(libacado libacado_toolkit_s.so ${ACADO_DIR}/build/lib) 93 | 94 | add_library(predictive_configuration src/predictive_configuration.cpp) 95 | add_dependencies(predictive_configuration ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 96 | target_link_libraries(predictive_configuration 97 | ${catkin_LIBRARIES} 98 | ${orocos_kdl_LIBRARIES} 99 | ${libacado} 100 | ${CASADI_LIBRARIES} 101 | ) 102 | 103 | add_library(predictive_trajectory_generator src/predictive_trajectory_generator.cpp) 104 | add_dependencies(predictive_trajectory_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 105 | target_link_libraries(predictive_trajectory_generator 106 | predictive_configuration 107 | ${catkin_LIBRARIES} 108 | ${orocos_kdl_LIBRARIES} 109 | ${libacado} 110 | ) 111 | 112 | add_library(predictive_controller src/mpcc_controller.cpp) 113 | add_dependencies(predictive_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 114 | target_link_libraries(predictive_controller 115 | predictive_configuration 116 | predictive_trajectory_generator 117 | ${catkin_LIBRARIES} 118 | ${orocos_kdl_LIBRARIES} 119 | ${libacado} 120 | ) 121 | 122 | add_executable(predictive_control_node src/predictive_control_node.cpp) 123 | add_dependencies(predictive_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 124 | target_link_libraries(predictive_control_node 125 | predictive_controller 126 | ${catkin_LIBRARIES} 127 | ) 128 | 129 | ### Test Case #### 130 | 131 | install( 132 | DIRECTORY include/predictive_control/ 133 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 134 | ) 135 | 136 | install( 137 | TARGETS 138 | predictive_trajectory_generator 139 | predictive_controller 140 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 141 | ) 142 | -------------------------------------------------------------------------------- /FindACADO.cmake: -------------------------------------------------------------------------------- 1 | ################################################################################ 2 | # 3 | # Description: 4 | # ACADO Toolkit package configuration file 5 | # 6 | # Defines: 7 | # - Variable: ACADO_INCLUDE_DIRS 8 | # - Variable: ACADO_LIBRARY_DIRS 9 | # - Variable: ACADO_STATIC_LIBS_FOUND 10 | # - Variable: ACADO_STATIC_LIBRARIES 11 | # - Variable: ACADO_SHARED_LIBS_FOUND 12 | # - Variable: ACADO_SHARED_LIBRARIES 13 | # 14 | # Authors: 15 | # Milan Vukov, milan.vukov@esat.kuleuven.be 16 | # 17 | # Year: 18 | # 2011 - 2013. 19 | # 20 | # NOTE: 21 | # - This script is for Linux/Unix use only. 22 | # - Use this script only (and only :)) if you do not want to install ACADO 23 | # toolkit. If you install ACADO toolkit you do not need this script. 24 | # 25 | # - PREREQUISITE: sourced acado_env.sh in your ~/.bashrc file. This script 26 | # will try to find ACADO folders, libraries etc., but looking for them 27 | # in enviromental variables. 28 | # 29 | # Usage: 30 | # - Linux/Unix: TODO 31 | # 32 | ################################################################################ 33 | 34 | ################################################################################ 35 | # 36 | # Search for package components 37 | # 38 | ################################################################################ 39 | 40 | MESSAGE( STATUS "********************************************************************************" ) 41 | MESSAGE( STATUS "---> Looking for ACADO toolkit:" ) 42 | 43 | # 44 | # Include folders 45 | # 46 | SET( ACADO_INCLUDE_DIRS $ENV{ACADO_ENV_INCLUDE_DIRS} ) 47 | IF( ACADO_INCLUDE_DIRS ) 48 | MESSAGE( STATUS "Found ACADO toolkit include directories: ${ACADO_INCLUDE_DIRS}" ) 49 | SET( ACADO_INCLUDE_DIRS_FOUND TRUE ) 50 | ELSE( ACADO_INCLUDE_DIRS ) 51 | MESSAGE( STATUS "Could not find ACADO toolkit include directories" ) 52 | ENDIF( ACADO_INCLUDE_DIRS ) 53 | 54 | # 55 | # Library folders 56 | # 57 | SET( ACADO_LIBRARY_DIRS $ENV{ACADO_ENV_LIBRARY_DIRS} ) 58 | IF( ACADO_LIBRARY_DIRS ) 59 | MESSAGE( STATUS "Found ACADO toolkit library directories: ${ACADO_LIBRARY_DIRS}" ) 60 | SET( ACADO_LIBRARY_DIRS_FOUND TRUE ) 61 | ELSE( ACADO_LIBRARY_DIRS ) 62 | MESSAGE( STATUS "Could not find ACADO toolkit library directories" ) 63 | ENDIF( ACADO_LIBRARY_DIRS ) 64 | 65 | # 66 | # Static libs 67 | # 68 | 69 | SET ( STATIC_TMP $ENV{ACADO_ENV_STATIC_LIBRARIES}) 70 | IF ( NOT STATIC_TMP ) 71 | MESSAGE( STATUS "Could not find ACADO static library." ) 72 | SET( ACADO_STATIC_LIBS_FOUND FALSE ) 73 | ELSE() 74 | SET( ACADO_STATIC_LIBS_FOUND TRUE ) 75 | UNSET( ACADO_STATIC_LIBRARIES ) 76 | FOREACH( LIB $ENV{ACADO_ENV_STATIC_LIBRARIES} ) 77 | UNSET( ACADO_TOOLKIT_STATIC_${LIB} ) 78 | 79 | FIND_LIBRARY( ACADO_TOOLKIT_STATIC_${LIB} 80 | NAMES ${LIB} 81 | PATHS ${ACADO_LIBRARY_DIRS} 82 | NO_DEFAULT_PATH 83 | ) 84 | 85 | IF( ACADO_TOOLKIT_STATIC_${LIB} ) 86 | MESSAGE( STATUS "Found ACADO static library: ${LIB}" ) 87 | SET( ACADO_STATIC_LIBRARIES 88 | ${ACADO_STATIC_LIBRARIES} ${ACADO_TOOLKIT_STATIC_${LIB}} 89 | ) 90 | ELSE( ) 91 | MESSAGE( STATUS "Could not find ACADO static library: ${LIB}" ) 92 | SET( ACADO_STATIC_LIBS_FOUND FALSE ) 93 | ENDIF( ) 94 | ENDFOREACH() 95 | ENDIF() 96 | 97 | SET( ACADO_BUILD_STATIC ${ACADO_STATIC_LIBS_FOUND} ) 98 | 99 | IF( VERBOSE ) 100 | MESSAGE( STATUS "Static libraries: ${ACADO_STATIC_LIBRARIES}\n" ) 101 | ENDIF() 102 | 103 | # 104 | # Shared libs 105 | # 106 | SET( SHARED_TMP $ENV{ACADO_ENV_SHARED_LIBRARIES} ) 107 | IF ( NOT SHARED_TMP ) 108 | MESSAGE( STATUS "Could not find ACADO shared library." ) 109 | SET( ACADO_SHARED_LIBS_FOUND FALSE ) 110 | ELSE() 111 | SET( ACADO_SHARED_LIBS_FOUND TRUE ) 112 | UNSET( ACADO_SHARED_LIBRARIES ) 113 | FOREACH( LIB $ENV{ACADO_ENV_SHARED_LIBRARIES} ) 114 | UNSET( ACADO_TOOLKIT_SHARED_${LIB} ) 115 | 116 | FIND_LIBRARY( ACADO_TOOLKIT_SHARED_${LIB} 117 | NAMES ${LIB} 118 | PATHS ${ACADO_LIBRARY_DIRS} 119 | NO_DEFAULT_PATH 120 | ) 121 | 122 | IF( ACADO_TOOLKIT_SHARED_${LIB} ) 123 | MESSAGE( STATUS "Found ACADO shared library: ${LIB}" ) 124 | SET( ACADO_SHARED_LIBRARIES 125 | ${ACADO_SHARED_LIBRARIES} ${ACADO_TOOLKIT_SHARED_${LIB}} 126 | ) 127 | ELSE( ) 128 | MESSAGE( STATUS "Could not find ACADO shared library: ${LIB}" ) 129 | SET( ACADO_SHARED_LIBS_FOUND FALSE ) 130 | ENDIF( ) 131 | ENDFOREACH() 132 | ENDIF() 133 | 134 | SET( ACADO_BUILD_SHARED ${ACADO_SHARED_LIBS_FOUND} ) 135 | 136 | IF( VERBOSE ) 137 | MESSAGE( STATUS "${ACADO_SHARED_LIBRARIES}\n" ) 138 | ENDIF() 139 | 140 | # 141 | # qpOASES embedded source files and include folders 142 | # TODO: checks 143 | # 144 | SET( ACADO_QPOASES_EMBEDDED_SOURCES $ENV{ACADO_ENV_QPOASES_EMBEDDED_SOURCES} ) 145 | SET( ACADO_QPOASES_EMBEDDED_INC_DIRS $ENV{ACADO_ENV_QPOASES_EMBEDDED_INC_DIRS} ) 146 | 147 | 148 | # 149 | # And finally set found flag... 150 | # 151 | IF( ACADO_INCLUDE_DIRS_FOUND AND ACADO_LIBRARY_DIRS_FOUND 152 | AND (ACADO_STATIC_LIBS_FOUND OR ACADO_SHARED_LIBS_FOUND) ) 153 | SET( ACADO_FOUND TRUE ) 154 | ENDIF() 155 | 156 | MESSAGE( STATUS "********************************************************************************" ) 157 | 158 | 159 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This repository contains a MPC regulator for Jackal. 2 | 3 | # Singularity image installing all dependencies 4 | singularity pull shub://bbrito/singularity_images:2.2 5 | 6 | # Build SIngularity Image 7 | 8 | # Run singularity image 9 | sudo singularity shell --writable mpc.img 10 | 11 | # Source ros instalation 12 | source /opt/ros/kinetic/setup.bash 13 | 14 | # Install ACADO 15 | cd /home/bdebrito_sing/ACADOtoolkit/build 16 | make install 17 | 18 | # Go to code folder 19 | cd /home/bdebrito_sing/catkin_ws 20 | catkin_make 21 | source devel/setup.bash 22 | 23 | # Running instructions 24 | This contains the necessary information to get the MPC regulator for jackal working in simulation. Launching requires three or four steps. Launching the simulator in gazebo, launching the moveit move group, optionally launching the obstacle publisher and finally launching the mpc controller node. This can be done with to the following commands: 25 | 26 | - roslaunch jackal_gazebo jackal_world.launch 27 | - roslaunch jackal_moveit_description move_group.launch 28 | - (roslaunch obstacle_feed obstacle_feed.launch) 29 | - roslaunch predictive_control predictive_controller.launch 30 | 31 | The move group will wait for the mpc controller to be launched, after which a rviz instance with a specific configuration will be launched. Both the obstacle_feed and predictive_controller node contain a configuration file in a config directory. Several parameters can be set in this file. 32 | 33 | The MPC regulator is called with a reference trajectory using an action service. Currently, the MPC node only uses the end position as goal. MoveIt is used to interface with the action service and is set up such, that is sends a planned motion as reference in the action service call. 34 | 35 | Withing the predictive controller node source files, mpcc_controller.cpp contains the controller class that is launched by predictive_control_node.cpp. This controller class iteratively calls the pregenerated mpc solver. Within the file predictive_trajectory_generator.cpp, the MPC controller is defined using ACADO, which is generated during compilation. 36 | 37 | # Files description 38 | 39 | #predictive_trajectory_generator.cpp 40 | - Contains code to pregenerate MPC solver using ACADO 41 | 42 | #predictive_configuration.cpp 43 | - Contains code to MPC load parameters from predictive_config_parameter.yaml in the config directory 44 | 45 | #mpcc_controller.cpp 46 | - Contains the controller class that is launched by the predictive_control_node 47 | -------------------------------------------------------------------------------- /action/move.action: -------------------------------------------------------------------------------- 1 | #goal definition, use intrative marker to track desired position,frame id to track, goal position need to be set 2 | string target_frame_id 3 | geometry_msgs/PoseStamped target_endeffector_pose 4 | --- 5 | #result definition, reach to goal pose 6 | bool reach 7 | --- 8 | #feedback, current position of endeffector 9 | geometry_msgs/Pose current_endeffector_pose 10 | -------------------------------------------------------------------------------- /action/traj.action: -------------------------------------------------------------------------------- 1 | #goal definition, use intrative marker to track desired position,frame id to track, goal position need to be set 2 | moveit_msgs/RobotTrajectory trajectory 3 | --- 4 | #result definition, reach to goal pose 5 | bool reach 6 | --- 7 | #feedback, current position of endeffector 8 | geometry_msgs/Pose tracking_error 9 | -------------------------------------------------------------------------------- /config/predictive_config_parameter.yaml: -------------------------------------------------------------------------------- 1 | 2 | ### Kinematic configuration parameter 3 | 4 | # Debug mode 5 | activate_output: true 6 | activate_debug_output: true 7 | activate_controller_node_output: true 8 | plotting_result: true 9 | 10 | # Clock frequency //hz 11 | clock_frequency: 50 12 | 13 | #Optimal control problem dimensions 14 | state_dim: 4 15 | control_dim: 2 16 | 17 | #Inputs and output topics topic 18 | output_cmd: '/jackal_velocity_controller/cmd_vel' 19 | robot_state_topic: '/robot_state' 20 | 21 | # Kinematic chain parameter 22 | robot_base_link: base_link 23 | target_frame: odom 24 | tracking_frame: odom 25 | 26 | # Parameters for collision constraints 27 | obstacles: 28 | n_obstacles: 3 29 | sub_ellipse_topic: ellipse_objects_feed 30 | n_discs: 2 31 | ego_l: 0.5 32 | ego_w: 0.4 33 | 34 | # predictive_config: # minimum_collision_distance should not be larger than ball radius 35 | self_collision: 36 | ball_radius: 0.12 37 | minimum_collision_distance: 0.15 38 | collision_weight_factor: 0.01 39 | 40 | # the following obstacles are considered for collision avoidance 41 | collision_check_obstacles: [] 42 | 43 | constraints: 44 | velocity_constraints: 45 | min: [-0.2, 0.2] 46 | max: [0.2,0.2] 47 | #orientation_constraints: 48 | 49 | acado_config: 50 | max_num_iteration: 10 51 | kkt_tolerance: 1e-6 52 | integrator_tolerance: 1e-8 53 | start_time_horizon: 0.0 54 | end_time_horizon: 5.0 # control and prediction horizon 55 | discretization_intervals: 10 56 | sampling_time: 0.02 57 | use_lagrange_term: false 58 | use_LSQ_term: false 59 | use_mayer_term: true 60 | weight_factors: 61 | lsq_state_weight_factors: 62 | #always 6 component 3 linear/position and 3 angular/Euler angle 63 | [5.0, 5.0, 5.0, 5.0] 64 | lsq_control_weight_factors: 65 | # same as degree of freedom 66 | [5.0, 20.0] 67 | -------------------------------------------------------------------------------- /include/predictive_control/mpcc_controller.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PREDICTIVE_CONTROL_PREDICTIVE_CONTROLLER_H 3 | #define PREDICTIVE_CONTROL_PREDICTIVE_CONTROLLER_H 4 | 5 | // ros includes 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // eigen includes 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // std includes 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // boost includes 32 | #include 33 | #include 34 | 35 | // Visualization messages 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | // yaml parsing 42 | #include 43 | #include 44 | 45 | // predicitve includes 46 | #include 47 | #include 48 | 49 | // actions, srvs, msgs 50 | #include 51 | #include 52 | #include 53 | #include 54 | //#include 55 | 56 | // joint trajectory interface 57 | #include 58 | #include 59 | #include 60 | 61 | #include 62 | 63 | // Add obstacle messages 64 | #include 65 | #include 66 | 67 | /* 68 | struct hold_pose 69 | { 70 | bool hold_success_; 71 | Eigen::VectorXd pose_hold_vector_; 72 | };*/ 73 | 74 | class MPCC 75 | { 76 | /** Managing execution of all classes of predictive control 77 | * - Handle self collsion avoidance 78 | * - Extract current position and velocity of manipulator joints 79 | * - Publish controlled joint velocity 80 | */ 81 | //Info: static member for transform std::vector to Eigen::vector 82 | 83 | public: 84 | 85 | /** 86 | * @brief MPCC: Default constructor, allocate memory 87 | */ 88 | MPCC(); 89 | 90 | /** 91 | * @brief ~MPCC: Default distructor, free memory 92 | */ 93 | ~MPCC(); 94 | 95 | /** 96 | * @brief initialize: Initialize all helper class of predictive control and subscibe joint state and publish controlled joint velocity 97 | * @return: True with successuflly initialize all classes else false 98 | */ 99 | bool initialize(); 100 | 101 | /** 102 | * @brief StateCallBack: Get current state of the robot 103 | * @param msg: Read data from mobile_robot_state_publisher_node default type: 104 | */ 105 | void StateCallBack(const geometry_msgs::Pose::ConstPtr& msg); 106 | 107 | void ObstacleCallBack(const obstacle_feed::Obstacles& obstacles); 108 | 109 | /** 110 | * @brief controlSquence: Known as main control of all classes 111 | */ 112 | void controlSquence(void); 113 | 114 | /** 115 | * @brief getTransform: Find transformation stamed rotation is in the form of quaternion 116 | * @param from: source frame from find transformation 117 | * @param to: target frame till find transformation 118 | * @param stamped_pose: Resultant poseStamed between source and target frame 119 | * @return: true if transform else false 120 | */ 121 | bool getTransform(const std::string& from, 122 | const std::string& to, 123 | geometry_msgs::PoseStamped& stamped_pose 124 | ); 125 | 126 | /** 127 | * @brief getTransform: Find transformation stamed rotation is in the form of quaternion 128 | * @param from: source frame from find transformation 129 | * @param to: target frame till find transformation 130 | * @param stamped_pose: Resultant poseStamed between source and target frame 131 | * @return: true if transform else false 132 | */ 133 | 134 | bool transformEigenToGeometryPose(const Eigen::VectorXd& eigen_vector, geometry_msgs::Pose& pose); 135 | 136 | /** public data member */ 137 | // joint state subsciber to get current joint value 138 | ros::Subscriber robot_state_sub_; 139 | 140 | // subscriber for obstacle feed 141 | ros::Subscriber obstacle_feed_sub_; 142 | 143 | // controlled joint velocity, should be control velocity of controller 144 | ros::Publisher controlled_velocity_pub_; 145 | 146 | // publishes error vector between tracking and target frame 147 | ros::Publisher cartesian_error_pub_; 148 | 149 | // publish trajectory 150 | ros::Publisher traj_pub_, tr_path_pub_, pred_traj_pub_; 151 | //Predicted trajectory 152 | nav_msgs::Path pred_traj_; 153 | VariablesGrid states; 154 | DVector state; 155 | 156 | private: 157 | 158 | ros::NodeHandle nh; 159 | 160 | // DEBUG 161 | bool plotting_result_; 162 | 163 | tf::TransformListener tf_listener_; 164 | 165 | // Clock frequency 166 | double clock_frequency_; 167 | 168 | // Timmer 169 | ros::Timer timer_; 170 | 171 | // activate output of this node 172 | bool activate_debug_output_; 173 | // used to set desired position by mannually or using interactive marker node 174 | bool tracking_; 175 | std::string target_frame_; 176 | 177 | 178 | // store pose value for visualize trajectory 179 | //geometry_msgs::PoseArray traj_pose_array_; 180 | visualization_msgs::MarkerArray traj_marker_array_; 181 | 182 | // Distance between traget frame and tracking frame relative to base link 183 | Eigen::Vector3d current_state_, last_state_; 184 | Eigen::Vector3d goal_pose_, prev_pose_,next_pose_; 185 | Eigen::VectorXd tf_traget_from_tracking_vector_; 186 | 187 | Eigen::VectorXd min_velocity_limit_; 188 | Eigen::VectorXd max_velocity_limit_; 189 | 190 | //MoveIt TRAJECTORY VARIABLE 191 | moveit_msgs::RobotTrajectory traj; 192 | 193 | //TRajectory execution variables 194 | double next_point_dist, goal_dist, prev_point_dist; 195 | int idx, idy; 196 | double epsilon_; 197 | 198 | // Kinematic variables 199 | //To be done kinematic model car 200 | 201 | // Obstacles 202 | obstacle_feed::Obstacles obstacles_; 203 | obstacle_feed::Obstacles obstacles_init_; 204 | 205 | // Current and last position and velocity from joint state callback 206 | //Eigen::VectorXd current_position_; 207 | Eigen::VectorXd last_position_; 208 | //Eigen::VectorXd current_velocity_; 209 | Eigen::VectorXd last_velocity_; 210 | 211 | // Type of variable used to publish joint velocity 212 | geometry_msgs::Twist controlled_velocity_; 213 | 214 | // predictive configuration 215 | boost::shared_ptr controller_config_; 216 | 217 | // kinematic calculation 218 | //TO BE REPLACED BY CAR CLASS CALCULATION 219 | //boost::shared_ptr kinematic_solver_; 220 | 221 | // self collision detector/avoidance 222 | //boost::shared_ptr collision_detect_; 223 | //boost::shared_ptr collision_avoidance_; 224 | 225 | // static collision detector/avoidance 226 | //boost::shared_ptr static_collision_avoidance_; 227 | 228 | // predictive trajectory generator 229 | boost::shared_ptr pd_trajectory_generator_; 230 | 231 | // move to goal position action 232 | boost::scoped_ptr > move_action_server_; 233 | 234 | boost::scoped_ptr > moveit_action_server_; 235 | 236 | /// Action interface 237 | predictive_control::moveResult move_action_result_; 238 | predictive_control::moveFeedback move_action_feedback_; 239 | predictive_control::trajActionFeedback moveit_action_feedback_; 240 | predictive_control::trajActionResult moveit_action_result_; 241 | void moveGoalCB(); 242 | void movePreemptCB(); 243 | void moveitGoalCB(); 244 | void actionSuccess(); 245 | void actionAbort(); 246 | 247 | /** 248 | * @brief spinNode: spin node means ROS is still running 249 | */ 250 | void spinNode(); 251 | 252 | /** 253 | * @brief runNode: Continue updating this function depend on clock frequency 254 | * @param event: Used for computation of duration of first and last event 255 | */ 256 | void runNode(const ros::TimerEvent& event); 257 | 258 | /** 259 | * @brief publishZeroJointVelocity: published zero joint velocity is statisfied cartesian distance 260 | */ 261 | void publishZeroJointVelocity(); 262 | 263 | /** 264 | * @brief publishErrorPose: published error pose between traget pose and tracking frame, it should approach to zero 265 | */ 266 | void publishErrorPose(const Eigen::VectorXd& error); 267 | 268 | 269 | void publishTrajectory(void); 270 | /** 271 | * @brief publishPredictedTrajectory: publish predicted trajectory 272 | */ 273 | void publishPredictedTrajectory(void); 274 | 275 | void publishPathFromTrajectory(const moveit_msgs::RobotTrajectory& traj); 276 | /** 277 | * @brief checkVelocityLimitViolation: check velocity limit violate, limit containts lower and upper limit 278 | * @param joint_velocity: current velocity joint values 279 | * @param velocity_tolerance: tolerance in joint values after reaching minimum and maximum values 280 | * @return true with velocity limit violation else false 281 | */ 282 | // bool checkVelocityLimitViolation(const std_msgs::Float64MultiArray& joint_velocity, 283 | // const double& velocity_tolerance = 0.0 284 | // ); 285 | // void enforceVelocityInLimits(const std_msgs::Float64MultiArray& joint_velocity, 286 | // std_msgs::Float64MultiArray& enforced_joint_velocity); 287 | // 288 | // /** 289 | // * @brief transformStdVectorToEigenVector: tranform std vector to eigen vectors as std vectos are slow to random access 290 | // * @param vector: std vectors want to tranfrom 291 | // * @return Eigen vectors transform from std vectos 292 | // */ 293 | /*template 294 | static inline Eigen::VectorXd transformStdVectorToEigenVector(const std::vector& vector) 295 | { 296 | // resize eigen vector 297 | Eigen::VectorXd eigen_vector = Eigen::VectorXd(vector.size()); 298 | 299 | // convert std to eigen vector 300 | for (uint32_t i = 0; i < vector.size(); ++i) 301 | { 302 | std::cout << vector.at(i) << std::endl; 303 | eigen_vector(i) = vector.at(i); 304 | } 305 | 306 | return eigen_vector; 307 | }*/ 308 | 309 | /** 310 | * @brief clearDataMember: clear vectors means free allocated memory 311 | */ 312 | void clearDataMember(); 313 | 314 | /** 315 | * @brief executeTrajectory: changes the goal state of the mpcc to each point of trajectory 316 | */ 317 | void executeTrajectory(const moveit_msgs::RobotTrajectory & traj); 318 | }; 319 | 320 | #endif 321 | -------------------------------------------------------------------------------- /include/predictive_control/predictive_configuration.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PREDICTIVE_CONTROL_PREDICITVE_CONFIGURATION_H 3 | #define PREDICTIVE_CONTROL_PREDICITVE_CONFIGURATION_H 4 | 5 | // ros includes 6 | #include 7 | 8 | //c++ includes 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include //print false or true 14 | #include 15 | 16 | class predictive_configuration 17 | { 18 | /** 19 | * @brief All neccessary configuration parameter of predictive control repository 20 | * Read data from parameter server 21 | * Updated old data with new data 22 | * Note: All data member name used like xyz_ and all parameter name is normal like xyz. 23 | */ 24 | 25 | public: 26 | 27 | /** function member of class **/ 28 | 29 | // constructor and distructor 30 | /** 31 | * @brief predictive_configuration: defualt constructor of this class 32 | */ 33 | predictive_configuration(); 34 | 35 | /** 36 | * @brief ~predictive_configuration: defualt distructor of this class 37 | */ 38 | ~predictive_configuration(); 39 | 40 | /** 41 | * @brief intialize: check parameter on paramter server and read from there 42 | * @param node_handle_name: node handler initialize from name, as parameter set inside that name 43 | * @return true all parameter initialize successfully else false 44 | */ 45 | bool initialize(); //const std::string& node_handle_name 46 | 47 | /** 48 | * @brief updateConfiguration: update configuration parameter with new parameter 49 | * @param new_config: changed configuration parameter 50 | * @return true all parameter update successfully else false 51 | */ 52 | bool updateConfiguration(const predictive_configuration& new_config); 53 | 54 | /** data member of class **/ 55 | // DEBUG 56 | bool activate_debug_output_; 57 | bool plotting_result_; 58 | bool activate_controller_node_output_; 59 | bool initialize_success_; 60 | bool set_velocity_constraints_; 61 | 62 | /** inputs and output topic definition **/ 63 | std::string output_cmd; 64 | std::string robot_state_topic_; 65 | 66 | // kinematic of robotics manipulator 67 | unsigned int degree_of_freedom_; 68 | 69 | // use for finding kinematic chain and urdf model 70 | std::string robot_base_link_; 71 | std::string tracking_frame_; // End effector of arm 72 | std::string sub_ellipse_topic_; 73 | 74 | // limiting parameter, use to enforce joint to be in limit 75 | std::vector collision_check_obstacles_; 76 | std::vector vel_min_limit_; 77 | std::vector vel_max_limit_; 78 | 79 | std::vector lsq_state_weight_factors_; 80 | std::vector lsq_control_weight_factors_; 81 | 82 | // predictive control 83 | double clock_frequency_; //hz clock Frequency 84 | double sampling_time_; 85 | int state_dim_; 86 | int control_dim_; 87 | 88 | // self collision distance 89 | double ball_radius_; 90 | double minimum_collision_distance_; 91 | double collision_weight_factor_; 92 | 93 | // acado configuration 94 | bool use_lagrange_term_; 95 | bool use_LSQ_term_; 96 | bool use_mayer_term_; 97 | int max_num_iteration_; 98 | int discretization_intervals_; 99 | double kkt_tolerance_; 100 | double integrator_tolerance_; 101 | double start_time_horizon_; 102 | double end_time_horizon_; 103 | 104 | int n_obstacles_; 105 | int n_discs_; 106 | double ego_l_; 107 | double ego_w_; 108 | 109 | 110 | private: 111 | 112 | /** 113 | * @brief free_allocated_memory: remove all allocated data just for memory management 114 | */ 115 | void free_allocated_memory(); 116 | 117 | /** 118 | * @brief print_configuration_parameter: debug purpose print set data member of this class 119 | */ 120 | void print_configuration_parameter(); 121 | 122 | }; 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /include/predictive_control/predictive_controller.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PREDICTIVE_CONTROL_PREDICTIVE_CONTROLLER_H 3 | #define PREDICTIVE_CONTROL_PREDICTIVE_CONTROLLER_H 4 | 5 | // ros includes 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // eigen includes 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | // std includes 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | // boost includes 32 | #include 33 | #include 34 | 35 | // yaml parsing 36 | #include 37 | #include 38 | 39 | // predicitve includes 40 | #include 41 | //#include 42 | //#include 43 | #include 44 | 45 | // actions, srvs, msgs 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | /* 53 | struct hold_pose 54 | { 55 | bool hold_success_; 56 | Eigen::VectorXd pose_hold_vector_; 57 | };*/ 58 | 59 | class MPCC 60 | { 61 | /** Managing execution of all classes of predictive control 62 | * - Handle self collsion avoidance 63 | * - Extract current position and velocity of manipulator joints 64 | * - Publish controlled joint velocity 65 | */ 66 | //Info: static member for transform std::vector to Eigen::vector 67 | 68 | public: 69 | 70 | /** 71 | * @brief MPCC: Default constructor, allocate memory 72 | */ 73 | MPCC(); 74 | 75 | /** 76 | * @brief ~MPCC: Default distructor, free memory 77 | */ 78 | ~MPCC(); 79 | 80 | /** 81 | * @brief initialize: Initialize all helper class of predictive control and subscibe joint state and publish controlled joint velocity 82 | * @return: True with successuflly initialize all classes else false 83 | */ 84 | bool initialize(); 85 | 86 | /** 87 | * @brief jointStateCallBack: Get current position and velocity at each joint 88 | * @param msg: Read data from sensor_msgs::JointState 89 | */ 90 | void jointStateCallBack(const sensor_msgs::JointState::ConstPtr& msg); 91 | 92 | /** 93 | * @brief controlSquence: Known as main control of all classes 94 | */ 95 | void controlSquence(void); 96 | 97 | /** 98 | * @brief getTransform: Find transformation stamed rotation is in the form of quaternion 99 | * @param from: source frame from find transformation 100 | * @param to: target frame till find transformation 101 | * @param stamped_pose: Resultant poseStamed between source and target frame 102 | * @return: true if transform else false 103 | */ 104 | /*bool getTransform(const std::string& from, 105 | const std::string& to, 106 | geometry_msgs::PoseStamped& stamped_pose 107 | );*/ 108 | 109 | /** 110 | * @brief getTransform: Find transformation stamed rotation is in the form of quaternion 111 | * @param from: source frame from find transformation 112 | * @param to: target frame till find transformation 113 | * @param stamped_pose: Resultant poseStamed between source and target frame 114 | * @return: true if transform else false 115 | */ 116 | bool getTransform(const std::string& from, 117 | const std::string& to, 118 | Eigen::VectorXd& stamped_pose 119 | ); 120 | 121 | 122 | bool transformEigenToGeometryPose(const Eigen::VectorXd& eigen_vector, geometry_msgs::Pose& pose); 123 | 124 | /** public data member */ 125 | // joint state subsciber to get current joint value 126 | ros::Subscriber robot_state_sub_; 127 | 128 | // controlled joint velocity, should be control velocity of controller 129 | ros::Publisher controlled_velocity_pub_; 130 | 131 | // publishes error vector between tracking and target frame 132 | ros::Publisher cartesian_error_pub_; 133 | 134 | // publish trajectory 135 | ros::Publisher traj_pub_; 136 | 137 | private: 138 | 139 | ros::NodeHandle nh; 140 | 141 | // DEBUG 142 | bool plotting_result_; 143 | 144 | tf::TransformListener tf_listener_; 145 | 146 | // degree of freedom 147 | uint32_t degree_of_freedom_; 148 | 149 | // Clock frequency 150 | double clock_frequency_; 151 | 152 | // Cartesian distance/error 153 | double cartesian_dist_; 154 | 155 | // Rotation distance/error 156 | double rotation_dist_; 157 | 158 | // Timmer 159 | ros::Timer timer_; 160 | 161 | // activate output of this node 162 | bool activate_debug_output_; 163 | // used to set desired position by mannually or using interactive marker node 164 | bool tracking_; 165 | std::string target_frame_; 166 | 167 | 168 | // store pose value for visualize trajectory 169 | //geometry_msgs::PoseArray traj_pose_array_; 170 | visualization_msgs::MarkerArray traj_marker_array_; 171 | 172 | // Distance between traget frame and tracking frame relative to base link 173 | Eigen::VectorXd tf_traget_from_tracking_vector_; 174 | 175 | Eigen::VectorXd min_velocity_limit_; 176 | Eigen::VectorXd max_velocity_limit_; 177 | 178 | // Kinematic variables 179 | //To be done kinematic model car 180 | 181 | // Current and last position and velocity from joint state callback 182 | //Eigen::VectorXd current_position_; 183 | Eigen::VectorXd last_position_; 184 | //Eigen::VectorXd current_velocity_; 185 | Eigen::VectorXd last_velocity_; 186 | 187 | // Type of variable used to publish joint velocity 188 | std_msgs::Float64MultiArray controlled_velocity_; 189 | 190 | // predictive configuration 191 | boost::shared_ptr controller_config_; 192 | 193 | // kinematic calculation 194 | //TO BE REPLACED BY CAR CLASS CALCULATION 195 | //boost::shared_ptr kinematic_solver_; 196 | 197 | // self collision detector/avoidance 198 | boost::shared_ptr collision_detect_; 199 | boost::shared_ptr collision_avoidance_; 200 | 201 | // static collision detector/avoidance 202 | boost::shared_ptr static_collision_avoidance_; 203 | 204 | // predictive trajectory generator 205 | boost::shared_ptr pd_trajectory_generator_; 206 | 207 | // move to goal position action 208 | boost::scoped_ptr > move_action_server_; 209 | 210 | /// Action interface 211 | predictive_control::moveResult move_action_result_; 212 | predictive_control::moveFeedback move_action_feedback_; 213 | void moveGoalCB(); 214 | void movePreemptCB(); 215 | void actionSuccess(); 216 | void actionAbort(); 217 | 218 | /** 219 | * @brief spinNode: spin node means ROS is still running 220 | */ 221 | void spinNode(); 222 | 223 | /** 224 | * @brief runNode: Continue updating this function depend on clock frequency 225 | * @param event: Used for computation of duration of first and last event 226 | */ 227 | void runNode(const ros::TimerEvent& event); 228 | 229 | /** 230 | * @brief publishZeroJointVelocity: published zero joint velocity is statisfied cartesian distance 231 | */ 232 | void publishZeroJointVelocity(); 233 | 234 | /** 235 | * @brief publishErrorPose: published error pose between traget pose and tracking frame, it should approach to zero 236 | */ 237 | void publishErrorPose(const Eigen::VectorXd& error); 238 | 239 | 240 | void publishTrajectory(void); 241 | 242 | /** 243 | * @brief checkPositionLimitViolation: check velocity limit violate, limit containts lower and upper limit 244 | * @param joint_velocity: current velocity joint values 245 | * @param velocity_tolerance: tolerance in joint values after reaching minimum and maximum values 246 | * @return true with velocity limit violation else false 247 | */ 248 | bool checkVelocityLimitViolation(const std_msgs::Float64MultiArray& joint_velocity, 249 | const double& velocity_tolerance = 0.0 250 | ); 251 | 252 | /** 253 | * @brief checkInfinitesimalPose: check goal tolereance statisfied, check tolerance with goal_tolerance_ data member 254 | * @param pose: end effoctor pose 255 | * @return true statisfied else false 256 | */ 257 | bool checkInfinitesimalPose(const Eigen::VectorXd& pose); 258 | 259 | /** 260 | * @brief transformStdVectorToEigenVector: tranform std vector to eigen vectors as std vectos are slow to random access 261 | * @param vector: std vectors want to tranfrom 262 | * @return Eigen vectors transform from std vectos 263 | */ 264 | /*template 265 | static inline Eigen::VectorXd transformStdVectorToEigenVector(const std::vector& vector) 266 | { 267 | // resize eigen vector 268 | Eigen::VectorXd eigen_vector = Eigen::VectorXd(vector.size()); 269 | 270 | // convert std to eigen vector 271 | for (uint32_t i = 0; i < vector.size(); ++i) 272 | { 273 | std::cout << vector.at(i) << std::endl; 274 | eigen_vector(i) = vector.at(i); 275 | } 276 | 277 | return eigen_vector; 278 | }*/ 279 | 280 | /** 281 | * @brief clearDataMember: clear vectors means free allocated memory 282 | */ 283 | void clearDataMember(); 284 | }; 285 | 286 | #endif 287 | -------------------------------------------------------------------------------- /include/predictive_control/predictive_trajectory_generator.h: -------------------------------------------------------------------------------- 1 | 2 | //This file containts cost function intsert in to generated trajectory. 3 | 4 | #ifndef PREDICTIVE_CONTROL_PREDICITVE_TRAJECTORY_GENERATOR_H 5 | #define PREDICTIVE_CONTROL_PREDICITVE_TRAJECTORY_GENERATOR_H 6 | 7 | // ros includes 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | //Eigen includes 18 | #include 19 | #include 20 | #include 21 | 22 | // std includes 23 | #include 24 | #include 25 | #include 26 | #include //print false or true 27 | #include 28 | 29 | // boost includes 30 | #include 31 | 32 | // yaml parsing 33 | #include 34 | #include 35 | 36 | //acado includes 37 | #include 38 | #include 39 | #include 40 | 41 | // predictive includes 42 | #include 43 | 44 | //splines 45 | //#include 46 | 47 | // Add obstacle messages 48 | #include 49 | #include 50 | 51 | // Trajectory info 52 | #include 53 | 54 | using namespace ACADO; 55 | 56 | class pd_frame_tracker: public predictive_configuration 57 | { 58 | /** Frame tracker node, optimal control problem solver using MPC 59 | * - solver optimal control problem using ACADO Toolkit 60 | * - Add constrints as self collision 61 | * - Generate controlled velocity 62 | */ 63 | 64 | public: 65 | 66 | double s_; // this variable is going to be later replaced by a ACADO Process to simulate 67 | //spline and other parameters 68 | double* p; 69 | // tk::spline ref_path_x, ref_path_y; 70 | 71 | /** 72 | * @brief pd_frame_tracker: Default constructor, allocate memory 73 | */ 74 | pd_frame_tracker(): x_("", 4, 1), 75 | v_("",2,1){}; 76 | 77 | /** 78 | *@brief ~pd_frame_tracker: Default distructor, free memory 79 | */ 80 | ~pd_frame_tracker(); 81 | 82 | /** 83 | * @brief initialize: initialize frame tracker class and data members 84 | * @return true with successful initialize else false 85 | */ 86 | bool initialize(); //virtual 87 | 88 | /** 89 | * @brief initializeOptimalControlProblem: initialize the parameter grid of optimal control problem using ACADO Toolkit 90 | * @param parameters: parameter vector as used in matlab MPCC 91 | */ 92 | void initializeOptimalControlProblem(std::vector parameters); 93 | 94 | 95 | /** 96 | * @brief solveOptimalControlProblem: Handle execution of whole class, solve optimal control problem using ACADO Toolkit 97 | * @param Jacobian_Matrix: Jacobian Matrix use to generate dynamic system of equation 98 | * @param last_position: current/last joint values used to initialize states 99 | * @param goal_pose: Goal pose where want to reach 100 | * @param controlled_velocity: controlled velocity use to publish 101 | */ 102 | VariablesGrid solveOptimalControlProblem(const Eigen::VectorXd& last_position, 103 | const Eigen::Vector3d& prev_pose, 104 | const Eigen::Vector3d& next_pose, 105 | const Eigen::Vector3d& goal_pose, 106 | const obstacle_feed::Obstacles& obstacles, 107 | geometry_msgs::Twist& controlled_velocity 108 | ); 109 | 110 | /** 111 | * @brief hardCodedOptimalControlSolver: hard coded optimal conrol solver just for debug purpose 112 | * @return controlled joint velocity 113 | */ 114 | std_msgs::Float64MultiArray hardCodedOptimalControlSolver(); 115 | 116 | // STATIC FUNCTION, NO NEED OBJECT OF CLASS, DIRECT CALL WITHOUT OBJECT 117 | /** 118 | * @brief transformStdVectorToEigenVector: tranform std vector to eigen vectors as std vectos are slow to random access 119 | * @param vector: std vectors want to tranfrom 120 | * @return Eigen vectors transform from std vectos 121 | */ 122 | template 123 | static inline Eigen::VectorXd transformStdVectorToEigenVector(const std::vector& vector) 124 | { 125 | // resize eigen vector 126 | Eigen::VectorXd eigen_vector = Eigen::VectorXd(vector.size()); 127 | 128 | // convert std to eigen vector 129 | for (uint32_t i = 0; i < vector.size(); ++i) 130 | { 131 | eigen_vector(i) = vector.at(i); 132 | } 133 | 134 | return eigen_vector; 135 | } 136 | 137 | private: 138 | 139 | // tf listerner 140 | tf::TransformListener tf_listener_; 141 | 142 | // state initialization 143 | DVector state_initialize_; 144 | 145 | // control initialization 146 | DVector control_initialize_; 147 | 148 | // constraints 149 | DVector control_min_constraint_; 150 | DVector control_max_constraint_; 151 | 152 | // Obstacles 153 | obstacle_feed::Obstacles obstacles_; 154 | int n_obstacles_; 155 | double r_discs_; 156 | Eigen::VectorXd x_discs_; 157 | 158 | 159 | //acado configuration paramter, 160 | int max_num_iteration_; 161 | double kkt_tolerance_; 162 | double integrator_tolerance_; 163 | 164 | // control and/or prediction horizon parameter 165 | double start_time_; 166 | double end_time_; 167 | int discretization_intervals_; 168 | double sampling_time_; 169 | int horizon_steps_; 170 | 171 | // objective function minimization type 172 | bool use_lagrange_term_; 173 | bool use_LSQ_term_; 174 | bool use_mayer_term_; 175 | 176 | // collioion cost weighting/constant term 177 | double self_collision_cost_constant_term_; 178 | 179 | // lsq weight factors 180 | Eigen::VectorXd lsq_state_weight_factors_; 181 | uint32_t state_vector_size_; 182 | Eigen::VectorXd lsq_control_weight_factors_; 183 | uint32_t control_vector_size_; 184 | 185 | //OCP Parameters 186 | DVector param_; 187 | 188 | //ACADO variables 189 | DifferentialEquation f; 190 | DifferentialState x_; // position 191 | Control v_; // velocities 192 | VariablesGrid pred_states; 193 | DVector u; 194 | 195 | /** 196 | * @brief generateCostFunction: generate cost function, minimizeMayaerTerm, LSQ using weighting matrix and reference vector 197 | * Langrange 198 | * @param OCP_problem: Current optimal control problem 199 | * @param x: Differential state represent dynamic system of equations 200 | * @param v: Control state use to control manipulator, in our case joint velocity 201 | * @param goal_pose: Target pose where want to move 202 | */ 203 | 204 | void generateCostFunction(OCP& OCP_problem, 205 | const DifferentialState& x, 206 | const Control& v, 207 | const Eigen::Vector3d& goal_pose 208 | ); 209 | 210 | void iniKinematics(const DifferentialState& x, 211 | const Control& v 212 | ); 213 | 214 | void setCollisionConstraints(OCP& OCP_problem, 215 | const DifferentialState& x, 216 | const obstacle_feed::Obstacles& obstacles, 217 | const double& delta_t 218 | ); 219 | 220 | /** 221 | * @brief setAlgorithmOptions: setup solver options, Optimal control solver or RealTimeSolver(MPC) 222 | * @param OCP_solver: optimal control solver used to solver system of equations 223 | */ 224 | void setAlgorithmOptions(RealTimeAlgorithm& OCP_solver); 225 | 226 | /** 227 | * @brief clearDataMember: clear vectors means free allocated memory 228 | */ 229 | void clearDataMember(); 230 | 231 | void computeEgoDiscs(); 232 | 233 | }; 234 | 235 | #endif 236 | -------------------------------------------------------------------------------- /launch/predictive_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | # load Cartesian controller config 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /matlab/relative_motion.m: -------------------------------------------------------------------------------- 1 | %% Relative motion dynamics 2 | 3 | p_I = [1,2,0]; 4 | p_B=[]; 5 | j=1; 6 | p_B(1,1:3) =[1;2;0]; 7 | dt=0.1; 8 | w=0; 9 | v=0; 10 | p_goal =[1;1;0]; 11 | p=[0;0;0]; 12 | 13 | %controller 14 | K=0.1*[1 1 1; 15 | 1 1 1]; 16 | kp=3; 17 | kalpha = 8; 18 | kbeta = -1.5; 19 | 20 | for t=0:dt:10 21 | j=j+1; 22 | % error computation 23 | e(:,j) = p_goal-p(:,j-1); 24 | 25 | %angles 26 | alpha = -p(3,j-1)+atan2(e(2,j),e(1,j)); 27 | beta = -p(3,j-1)-alpha; 28 | 29 | %compute control input 30 | u= [kp*norm(e(1:2,j));kalpha*alpha+kbeta*beta]; 31 | 32 | p(:,j)=p(:,j-1)+dt*[cos(p(3,j-1)) 0;sin(p(3,j-1)) 0;0 1]*u; 33 | 34 | %p_B(j,:)=p_B(j-1,:)+[w*p_B(j-1,2)-v*cos]; 35 | 36 | end 37 | 38 | figure 39 | hold on; 40 | 41 | plot(0:dt:10+dt,p); -------------------------------------------------------------------------------- /matlab/relative_motion2.m: -------------------------------------------------------------------------------- 1 | %% Relative motion dynamics 2 | clc; 3 | clear; 4 | close all; 5 | 6 | p_I = [1,2,0]; 7 | p_B=[]; 8 | j=1; 9 | p_B(1,1:3) =[1;2;0]; 10 | dt=0.1; 11 | w=0; 12 | v=0; 13 | p_goal =[1;1;atan2(1,1)]; 14 | p=p_goal; 15 | 16 | %controller 17 | K=0.1*[1 1 1; 18 | 1 1 1]; 19 | kp=0.1; 20 | kalpha = 0; 21 | kbeta = 0; 22 | 23 | for t=0:dt:50 24 | j=j+1; 25 | % error computation 26 | e(:,j) = p(:,j-1); 27 | 28 | %angles 29 | alpha = -p(3,j-1)+atan2(e(2,j),e(1,j)); 30 | beta = -p(3,j-1)-alpha; 31 | 32 | %compute control input 33 | u= [kp*norm(e(1:2,j));kp*norm(e(3,j))]; 34 | 35 | 36 | p(:,j)=p(:,j-1)+dt*[u(2)*p(2,j-1)-u(1)*cos(p(3,j-1));-u(2)*p(1,j-1)-u(1)*sin(p(3,j-1));-u(2)]; 37 | p(3,j) = atan2(sin(p(3,j)),cos(p(3,j))); 38 | 39 | %p_B(j,:)=p_B(j-1,:)+[w*p_B(j-1,2)-v*cos]; 40 | 41 | end 42 | 43 | figure 44 | hold on; 45 | 46 | plot(0:dt:50+dt,e); -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | predictive_control 4 | 0.6.14 5 | The predictive_control package 6 | 7 | Mayank Patel 8 | IPA 9 | 10 | catkin 11 | roslint 12 | 13 | actionlib 14 | actionlib_msgs 15 | boost 16 | cmake_modules 17 | control_msgs 18 | dynamic_reconfigure 19 | eigen_conversions 20 | eigen 21 | geometry_msgs 22 | kdl_conversions 23 | kdl_parser 24 | moveit_core 25 | nav_msgs 26 | orocos_kdl 27 | pluginlib 28 | roscpp 29 | sensor_msgs 30 | std_msgs 31 | tf 32 | tf_conversions 33 | trajectory_msgs 34 | urdf 35 | visualization_msgs 36 | shape_msgs 37 | moveit_msgs 38 | obstacle_feed 39 | 40 | 41 | robot_state_publisher 42 | rospy 43 | rviz 44 | topic_tools 45 | xacro 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /predictive_control.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: predictive_control 3 | uri: https://github.com/bbrito/predictive_control.git 4 | version: master 5 | - git: 6 | local-name: base_moveit_controller_manager 7 | uri: https://github.com/bbrito/base_moveit_controller_manager.git 8 | version: master 9 | - git: 10 | local-name: mobile_robot_state_publisher 11 | uri: https://github.com/bbrito/mobile_robot_state_publisher.git 12 | version: master 13 | - git: 14 | local-name: jackal_moveit_description 15 | uri: https://github.com/bbrito/jackal_moveit_description.git 16 | version: master 17 | - git: 18 | local-name: obstacle_feed 19 | uri: https://github.com/bbrito/obstacle_feed.git 20 | version: master 21 | - git: 22 | local-name: autoware_msgs 23 | uri: https://github.com/bbrito/autoware_msgs 24 | version: master 25 | -------------------------------------------------------------------------------- /rviz_config/rviz_config.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Axes1 10 | - /Axes2 11 | Splitter Ratio: 0.5 12 | Tree Height: 124 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Alpha: 1 54 | Class: rviz/RobotModel 55 | Collision Enabled: false 56 | Enabled: true 57 | Links: 58 | All Links Enabled: true 59 | Expand Joint Details: false 60 | Expand Link Details: false 61 | Expand Tree: false 62 | Link Tree Style: Links in Alphabetic Order 63 | base_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | chassis_link: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | fenders_link: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | front_left_wheel_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | front_mount: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | front_right_wheel_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | imu_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | mid_mount: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | navsat_link: 100 | Alpha: 1 101 | Show Axes: false 102 | Show Trail: false 103 | Value: true 104 | rear_left_wheel_link: 105 | Alpha: 1 106 | Show Axes: false 107 | Show Trail: false 108 | Value: true 109 | rear_mount: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | rear_right_wheel_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | Name: RobotModel 119 | Robot Description: robot_description 120 | TF Prefix: "" 121 | Update Interval: 0 122 | Value: true 123 | Visual Enabled: true 124 | - Class: rviz/Axes 125 | Enabled: true 126 | Length: 1 127 | Name: Axes 128 | Radius: 0.100000001 129 | Reference Frame: base_link 130 | Value: true 131 | - Class: rviz/Axes 132 | Enabled: true 133 | Length: 1 134 | Name: Axes 135 | Radius: 0.100000001 136 | Reference Frame: odom 137 | Value: true 138 | - Class: moveit_rviz_plugin/MotionPlanning 139 | Enabled: true 140 | Move Group Namespace: "" 141 | MoveIt_Allow_Approximate_IK: false 142 | MoveIt_Allow_External_Program: false 143 | MoveIt_Goal_Tolerance: 0 144 | MoveIt_Planning_Attempts: 10 145 | MoveIt_Planning_Time: 5 146 | MoveIt_Use_Constraint_Aware_IK: true 147 | MoveIt_Warehouse_Host: 127.0.0.1 148 | MoveIt_Warehouse_Port: 33829 149 | MoveIt_Workspace: 150 | Center: 151 | X: 0 152 | Y: 0 153 | Z: 0 154 | Size: 155 | X: 15 156 | Y: 15 157 | Z: 15 158 | Name: MotionPlanning 159 | Planned Path: 160 | Color Enabled: false 161 | Interrupt Display: false 162 | Links: 163 | All Links Enabled: true 164 | Expand Joint Details: false 165 | Expand Link Details: false 166 | Expand Tree: false 167 | Link Tree Style: Links in Alphabetic Order 168 | base_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | chassis_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | Value: true 177 | fenders_link: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | Value: true 182 | front_left_wheel_link: 183 | Alpha: 1 184 | Show Axes: false 185 | Show Trail: false 186 | Value: true 187 | front_mount: 188 | Alpha: 1 189 | Show Axes: false 190 | Show Trail: false 191 | front_right_wheel_link: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | imu_link: 197 | Alpha: 1 198 | Show Axes: false 199 | Show Trail: false 200 | mid_mount: 201 | Alpha: 1 202 | Show Axes: false 203 | Show Trail: false 204 | navsat_link: 205 | Alpha: 1 206 | Show Axes: false 207 | Show Trail: false 208 | Value: true 209 | rear_left_wheel_link: 210 | Alpha: 1 211 | Show Axes: false 212 | Show Trail: false 213 | Value: true 214 | rear_mount: 215 | Alpha: 1 216 | Show Axes: false 217 | Show Trail: false 218 | rear_right_wheel_link: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | Loop Animation: false 224 | Robot Alpha: 0.5 225 | Robot Color: 150; 50; 150 226 | Show Robot Collision: false 227 | Show Robot Visual: true 228 | Show Trail: false 229 | State Display Time: 0.05 s 230 | Trail Step Size: 1 231 | Trajectory Topic: /move_group/display_planned_path 232 | Planning Metrics: 233 | Payload: 1 234 | Show Joint Torques: false 235 | Show Manipulability: false 236 | Show Manipulability Index: false 237 | Show Weight Limit: false 238 | TextHeight: 0.0799999982 239 | Planning Request: 240 | Colliding Link Color: 255; 0; 0 241 | Goal State Alpha: 1 242 | Goal State Color: 250; 128; 0 243 | Interactive Marker Size: 0 244 | Joint Violation Color: 255; 0; 255 245 | Planning Group: base 246 | Query Goal State: true 247 | Query Start State: false 248 | Show Workspace: false 249 | Start State Alpha: 1 250 | Start State Color: 0; 255; 0 251 | Planning Scene Topic: move_group/monitored_planning_scene 252 | Robot Description: robot_description 253 | Scene Geometry: 254 | Scene Alpha: 0.899999976 255 | Scene Color: 50; 230; 50 256 | Scene Display Time: 0.200000003 257 | Show Scene Geometry: true 258 | Voxel Coloring: Z-Axis 259 | Voxel Rendering: Occupied Voxels 260 | Scene Robot: 261 | Attached Body Color: 150; 50; 150 262 | Links: 263 | All Links Enabled: true 264 | Expand Joint Details: false 265 | Expand Link Details: false 266 | Expand Tree: false 267 | Link Tree Style: Links in Alphabetic Order 268 | base_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | chassis_link: 273 | Alpha: 1 274 | Show Axes: false 275 | Show Trail: false 276 | Value: true 277 | fenders_link: 278 | Alpha: 1 279 | Show Axes: false 280 | Show Trail: false 281 | Value: true 282 | front_left_wheel_link: 283 | Alpha: 1 284 | Show Axes: false 285 | Show Trail: false 286 | Value: true 287 | front_mount: 288 | Alpha: 1 289 | Show Axes: false 290 | Show Trail: false 291 | front_right_wheel_link: 292 | Alpha: 1 293 | Show Axes: false 294 | Show Trail: false 295 | Value: true 296 | imu_link: 297 | Alpha: 1 298 | Show Axes: false 299 | Show Trail: false 300 | mid_mount: 301 | Alpha: 1 302 | Show Axes: false 303 | Show Trail: false 304 | navsat_link: 305 | Alpha: 1 306 | Show Axes: false 307 | Show Trail: false 308 | Value: true 309 | rear_left_wheel_link: 310 | Alpha: 1 311 | Show Axes: false 312 | Show Trail: false 313 | Value: true 314 | rear_mount: 315 | Alpha: 1 316 | Show Axes: false 317 | Show Trail: false 318 | rear_right_wheel_link: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | Robot Alpha: 1 324 | Show Robot Collision: false 325 | Show Robot Visual: true 326 | Value: true 327 | - Class: rviz/MarkerArray 328 | Enabled: true 329 | Marker Topic: /visualize_ellipses_array 330 | Name: MarkerArray 331 | Namespaces: 332 | "": true 333 | Queue Size: 100 334 | Value: true 335 | - Alpha: 1 336 | Buffer Length: 1 337 | Class: rviz/Path 338 | Color: 25; 255; 0 339 | Enabled: true 340 | Head Diameter: 0.300000012 341 | Head Length: 0.200000003 342 | Length: 0.300000012 343 | Line Style: Lines 344 | Line Width: 0.0299999993 345 | Name: Path 346 | Offset: 347 | X: 0 348 | Y: 0 349 | Z: 0 350 | Pose Color: 255; 85; 255 351 | Pose Style: None 352 | Radius: 0.0299999993 353 | Shaft Diameter: 0.100000001 354 | Shaft Length: 0.100000001 355 | Topic: /mpc_horizon 356 | Unreliable: false 357 | Value: true 358 | Enabled: true 359 | Global Options: 360 | Background Color: 48; 48; 48 361 | Default Light: true 362 | Fixed Frame: odom 363 | Frame Rate: 30 364 | Name: root 365 | Tools: 366 | - Class: rviz/Interact 367 | Hide Inactive Objects: true 368 | - Class: rviz/MoveCamera 369 | - Class: rviz/Select 370 | - Class: rviz/FocusCamera 371 | - Class: rviz/Measure 372 | - Class: rviz/SetInitialPose 373 | Topic: /initialpose 374 | - Class: rviz/SetGoal 375 | Topic: /move_base_simple/goal 376 | - Class: rviz/PublishPoint 377 | Single click: true 378 | Topic: /clicked_point 379 | Value: true 380 | Views: 381 | Current: 382 | Class: rviz/Orbit 383 | Distance: 5.65891981 384 | Enable Stereo Rendering: 385 | Stereo Eye Separation: 0.0599999987 386 | Stereo Focal Distance: 1 387 | Swap Stereo Eyes: false 388 | Value: false 389 | Focal Point: 390 | X: 1.67611897 391 | Y: 0.865820765 392 | Z: 2.0891223 393 | Focal Shape Fixed Size: true 394 | Focal Shape Size: 0.0500000007 395 | Invert Z Axis: false 396 | Name: Current View 397 | Near Clip Distance: 0.00999999978 398 | Pitch: 1.28039777 399 | Target Frame: 400 | Value: Orbit (rviz) 401 | Yaw: 1.44540024 402 | Saved: ~ 403 | Window Geometry: 404 | Displays: 405 | collapsed: false 406 | Height: 1056 407 | Hide Left Dock: false 408 | Hide Right Dock: true 409 | MotionPlanning: 410 | collapsed: false 411 | MotionPlanning - Trajectory Slider: 412 | collapsed: false 413 | QMainWindow State: 000000ff00000000fd0000000400000000000002b800000390fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000010b000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000120020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001390000027f0000018300fffffffb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004a00ffffff000000010000010f00000347fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004300000347000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073f00000044fc0100000002fb0000000800540069006d006501000000000000073f0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004810000039000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 414 | Selection: 415 | collapsed: false 416 | Time: 417 | collapsed: false 418 | Tool Properties: 419 | collapsed: false 420 | Views: 421 | collapsed: true 422 | Width: 1855 423 | X: 65 424 | Y: 24 425 | -------------------------------------------------------------------------------- /src/mpcc_controller.cpp: -------------------------------------------------------------------------------- 1 | 2 | //This file containts read parameter from server, callback, call class objects, control all class, objects of all class 3 | 4 | #include 5 | 6 | MPCC::MPCC() 7 | { 8 | ; 9 | } 10 | 11 | MPCC::~MPCC() 12 | { 13 | clearDataMember(); 14 | } 15 | 16 | void MPCC::spinNode() 17 | { 18 | ROS_INFO(" Predictive control node is running, now it's 'Spinning Node'"); 19 | ros::spin(); 20 | } 21 | 22 | // disallocated memory 23 | void MPCC::clearDataMember() 24 | { 25 | last_position_ = Eigen::VectorXd(3); 26 | last_velocity_ = Eigen::VectorXd(3); 27 | 28 | } 29 | 30 | // initialize all helper class of predictive control and subscibe joint state and publish controlled joint velocity 31 | bool MPCC::initialize() 32 | { 33 | // make sure node is still running 34 | if (ros::ok()) 35 | { 36 | // initialize helper classes, make sure pd_config should be initialized first as mother of other class 37 | controller_config_.reset(new predictive_configuration()); 38 | bool controller_config_success = controller_config_->initialize(); 39 | 40 | bool kinematic_success = true; 41 | 42 | pd_trajectory_generator_.reset(new pd_frame_tracker()); 43 | bool pd_traj_success = pd_trajectory_generator_->initialize(); 44 | 45 | if (pd_traj_success == false || controller_config_success == false) 46 | { 47 | ROS_ERROR("MPCC: FAILED TO INITILIZED!!"); 48 | std::cout << "States: \n" 49 | << " pd_config: " << std::boolalpha << controller_config_success << "\n" 50 | //<< " kinematic solver: " << std::boolalpha << kinematic_success << "\n" 51 | //<< " collision avoidance: " << std::boolalpha << collision_avoidance_success << "\n" 52 | //<< " collision detect: " << std::boolalpha << collision_success << "\n" 53 | //<< " static collision avoidance: " << std::boolalpha << static_collision_success << "\n" 54 | << " pd traj generator: " << std::boolalpha << pd_traj_success << "\n" 55 | << " pd config init success: " << std::boolalpha << controller_config_->initialize_success_ 56 | << std::endl; 57 | return false; 58 | } 59 | 60 | // initialize data member of class 61 | clock_frequency_ = controller_config_->clock_frequency_; 62 | 63 | //DEBUG 64 | activate_debug_output_ = controller_config_->activate_debug_output_; 65 | tracking_ = true; 66 | move_action_result_.reach = false; 67 | plotting_result_ = controller_config_->plotting_result_; 68 | 69 | /// INFO: static function called transformStdVectorToEigenVector define in the predictive_trajectory_generator.h 70 | min_velocity_limit_ = pd_frame_tracker::transformStdVectorToEigenVector(controller_config_->vel_min_limit_); 71 | max_velocity_limit_ = pd_frame_tracker::transformStdVectorToEigenVector(controller_config_->vel_max_limit_); 72 | 73 | // DEBUG 74 | if (controller_config_->activate_controller_node_output_) 75 | { 76 | ROS_WARN("===== DEBUG INFO ACTIVATED ====="); 77 | } 78 | 79 | // resize position and velocity velocity vectors 80 | current_state_ = Eigen::Vector3d(0,0,0); 81 | last_state_ = Eigen::Vector3d(0,0,0); 82 | prev_pose_ = Eigen::Vector3d(0,0,0); 83 | goal_pose_ = Eigen::Vector3d(0,0,0); 84 | next_pose_= Eigen::Vector3d(0,0,0); 85 | prev_pose_.setZero(); 86 | goal_pose_.setZero(); 87 | 88 | // ros interfaces 89 | static const std::string MOVE_ACTION_NAME = "move_action"; 90 | move_action_server_.reset(new actionlib::SimpleActionServer(nh, MOVE_ACTION_NAME, false)); 91 | move_action_server_->registerGoalCallback(boost::bind(&MPCC::moveGoalCB, this)); 92 | move_action_server_->registerPreemptCallback(boost::bind(&MPCC::movePreemptCB, this)); 93 | move_action_server_->start(); 94 | 95 | // MOVEIT interfaces 96 | static const std::string MOVEIT_ACTION_NAME = "fake_base_controller"; 97 | moveit_action_server_.reset(new actionlib::SimpleActionServer(nh, MOVEIT_ACTION_NAME, false)); 98 | moveit_action_server_->registerGoalCallback(boost::bind(&MPCC::moveitGoalCB, this)); 99 | moveit_action_server_->start(); 100 | 101 | robot_state_sub_ = nh.subscribe(controller_config_->robot_state_topic_, 1, &MPCC::StateCallBack, this); 102 | 103 | obstacle_feed_sub_ = nh.subscribe(controller_config_->sub_ellipse_topic_, 1, &MPCC::ObstacleCallBack, this); 104 | 105 | //To be implemented 106 | traj_pub_ = nh.advertise("pd_trajectory",1); 107 | tr_path_pub_ = nh.advertise("horizon",1); 108 | controlled_velocity_pub_ = nh.advertise(controller_config_->output_cmd,1); 109 | 110 | ros::Duration(1).sleep(); 111 | 112 | timer_ = nh.createTimer(ros::Duration(1/clock_frequency_), &MPCC::runNode, this); 113 | timer_.start(); 114 | 115 | //Initialize trajectory variables 116 | next_point_dist = 0; 117 | goal_dist = 0; 118 | prev_point_dist = 0; 119 | idx = 1; 120 | idy = 1; 121 | epsilon_ = 0.01; 122 | 123 | moveit_msgs::RobotTrajectory j; 124 | traj = j; 125 | 126 | //initialize trajectory variable to plot prediction trajectory 127 | pred_traj_.poses.resize(controller_config_->discretization_intervals_+1); 128 | for(int i=0;i < controller_config_->discretization_intervals_; i++) 129 | { 130 | pred_traj_.poses[i].header.frame_id = "odom"; 131 | pred_traj_.header.frame_id = "odom"; 132 | } 133 | 134 | pred_traj_pub_ = nh.advertise("mpc_horizon",1); 135 | 136 | ROS_WARN("PREDICTIVE CONTROL INTIALIZED!!"); 137 | return true; 138 | } 139 | else 140 | { 141 | ROS_ERROR("MPCC: Failed to initialize as ROS Node is shoutdown"); 142 | return false; 143 | } 144 | } 145 | 146 | // update this function 1/clock_frequency 147 | void MPCC::runNode(const ros::TimerEvent &event) 148 | { 149 | 150 | if(((int)traj.multi_dof_joint_trajectory.points.size()) > 1){ 151 | 152 | idx = (int)traj.multi_dof_joint_trajectory.points.size() -2; 153 | goal_pose_(0) = traj.multi_dof_joint_trajectory.points[idx+1].transforms[0].translation.x; 154 | goal_pose_(1) = traj.multi_dof_joint_trajectory.points[idx+1].transforms[0].translation.y; 155 | goal_pose_(2) = traj.multi_dof_joint_trajectory.points[idx+1].transforms[0].rotation.z; 156 | 157 | states = pd_trajectory_generator_->solveOptimalControlProblem(current_state_,prev_pose_,next_pose_, goal_pose_, obstacles_, controlled_velocity_); 158 | 159 | publishPredictedTrajectory(); 160 | } 161 | 162 | // publish zero controlled velocity 163 | if (!tracking_) 164 | { 165 | actionSuccess(); 166 | } 167 | //publishZeroJointVelocity(); 168 | controlled_velocity_pub_.publish(controlled_velocity_); 169 | } 170 | 171 | void MPCC::moveGoalCB() 172 | { 173 | if(move_action_server_->isNewGoalAvailable()) 174 | { 175 | boost::shared_ptr move_action_goal_ptr = move_action_server_->acceptNewGoal(); 176 | tracking_ = false; 177 | 178 | //erase previous trajectory 179 | for (auto it = traj_marker_array_.markers.begin(); it != traj_marker_array_.markers.end(); ++it) 180 | { 181 | it->action = visualization_msgs::Marker::DELETE; 182 | traj_pub_.publish(traj_marker_array_); 183 | } 184 | 185 | traj_marker_array_.markers.clear(); 186 | } 187 | } 188 | 189 | void MPCC::moveitGoalCB() 190 | { 191 | ROS_INFO_STREAM("Got new MoveIt goal!!!"); 192 | //Reset trajectory index 193 | idx = 1; 194 | pd_trajectory_generator_->s_=0; 195 | if(moveit_action_server_->isNewGoalAvailable()) 196 | { 197 | boost::shared_ptr moveit_action_goal_ptr = moveit_action_server_->acceptNewGoal(); 198 | traj = moveit_action_goal_ptr->trajectory; 199 | tracking_ = false; 200 | //start trajectory execution 201 | } 202 | } 203 | 204 | void MPCC::executeTrajectory(const moveit_msgs::RobotTrajectory & traj){ 205 | 206 | } 207 | 208 | void MPCC::movePreemptCB() 209 | { 210 | move_action_result_.reach = true; 211 | move_action_server_->setPreempted(move_action_result_, "Action has been preempted"); 212 | tracking_ = true; 213 | } 214 | 215 | void MPCC::actionSuccess() 216 | { 217 | move_action_server_->setSucceeded(move_action_result_, "Goal succeeded!"); 218 | tracking_ = true; 219 | } 220 | 221 | void MPCC::actionAbort() 222 | { 223 | move_action_server_->setAborted(move_action_result_, "Action has been aborted"); 224 | tracking_ = true; 225 | } 226 | 227 | // read current position and velocity of robot joints 228 | void MPCC::StateCallBack(const geometry_msgs::Pose::ConstPtr& msg) 229 | { 230 | if (activate_debug_output_) 231 | { 232 | //ROS_INFO("MPCC::StateCallBack"); 233 | } 234 | last_state_ = current_state_; 235 | current_state_(0) = msg->position.x; 236 | current_state_(1) = msg->position.y; 237 | current_state_(2) = msg->orientation.z; 238 | } 239 | 240 | void MPCC::ObstacleCallBack(const obstacle_feed::Obstacles& obstacles) 241 | { 242 | 243 | obstacles_ = obstacles; 244 | 245 | } 246 | 247 | void MPCC::publishZeroJointVelocity() 248 | { 249 | if (activate_debug_output_) 250 | { 251 | ROS_INFO("Publishing ZERO joint velocity!!"); 252 | } 253 | geometry_msgs::Twist pub_msg; 254 | 255 | controlled_velocity_ = pub_msg; 256 | 257 | controlled_velocity_pub_.publish(controlled_velocity_); 258 | } 259 | 260 | void MPCC::publishPredictedTrajectory(void) 261 | { 262 | 263 | for(int i=0;i 3 | 4 | predictive_configuration::predictive_configuration() 5 | { 6 | 7 | set_velocity_constraints_ = true; 8 | 9 | initialize_success_ = false; 10 | } 11 | 12 | predictive_configuration::~predictive_configuration() 13 | { 14 | free_allocated_memory(); 15 | } 16 | 17 | // read predicitve configuration paramter from paramter server 18 | bool predictive_configuration::initialize() //const std::string& node_handle_name 19 | { 20 | ros::NodeHandle nh_config;//("predictive_config"); 21 | ros::NodeHandle nh; 22 | 23 | // read paramter from parameter server if not set than terminate code, as this parameter is essential parameter 24 | if (!nh.getParam ("robot_base_link", robot_base_link_) ) 25 | { 26 | ROS_WARN(" Parameter 'robot_base_link' not set on %s node " , ros::this_node::getName().c_str()); 27 | return false; 28 | } 29 | 30 | if (!nh.getParam ("tracking_frame", tracking_frame_) ) 31 | { 32 | ROS_WARN(" Parameter 'tracking_frame' not set on %s node " , ros::this_node::getName().c_str()); 33 | return false; 34 | } 35 | 36 | if (!nh.getParam ("self_collision/collision_check_obstacles", collision_check_obstacles_) ) 37 | { 38 | ROS_WARN(" Parameter 'self_collision/collision_check_obstacles' not set on %s node please look at ../self_collision.yaml" , ros::this_node::getName().c_str()); 39 | collision_check_obstacles_.resize(degree_of_freedom_, std::string("")); 40 | } 41 | 42 | // read and set constraints 43 | // read and set velocity constrints 44 | if (!nh_config.getParam ("constraints/velocity_constraints/min", vel_min_limit_) ) 45 | { 46 | ROS_WARN(" Parameter '/constraints/velocity_constraints/min' not set on %s node" , ros::this_node::getName().c_str()); 47 | //constraining vx vy and w 48 | vel_min_limit_.resize(3, -1.0); 49 | set_velocity_constraints_ = false; 50 | 51 | for (int i = 0u; i < 3; ++i) 52 | { 53 | ROS_INFO("Velocity default min limit value %f", vel_min_limit_.at(i)); 54 | } 55 | } 56 | 57 | if (!nh_config.getParam ("constraints/velocity_constraints/max", vel_max_limit_) ) 58 | { 59 | ROS_WARN(" Parameter '/constraints/velocity_constraints/max' not set on %s node " , ros::this_node::getName().c_str()); 60 | vel_max_limit_.resize(3, 1.0); 61 | set_velocity_constraints_ = false; 62 | 63 | for (int i = 0u; i < 3 ; ++i) 64 | { 65 | ROS_INFO("Velocity default min limit value %f", vel_max_limit_.at(i)); 66 | } 67 | } 68 | 69 | // read and set other constraints... 70 | 71 | // read and set lsq state weight factors 72 | if (!nh_config.getParam ("acado_config/weight_factors/lsq_state_weight_factors", lsq_state_weight_factors_) ) 73 | { 74 | ROS_WARN(" Parameter 'acado_config/weight_factors/lsq_state_weight_factors' not set on %s node " , 75 | ros::this_node::getName().c_str()); 76 | // 3 position and 3 orientation(rpy) tolerance 77 | lsq_state_weight_factors_.resize(6, 5.0); 78 | 79 | for (int i = 0u; i < lsq_state_weight_factors_.size(); ++i) 80 | { 81 | ROS_INFO("Defualt lsq state weight factors value %f", lsq_state_weight_factors_.at(i)); 82 | } 83 | } 84 | 85 | // read and set lsq control weight factors 86 | if (!nh_config.getParam ("acado_config/weight_factors/lsq_control_weight_factors", lsq_control_weight_factors_) ) 87 | { 88 | ROS_WARN(" Parameter 'acado_config/weight_factors/lsq_control_weight_factors' not set on %s node " , 89 | ros::this_node::getName().c_str()); 90 | // same as degree of freedom 91 | lsq_control_weight_factors_.resize(degree_of_freedom_, 1.0); 92 | 93 | for (int i = 0u; i < lsq_control_weight_factors_.size(); ++i) 94 | { 95 | ROS_INFO("Defualt lsq control weight factors value %f", lsq_control_weight_factors_.at(i)); 96 | } 97 | } 98 | 99 | // Set optimal control problem dimensions 100 | if (!nh.getParam("state_dim", state_dim_) ) 101 | { 102 | ROS_WARN(" Parameter 'state_dim' not set on %s node " , ros::this_node::getName().c_str()); 103 | return false; 104 | } 105 | 106 | if (!nh.getParam ("control_dim", control_dim_) ) 107 | { 108 | ROS_WARN(" Parameter 'control_dim' not set on %s node " , ros::this_node::getName().c_str()); 109 | return false; 110 | } 111 | 112 | if (!nh.getParam ("output_cmd", output_cmd) ) 113 | { 114 | ROS_WARN(" Parameter 'output_cmd' not set on %s node " , ros::this_node::getName().c_str()); 115 | return false; 116 | } 117 | 118 | if (!nh.getParam ("robot_state_topic", robot_state_topic_) ) 119 | { 120 | ROS_WARN(" Parameter 'robot_state_topic' not set on %s node " , ros::this_node::getName().c_str()); 121 | return false; 122 | } 123 | 124 | if (!nh.getParam ("obstacles/n_obstacles", n_obstacles_) ) 125 | { 126 | ROS_WARN(" Parameter 'n_obstacles' not set on %s node " , ros::this_node::getName().c_str()); 127 | return false; 128 | } 129 | 130 | if (!nh.getParam ("obstacles/sub_ellipse_topic", sub_ellipse_topic_) ) 131 | { 132 | ROS_WARN(" Parameter 'robot_state_topic' not set on %s node " , ros::this_node::getName().c_str()); 133 | return false; 134 | } 135 | 136 | if (!nh.getParam ("obstacles/n_discs", n_discs_) ) 137 | { 138 | ROS_WARN(" Parameter 'n_discs' not set on %s node " , ros::this_node::getName().c_str()); 139 | return false; 140 | } 141 | 142 | if (!nh.getParam ("obstacles/ego_l", ego_l_) ) 143 | { 144 | ROS_WARN(" Parameter 'ego_l' not set on %s node " , ros::this_node::getName().c_str()); 145 | return false; 146 | } 147 | 148 | if (!nh.getParam ("obstacles/ego_w", ego_w_) ) 149 | { 150 | ROS_WARN(" Parameter 'ego_w' not set on %s node " , ros::this_node::getName().c_str()); 151 | return false; 152 | } 153 | 154 | // check requested parameter availble on parameter server if not than set default value 155 | nh.param("clock_frequency", clock_frequency_, double(50.0)); // 50 hz 156 | nh.param("sampling_time", sampling_time_, double(0.025)); // 0.025 second 157 | nh.param("activate_debug_output", activate_debug_output_, bool(false)); // debug 158 | nh.param("activate_controller_node_output", activate_controller_node_output_, bool(false)); // debug 159 | nh.param("plotting_result", plotting_result_, bool(false)); // plotting 160 | 161 | // self collision avoidance parameter 162 | nh_config.param("self_collision/ball_radius", ball_radius_, double(0.12)); // self collision avoidance ball radius 163 | nh_config.param("self_collision/minimum_collision_distance", minimum_collision_distance_, double(0.12)); // self collision avoidance minimum distance 164 | nh_config.param("self_collision/collision_weight_factor", collision_weight_factor_, double(0.01)); // self collision avoidance weight factor 165 | 166 | // acado configuration parameter 167 | nh_config.param("acado_config/max_num_iteration", max_num_iteration_, int(10)); // maximum number of iteration for slution of OCP 168 | nh_config.param("acado_config/discretization_intervals", discretization_intervals_, int(4)); // discretization_intervals for slution of OCP 169 | nh_config.param("acado_config/kkt_tolerance", kkt_tolerance_, double(1e-6)); // kkt condition for optimal solution 170 | nh_config.param("acado_config/integrator_tolerance", integrator_tolerance_, double(1e-8)); // intergrator tolerance 171 | nh_config.param("acado_config/start_time_horizon", start_time_horizon_, double(0.0)); // start time horizon for defining OCP problem 172 | nh_config.param("acado_config/end_time_horizon", end_time_horizon_, double(1.0)); // end time horizon for defining OCP problem 173 | nh_config.param("acado_config/use_LSQ_term", use_LSQ_term_, bool(false)); // use for minimize objective function 174 | nh_config.param("acado_config/use_lagrange_term", use_lagrange_term_, bool(false)); // use for minimize objective function 175 | nh_config.param("acado_config/use_mayer_term", use_mayer_term_, bool(true)); // use for minimize objective function 176 | 177 | initialize_success_ = true; 178 | 179 | if (activate_debug_output_) 180 | { 181 | print_configuration_parameter(); 182 | } 183 | 184 | ROS_WARN(" PREDICTIVE PARAMETER INITIALIZED!!"); 185 | return true; 186 | } 187 | 188 | // update configuration parameter 189 | bool predictive_configuration::updateConfiguration(const predictive_configuration &new_config) 190 | { 191 | activate_debug_output_ = new_config.activate_debug_output_; 192 | activate_controller_node_output_ = new_config.activate_controller_node_output_; 193 | initialize_success_ = new_config.initialize_success_; 194 | 195 | set_velocity_constraints_ = new_config.set_velocity_constraints_; 196 | 197 | plotting_result_ = new_config.plotting_result_; 198 | 199 | degree_of_freedom_ = new_config.degree_of_freedom_; 200 | robot_base_link_ = new_config.robot_base_link_; 201 | 202 | tracking_frame_ = new_config.tracking_frame_; 203 | 204 | 205 | collision_check_obstacles_ = new_config.collision_check_obstacles_; 206 | 207 | vel_min_limit_ = new_config.vel_min_limit_; 208 | vel_max_limit_ = new_config.vel_max_limit_; 209 | 210 | 211 | lsq_state_weight_factors_ = new_config.lsq_state_weight_factors_; 212 | lsq_control_weight_factors_ = new_config.lsq_control_weight_factors_; 213 | 214 | clock_frequency_ = new_config.clock_frequency_; 215 | sampling_time_ = new_config.sampling_time_; 216 | ball_radius_ = new_config.ball_radius_; 217 | minimum_collision_distance_ = new_config.minimum_collision_distance_; 218 | collision_weight_factor_ = new_config.collision_weight_factor_; 219 | 220 | use_lagrange_term_ = new_config.use_lagrange_term_; 221 | use_LSQ_term_ = new_config.use_LSQ_term_; 222 | use_mayer_term_ = new_config.use_mayer_term_; 223 | max_num_iteration_ = new_config.max_num_iteration_; 224 | discretization_intervals_ = new_config.discretization_intervals_; 225 | kkt_tolerance_ = new_config.kkt_tolerance_; 226 | integrator_tolerance_ = new_config.integrator_tolerance_; 227 | start_time_horizon_ = new_config.start_time_horizon_; 228 | end_time_horizon_ = new_config.end_time_horizon_; 229 | 230 | if (activate_debug_output_) 231 | { 232 | print_configuration_parameter(); 233 | } 234 | 235 | return initialize_success_; 236 | } 237 | 238 | // print all data member of this class 239 | void predictive_configuration::print_configuration_parameter() 240 | { 241 | ROS_INFO_STREAM("Activate_controller_node_output: " << std::boolalpha << activate_controller_node_output_); 242 | ROS_INFO_STREAM("Initialize_success: " << std::boolalpha << initialize_success_); 243 | 244 | ROS_INFO_STREAM("Set velocity constrints: " << std::boolalpha << set_velocity_constraints_); 245 | 246 | ROS_INFO_STREAM("Plotting results: " << std::boolalpha << plotting_result_); 247 | ROS_INFO_STREAM("Degree_of_freedom: " << degree_of_freedom_); 248 | ROS_INFO_STREAM("robot_base_link: " << robot_base_link_); 249 | 250 | ROS_INFO_STREAM("Clock_frequency: " << clock_frequency_); 251 | ROS_INFO_STREAM("Sampling_time: " << sampling_time_); 252 | ROS_INFO_STREAM("Ball_radius: " << ball_radius_); 253 | ROS_INFO_STREAM("Minimum collision distance: " << minimum_collision_distance_); 254 | ROS_INFO_STREAM("Collision weight factor: " << collision_weight_factor_); 255 | ROS_INFO_STREAM("Use lagrange term: " << std::boolalpha << use_lagrange_term_); 256 | ROS_INFO_STREAM("Use LSQ term: " << std::boolalpha << use_LSQ_term_); 257 | ROS_INFO_STREAM("Use mayer term: " << std::boolalpha << use_mayer_term_); 258 | ROS_INFO_STREAM("Max num iteration: " << max_num_iteration_); 259 | ROS_INFO_STREAM("Discretization intervals: " << discretization_intervals_); 260 | ROS_INFO_STREAM("KKT tolerance: " << kkt_tolerance_); 261 | ROS_INFO_STREAM("Integrator tolerance: " << integrator_tolerance_); 262 | ROS_INFO_STREAM("Start time horizon: " << start_time_horizon_); 263 | ROS_INFO_STREAM("End time horizon: " << end_time_horizon_); 264 | 265 | // print joints name 266 | std::cout << "self collision map: ["; 267 | for_each(collision_check_obstacles_.begin(), collision_check_obstacles_.end(), [](std::string& str) 268 | { 269 | std::cout << str << ", " ; 270 | } 271 | ); 272 | std::cout<<"]"< 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | try 8 | { 9 | ros::init(argc, argv, ros::this_node::getName()); 10 | MPCC controller_; 11 | 12 | // initialize predictive control node 13 | if (!controller_.initialize()) 14 | { 15 | ROS_ERROR_STREAM_NAMED("FILED TO INITIALIZE %s", ros::this_node::getName().c_str()); 16 | exit(1); 17 | } 18 | else 19 | { 20 | // spin node, till ROS node is running on 21 | ROS_INFO_STREAM_NAMED("%s INITIALIZE SUCCESSFULLY!!", ros::this_node::getName().c_str()); 22 | ros::spin(); 23 | } 24 | } 25 | 26 | catch (ros::Exception& e) 27 | { 28 | ROS_ERROR("predictive_control_node: Error occured: %s ", e.what()); 29 | exit(1); 30 | } 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /src/predictive_trajectory_generator.cpp: -------------------------------------------------------------------------------- 1 | 2 | //This file containts cost function intsert in to generated trajectory. 3 | 4 | #include 5 | 6 | pd_frame_tracker::~pd_frame_tracker() 7 | { 8 | clearDataMember(); 9 | } 10 | 11 | // diallocated memory 12 | void pd_frame_tracker::clearDataMember() 13 | { 14 | // resize matrix and vectors 15 | state_initialize_.resize(state_dim_); 16 | state_initialize_.setAll(0.0); 17 | control_initialize_.resize(control_dim_); 18 | control_initialize_.setAll(0.0); 19 | 20 | control_min_constraint_.resize(control_dim_); 21 | control_min_constraint_.setAll(0.0); 22 | control_max_constraint_.resize(control_dim_); 23 | control_max_constraint_.setAll(0.0); 24 | 25 | lsq_state_weight_factors_.resize(state_dim_); 26 | lsq_control_weight_factors_.resize(control_dim_); 27 | } 28 | 29 | // initialize data member of pd_frame_tracker class 30 | bool pd_frame_tracker::initialize() 31 | { 32 | // make sure predictive_configuration class initialized 33 | if (!predictive_configuration::initialize_success_) 34 | { 35 | predictive_configuration::initialize(); 36 | } 37 | 38 | // intialize data members 39 | state_initialize_.resize(state_dim_); 40 | state_initialize_.setAll(1E-5); 41 | control_initialize_.resize(control_dim_); 42 | control_initialize_.setAll(1E-5); 43 | 44 | // initialize acado configuration parameters 45 | max_num_iteration_ = predictive_configuration::max_num_iteration_; 46 | kkt_tolerance_ = predictive_configuration::kkt_tolerance_; 47 | integrator_tolerance_ = predictive_configuration::integrator_tolerance_; 48 | 49 | // initialize horizons, sampling time 50 | start_time_ = predictive_configuration::start_time_horizon_; 51 | end_time_ = predictive_configuration::end_time_horizon_; 52 | discretization_intervals_ = predictive_configuration::discretization_intervals_; 53 | sampling_time_ = predictive_configuration::sampling_time_; 54 | 55 | // optimaization type 56 | // use_lagrange_term_ = predictive_configuration::use_lagrange_term_; 57 | use_LSQ_term_ = predictive_configuration::use_LSQ_term_; 58 | use_mayer_term_ = predictive_configuration::use_mayer_term_; 59 | 60 | // initialize state and control weight factors 61 | lsq_state_weight_factors_ = transformStdVectorToEigenVector(predictive_configuration::lsq_state_weight_factors_); 62 | lsq_control_weight_factors_ = transformStdVectorToEigenVector(predictive_configuration::lsq_control_weight_factors_); 63 | 64 | //state_vector_size_ = predictive_configuration::lsq_state_weight_factors_.size(); 65 | //control_vector_size_ = predictive_configuration::lsq_control_weight_factors_.size(); 66 | state_vector_size_ = lsq_state_weight_factors_.rows()* lsq_state_weight_factors_.cols(); 67 | control_vector_size_ = lsq_control_weight_factors_.rows()*lsq_control_weight_factors_.cols(); 68 | horizon_steps_ = (int)(end_time_/sampling_time_); 69 | 70 | control_min_constraint_ = transformStdVectorToEigenVector(predictive_configuration::vel_min_limit_); 71 | control_max_constraint_ = transformStdVectorToEigenVector(predictive_configuration::vel_max_limit_); 72 | 73 | // self collision cost constant term 74 | self_collision_cost_constant_term_ = discretization_intervals_/ (end_time_-start_time_); 75 | 76 | //move to a kinematic function 77 | // Differential Kinematic 78 | iniKinematics(x_,v_); 79 | s_ = 0; 80 | 81 | // Initialize parameters concerning obstacles 82 | n_obstacles_ = predictive_configuration::n_obstacles_; 83 | 84 | // Compute ego discs for collision constraints 85 | computeEgoDiscs(); 86 | 87 | ROS_WARN("PD_FRAME_TRACKER INITIALIZED!!"); 88 | return true; 89 | } 90 | 91 | void pd_frame_tracker::iniKinematics(const DifferentialState& x, const Control& v){ 92 | 93 | ROS_WARN("pd_frame_tracker::iniKinematics"); 94 | 95 | f.reset(); 96 | f << dot(x(0)) == v(0)*cos(x(2)); 97 | f << dot(x(1)) == v(0)*sin(x(2)); 98 | f << dot(x(2)) == v(1); 99 | f << dot(x(3)) == v(0); 100 | } 101 | 102 | void pd_frame_tracker::computeEgoDiscs() 103 | { 104 | // Collect parameters for disc representation 105 | int n_discs = predictive_configuration::n_discs_; 106 | double length = predictive_configuration::ego_l_; 107 | double width = predictive_configuration::ego_w_; 108 | 109 | // Initialize positions of discs 110 | x_discs_.resize(n_discs); 111 | 112 | // Loop over discs and assign positions 113 | for ( int discs_it = 0; discs_it < n_discs; discs_it++){ 114 | x_discs_[discs_it] = -length/2 + (discs_it + 1)*(length/(n_discs + 1)); 115 | } 116 | 117 | // Compute radius of the discs 118 | r_discs_ = sqrt(pow(x_discs_[n_discs - 1] - length/2,2) + pow(width/2,2)); 119 | 120 | ROS_WARN_STREAM("Generated " << n_discs << " ego-vehicle discs with radius " << r_discs_); 121 | } 122 | 123 | void pd_frame_tracker::setCollisionConstraints(OCP& OCP_problem, const DifferentialState& x, const obstacle_feed::Obstacles& obstacles, const double& delta_t) { 124 | 125 | 126 | // Iterate over all given obstacles upto defined bound 127 | for (int obst_it = 0; obst_it < obstacles.Obstacles.size(); obst_it++) { 128 | // Iterate over all ego-vehicle discs 129 | for (int discs_it = 0; discs_it < predictive_configuration::n_discs_ && obst_it < n_obstacles_; discs_it++) { 130 | 131 | // Expression for position of obstacle 132 | double x_obst = obstacles.Obstacles[obst_it].pose.position.x; 133 | double y_obst = obstacles.Obstacles[obst_it].pose.position.y; 134 | 135 | // Size and heading of allipse 136 | double a = obstacles.Obstacles[obst_it].major_semiaxis + r_discs_; 137 | double b = obstacles.Obstacles[obst_it].minor_semiaxis + r_discs_; 138 | double phi = obstacles.Obstacles[obst_it].pose.orientation.z; // Orientation is an Euler angle 139 | 140 | // Distance from individual ego-vehicle discs to obstacle 141 | Expression deltaPos(2, 1); 142 | deltaPos(0) = x(0) - cos(x(2))*x_discs_[discs_it] - x_obst; 143 | deltaPos(1) = x(1) - sin(x(2))*x_discs_[discs_it] - y_obst; 144 | 145 | // deltaPos(0) = x(0) - x_obst; 146 | // deltaPos(1) = x(1) - y_obst; 147 | 148 | // Rotation matrix corresponding to the obstacle heading 149 | Expression R_obst(2, 2); 150 | R_obst(0, 0) = cos(phi-x(2)); 151 | R_obst(0, 1) = -sin(x(2)-x(2)); 152 | R_obst(1, 0) = sin(phi-x(2)); 153 | R_obst(1, 1) = cos(phi-x(2)); 154 | 155 | // Matrix with total clearance from obstacles 156 | Expression ab_mat(2, 2); 157 | ab_mat(0, 0) = 1 / (a * a); 158 | ab_mat(1, 1) = 1 / (b * b); 159 | ab_mat(0, 1) = 0; 160 | ab_mat(1, 0) = 0; 161 | 162 | // Total contraint on obstacle 163 | Expression c_k; 164 | c_k = deltaPos.transpose() * R_obst.transpose() * ab_mat * R_obst * deltaPos; 165 | // c_k = deltaPos.transpose() * ab_mat * deltaPos; 166 | 167 | // Add constraint to OCP problem 168 | OCP_problem.subjectTo(c_k >= 1); 169 | } 170 | } 171 | } 172 | 173 | // Generate cost function of optimal control problem 174 | void pd_frame_tracker::generateCostFunction(OCP& OCP_problem, 175 | const DifferentialState &x, 176 | const Control &v, 177 | const Eigen::Vector3d& goal_pose) 178 | { 179 | if (use_mayer_term_) 180 | { 181 | if (activate_debug_output_) 182 | { 183 | ROS_INFO("pd_frame_tracker::generateCostFunction: use_mayer_term_"); 184 | } 185 | Expression sqp = (lsq_state_weight_factors_(0) * ( (x(0) - goal_pose(0)) * (x(0) - goal_pose(0)) ) 186 | + (lsq_state_weight_factors_(1) * ( (x(1) - goal_pose(1)) * (x(1) - goal_pose(1)))) 187 | + lsq_state_weight_factors_(2) * ( (x(2) - goal_pose(2)) * (x(2) - goal_pose(2)))) 188 | + lsq_control_weight_factors_(0) * (v.transpose() * v) 189 | ; 190 | 191 | OCP_problem.minimizeMayerTerm( sqp ); 192 | } 193 | 194 | } 195 | 196 | VariablesGrid pd_frame_tracker::solveOptimalControlProblem(const Eigen::VectorXd &last_position, 197 | const Eigen::Vector3d &prev_pose, 198 | const Eigen::Vector3d &next_pose, 199 | const Eigen::Vector3d &goal_pose, 200 | const obstacle_feed::Obstacles &obstacles, 201 | geometry_msgs::Twist& controlled_velocity) 202 | { 203 | 204 | // control initialize with previous command 205 | control_initialize_(0) = controlled_velocity.linear.x; 206 | control_initialize_(1) = controlled_velocity.angular.z; 207 | 208 | // state initialize 209 | state_initialize_(0) = last_position(0); 210 | state_initialize_(1) = last_position(1); 211 | state_initialize_(2) = last_position(2); 212 | state_initialize_(3) = s_; 213 | 214 | obstacles_ = obstacles; 215 | 216 | // OCP variables 217 | // Optimal control problem 218 | OCP OCP_problem_(start_time_, end_time_, discretization_intervals_); 219 | 220 | //Equal constraints 221 | OCP_problem_.subjectTo(f); 222 | 223 | // generate cost function 224 | generateCostFunction(OCP_problem_, x_, v_, goal_pose); 225 | //path_function_spline_direct(OCP_problem_, x_, v_, goal_pose); 226 | 227 | setCollisionConstraints(OCP_problem_, x_, obstacles_, discretization_intervals_); 228 | 229 | // Optimal Control Algorithm 230 | RealTimeAlgorithm OCP_solver(OCP_problem_, 0.025); // 0.025 sampling time 231 | 232 | OCP_solver.initializeControls(control_initialize_); 233 | OCP_solver.initializeDifferentialStates(state_initialize_); 234 | 235 | setAlgorithmOptions(OCP_solver); 236 | 237 | OCP_solver.solve(0.0,state_initialize_); 238 | 239 | OCP_solver.getDifferentialStates(pred_states); 240 | 241 | 242 | // get control at first step and update controlled velocity vector 243 | 244 | OCP_solver.getU(u); 245 | 246 | if (u.size()>0){ 247 | controlled_velocity.linear.x = u(0); 248 | controlled_velocity.angular.z = u(1); 249 | 250 | // Simulate path varible 251 | // this should be later replaced by a process 252 | 253 | s_ += u(0) * sampling_time_; 254 | } 255 | 256 | return pred_states; 257 | } 258 | 259 | // setup acado algorithm options, need to set solver when calling this function 260 | void pd_frame_tracker::setAlgorithmOptions(RealTimeAlgorithm& OCP_solver) 261 | { 262 | /* // intergrator 263 | OCP_solver.set(INTEGRATOR_TYPE, LEVENBERG_MARQUARDT); // default INT_RK45: (INT_RK12, INT_RK23, INT_RK45, INT_RK78, INT_BDF) 264 | OCP_solver.set(INTEGRATOR_TOLERANCE, 1e-5); // default 1e-8 265 | 266 | // SQP method 267 | OCP_solver.set(HESSIAN_APPROXIMATION, EXACT_HESSIAN); // default BLOCK_BFGS_UPDATE: (CONSTANT_HESSIAN, FULL_BFGS_UPDATE, BLOCK_BFGS_UPDATE, GAUSS_NEWTON, EXACT_HESSIAN) 268 | OCP_solver.set(MAX_NUM_ITERATIONS, 10); // default 1000 269 | //OCP_solver.set(KKT_TOLERANCE, predictive_configuration::kkt_tolerance_); // default 1e-6 270 | // OCP_solver.set(HOTSTART_QP, true); // default true 271 | // OCP_solver.set(SPARSE_QP_SOLUTION, CONDENSING); // CONDENSING, FULL CONDENSING, SPARSE SOLVER 272 | 273 | // Discretization Type 274 | OCP_solver.set(DISCRETIZATION_TYPE, COLLOCATION); // default MULTIPLE_SHOOTING: (SINGLE_SHOOTING, MULTIPLE_SHOOTING) 275 | //OCP_solver.set(TERMINATE_AT_CONVERGENCE, true); // default true 276 | */ 277 | // output 278 | OCP_solver.set(PRINTLEVEL, NONE); // default MEDIUM (NONE, MEDIUM, HIGH) 279 | OCP_solver.set(PRINT_SCP_METHOD_PROFILE, false); // default false 280 | OCP_solver.set(PRINT_COPYRIGHT, false); // default true 281 | 282 | 283 | OCP_solver.set(MAX_NUM_ITERATIONS, max_num_iteration_); 284 | OCP_solver.set(LEVENBERG_MARQUARDT, integrator_tolerance_); 285 | OCP_solver.set( HESSIAN_APPROXIMATION, EXACT_HESSIAN ); 286 | OCP_solver.set( DISCRETIZATION_TYPE, COLLOCATION); 287 | OCP_solver.set(KKT_TOLERANCE, kkt_tolerance_); 288 | OCP_solver.set(HOTSTART_QP, true); // default true 289 | OCP_solver.set(SPARSE_QP_SOLUTION, CONDENSING); // CONDENSING, FULL CONDENSING, SPARSE SOLVER 290 | 291 | 292 | OCP_solver.set(INFEASIBLE_QP_HANDLING, defaultInfeasibleQPhandling); 293 | OCP_solver.set(INFEASIBLE_QP_RELAXATION, defaultInfeasibleQPrelaxation); 294 | 295 | // Discretization Type 296 | //OCP_solver.set(TERMINATE_AT_CONVERGENCE, true); // default true 297 | } 298 | 299 | 300 | /* 301 | // setup acado algorithm options, need to set solver when calling this function 302 | template 303 | void pd_frame_tracker::setAlgorithmOptions(boost::shared_ptr OCP_solver) 304 | { 305 | // intergrator 306 | OCP_solver->set(INTEGRATOR_TYPE, LEVENBERG_MARQUARDT); // default INT_RK45: (INT_RK12, INT_RK23, INT_RK45, INT_RK78, INT_BDF) 307 | OCP_solver->set(INTEGRATOR_TOLERANCE, predictive_configuration::integrator_tolerance_); // default 1e-8 308 | 309 | // SQP method 310 | OCP_solver->set(HESSIAN_APPROXIMATION, EXACT_HESSIAN); // default BLOCK_BFGS_UPDATE: (CONSTANT_HESSIAN, FULL_BFGS_UPDATE, BLOCK_BFGS_UPDATE, GAUSS_NEWTON, EXACT_HESSIAN) 311 | OCP_solver->set(MAX_NUM_ITERATIONS, predictive_configuration::max_num_iteration_); // default 1000 312 | OCP_solver->set(KKT_TOLERANCE, predictive_configuration::kkt_tolerance_); // default 1e-6 313 | // OCP_solver->set(HOTSTART_QP, true); // default true 314 | // OCP_solver->set(SPARSE_QP_SOLUTION, CONDENSING); // CONDENSING, FULL CONDENSING, SPARSE SOLVER 315 | 316 | // Discretization Type 317 | OCP_solver->set(DISCRETIZATION_TYPE, MULTIPLE_SHOOTING); // default MULTIPLE_SHOOTING: (SINGLE_SHOOTING, MULTIPLE_SHOOTING) 318 | OCP_solver->set(TERMINATE_AT_CONVERGENCE, true); // default true 319 | 320 | // output 321 | OCP_solver->set(PRINTLEVEL, NONE); // default MEDIUM (NONE, MEDIUM, HIGH) 322 | OCP_solver->set(PRINT_SCP_METHOD_PROFILE, false); // default false 323 | OCP_solver->set(PRINT_COPYRIGHT, false); // default true 324 | 325 | } 326 | */ 327 | 328 | /* 329 | // !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 330 | MatrixVariablesGrid: 331 | shiftBackwards (Matrix lastValue=emptyMatrix) 332 | Shifts all grid points backwards by one grid point, deleting the first one and doubling the value at last grid point. 333 | shiftTimes (double timeShift) 334 | Shifts times at all grid points by a given offset. 335 | Optimization Algorithm: 336 | double getObjectiveValue () const 337 | returnValue getSensitivitiesX 338 | // add NLP solver options 339 | addOption( MAX_NUM_ITERATIONS , defaultMaxNumIterations ); 340 | addOption( KKT_TOLERANCE , defaultKKTtolerance ); 341 | addOption( KKT_TOLERANCE_SAFEGUARD , defaultKKTtoleranceSafeguard ); 342 | addOption( LEVENBERG_MARQUARDT , defaultLevenbergMarguardt ); 343 | addOption( HESSIAN_PROJECTION_FACTOR , defaultHessianProjectionFactor ); 344 | addOption( PRINTLEVEL , defaultPrintlevel ); 345 | addOption( PRINT_COPYRIGHT , defaultPrintCopyright ); 346 | addOption( HESSIAN_APPROXIMATION , defaultHessianApproximation ); 347 | addOption( DYNAMIC_HESSIAN_APPROXIMATION, defaultDynamicHessianApproximation ); 348 | addOption( DYNAMIC_SENSITIVITY , defaultDynamicSensitivity ); 349 | addOption( OBJECTIVE_SENSITIVITY , defaultObjectiveSensitivity ); 350 | addOption( CONSTRAINT_SENSITIVITY , defaultConstraintSensitivity ); 351 | addOption( DISCRETIZATION_TYPE , defaultDiscretizationType ); 352 | addOption( LINESEARCH_TOLERANCE , defaultLinesearchTolerance ); 353 | addOption( MIN_LINESEARCH_PARAMETER , defaultMinLinesearchParameter ); 354 | addOption( MAX_NUM_QP_ITERATIONS , defaultMaxNumQPiterations ); 355 | addOption( HOTSTART_QP , defaultHotstartQP ); 356 | addOption( INFEASIBLE_QP_RELAXATION , defaultInfeasibleQPrelaxation ); 357 | addOption( INFEASIBLE_QP_HANDLING , defaultInfeasibleQPhandling ); 358 | addOption( USE_REALTIME_ITERATIONS , defaultUseRealtimeIterations ); 359 | addOption( TERMINATE_AT_CONVERGENCE , defaultTerminateAtConvergence ); 360 | addOption( SPARSE_QP_SOLUTION , defaultSparseQPsolution ); 361 | addOption( GLOBALIZATION_STRATEGY , defaultGlobalizationStrategy ); 362 | addOption( PRINT_SCP_METHOD_PROFILE , defaultprintSCPmethodProfile ); 363 | // add integration options 364 | addOption( FREEZE_INTEGRATOR , defaultFreezeIntegrator ); 365 | addOption( INTEGRATOR_TYPE , defaultIntegratorType ); 366 | addOption( FEASIBILITY_CHECK , defaultFeasibilityCheck ); 367 | addOption( PLOT_RESOLUTION , defaultPlotResoltion ); 368 | 369 | // add integrator options 370 | addOption( MAX_NUM_INTEGRATOR_STEPS , defaultMaxNumSteps ); 371 | addOption( INTEGRATOR_TOLERANCE , defaultIntegratorTolerance ); 372 | addOption( ABSOLUTE_TOLERANCE , defaultAbsoluteTolerance ); 373 | addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize ); 374 | addOption( MIN_INTEGRATOR_STEPSIZE , defaultMinStepsize ); 375 | addOption( MAX_INTEGRATOR_STEPSIZE , defaultMaxStepsize ); 376 | addOption( STEPSIZE_TUNING , defaultStepsizeTuning ); 377 | addOption( CORRECTOR_TOLERANCE , defaultCorrectorTolerance ); 378 | addOption( INTEGRATOR_PRINTLEVEL , defaultIntegratorPrintlevel ); 379 | addOption( LINEAR_ALGEBRA_SOLVER , defaultLinearAlgebraSolver ); 380 | addOption( ALGEBRAIC_RELAXATION , defaultAlgebraicRelaxation ); 381 | addOption( RELAXATION_PARAMETER , defaultRelaxationParameter ); 382 | addOption( PRINT_INTEGRATOR_PROFILE , defaultprintIntegratorProfile ); 383 | */ 384 | -------------------------------------------------------------------------------- /srv/GetFrameTrackingInfo.srv: -------------------------------------------------------------------------------- 1 | string data 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /srv/StaticCollisionObject.srv: -------------------------------------------------------------------------------- 1 | # static collision object request 2 | 3 | # add static object from file 4 | string file_name 5 | 6 | # the name of the object 7 | string object_name 8 | string object_id 9 | 10 | # position of object 11 | geometry_msgs/PoseStamped primitive_pose 12 | 13 | # dimension of object 14 | geometry_msgs/Vector3 dimension 15 | 16 | # current position of robot/other object, current poition means it should be center of object 17 | #geometry_msgs/Point current_position 18 | 19 | # threshold value used to be keep distance from robot body/other object 20 | #geometry_msgs/Vector3 distance_threshold 21 | 22 | # collision cost weighting factor 23 | #float64 collision_cost_weight_factor 24 | --- 25 | # Respose of static collision object service 26 | #float64[] static_collision_cost_vector 27 | bool success 28 | string message 29 | 30 | 31 | -------------------------------------------------------------------------------- /srv/StaticObstacle.srv: -------------------------------------------------------------------------------- 1 | # static collision object request 2 | moveit_msgs/CollisionObject static_collision_object 3 | string file_name 4 | --- 5 | # add successfully into environment 6 | bool success 7 | string message 8 | --------------------------------------------------------------------------------