├── .gitignore
├── CMakeLists.txt
├── LICENSE
├── README.md
├── doc
├── mainpage.dox
├── rosdoc.yaml
└── wiki
├── msg
└── VrepInfo.msg
├── package.xml
└── src
├── Agent.cpp
├── Agent.h
├── CMakeLists.txt
├── Constraint.cpp
├── Constraint.h
├── Coupling.cpp
├── Coupling.h
├── Event.cpp
├── Event.h
├── Indexing.h
├── MathLib.cpp
├── MathLib.h
├── Scheduler.cpp
├── Scheduler.h
├── controller
├── Cmscgmres.cpp
├── Cmscgmres.h
├── Controller.cpp
└── Controller.h
├── user_generated
├── Ardrone_20170227.cpp
├── Ardrone_20170227.h
├── CollisionAvoidanceCoupling_20170227.cpp
├── CollisionAvoidanceCoupling_20170227.h
├── OrientationConstraint_20170227.cpp
└── OrientationConstraint_20170227.h
└── user_scenarios
├── ardrone_pose_tracking.cpp
├── ardrone_sensor_tracking.cpp
└── scenario.cpp
/.gitignore:
--------------------------------------------------------------------------------
1 | # Prerequisites
2 | *.d
3 |
4 | # Compiled Object files
5 | *.slo
6 | *.lo
7 | *.o
8 | *.obj
9 |
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 |
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 |
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 |
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 |
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 |
--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | cmake_minimum_required(VERSION 2.8.3)
2 |
3 | project(denmpc)
4 | add_definitions(-std=c++11)
5 |
6 | find_package(catkin REQUIRED)
7 | find_package(catkin REQUIRED COMPONENTS
8 | roscpp
9 | rospy
10 | std_msgs
11 | message_generation
12 | )
13 |
14 | ## System dependencies are found with CMake's conventions
15 | find_package(Boost REQUIRED COMPONENTS system)
16 |
17 | ################################################
18 | ## Declare ROS messages, services and actions ##
19 | ################################################
20 |
21 | ## Generate messages in the 'msg' folder
22 | add_message_files( FILES VrepInfo.msg)
23 |
24 | ## Generate added messages and services with any dependencies listed here
25 | generate_messages(
26 | DEPENDENCIES
27 | std_msgs
28 | )
29 |
30 | ###################################
31 | ## catkin specific configuration ##
32 | ###################################
33 |
34 | catkin_package(
35 | INCLUDE_DIRS ${catkin_INCLUDE_DIRS}
36 | # LIBRARIES symbolic
37 | CATKIN_DEPENDS roscpp rospy std_msgs message_runtime
38 | DEPENDS Boost
39 | )
40 |
41 | ###########
42 | ## Build ##
43 | ###########
44 |
45 | ###################################################################
46 | include_directories(
47 | ${catkin_INCLUDE_DIRS}
48 | src/
49 | src/user_generated
50 | src/user_scenarios
51 | src/controller
52 | include
53 | )
54 | SET( LIB_CONTROLLER_DIR src/user_generated)
55 | set(LIB_CONTROLLER_SOURCES
56 | src/Agent.cpp
57 | src/Constraint.cpp
58 | src/Coupling.cpp
59 | src/controller/Controller.cpp
60 | src/controller/Cmscgmres.cpp
61 | src/MathLib.cpp
62 | src/Event.cpp
63 | src/Scheduler.cpp
64 | )
65 | FILE( GLOB CONTROLLERSOURCES ${LIB_CONTROLLER_DIR}/*.cpp )
66 | FOREACH( SRC ${CONTROLLERSOURCES} )
67 | SET( LIB_CONTROLLER_SOURCES ${LIB_CONTROLLER_SOURCES} ${SRC} )
68 | ENDFOREACH( SRC ${CONTROLLERSOURCES} )
69 | MESSAGE( STATUS "Generated Controller Sources: " ${CONTROLLERSOURCES} )
70 |
71 |
72 |
73 |
74 | SET( LIB_USER_DIR src/user_scenarios)
75 | FILE( GLOB USER_CODE ${LIB_USER_DIR}/*.cpp )
76 | MESSAGE( STATUS "All USER Sources: " ${USER_CODE} )
77 |
78 | FOREACH( SRC ${USER_CODE} )
79 | SET(NODENAME ${SRC})
80 | get_filename_component(NODENAME ${SRC} NAME_WE)
81 | SET(NODENAME scenario_${NODENAME}_node)
82 | add_executable(${NODENAME} ${SRC} ${LIB_CONTROLLER_SOURCES})
83 | add_dependencies(${NODENAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
84 | target_link_libraries(${NODENAME} ${catkin_LIBRARIES} )
85 | ENDFOREACH( SRC ${USER_CODE} )
86 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | #DENMPC
2 |
3 | @file README.md
4 | @Author Jan Dentler (jan.dentler@uni.lu)
5 | University of Luxembourg
6 | @date 26.October, 2017
7 | @time 16:14h
8 | @license GPLv3
9 | @brief README
10 |
11 | ## Outline
12 | DENMPC is providing an object-oriented real-time nonlinear model predictive control (NMPC) framework which has been developed at the Automation & Robotics Research Group http://wwwde.uni.lu/snt/research/automation_robotics_research_group at the University of Luxembourg.
13 |
14 | The basic idea of DENMPC is to provide a fast nonlinear MPC that can adjust at runtime to different systems.
15 | This refers to:
16 | * Multi-agent systems that can change tasks, objectives and topology
17 | * Fault-tolerant control, where the controller has to adapt to different system conditions
18 | * Control prototyping, where you want to explore different scenarios without creating the underlying Optimal Control Problem (OCP) from scratch
19 |
20 | In order to do so, DENMPC features an object-oriented modularization approach.
21 | This allows structuring the control scenario into agents, constraints and couplings.
22 | Out of these single components, DENMPC is dynamically creating the OCP at runtime.
23 | As a result, agents, constraints and couplings can be added, removed, and parameters can be changed at runtime. This addition, respectively subtraction is triggered by events
24 | which can be for example timer events, ROS-messages events, etc.
25 | For very complex tasks, this can further be used to combine step chains with DENMPC,
26 | to specialize the MPC for each task stage individually.
27 |
28 | ##Literature and Publication
29 | DENMPC is open-source software, available under available under [https://github.com/snt-robotics/denmpc](https://github.com/snt-robotics/denmpc) and [https://github.com/DentOpt/denmpc](https://github.com/DentOpt/denmpc). The usage of DENMPC use regulated under the terms of the GPL3 license (Proprietary licences are available under request.). If you are using the software in your research work, you are supposed to cite one or more of the following references:
30 |
31 | J. Dentler,
32 | "Real-time Model Predictive Control of Cooperative Aerial Manipulation",
33 | [http://orbilu.uni.lu/handle/10993/36965](http://orbilu.uni.lu/handle/10993/36965),
34 | PhD Thesis, University of Luxembourg, July 2018
35 |
36 | Jan Dentler, Somasundar Kannan, Souad Bezzaoucha, Miguel Angel Olivares-Mendez, and Holger Voos,
37 | Model predictive cooperative localization control of multiple UAVs using potential function sensor constraints.
38 | Autonomous Robots, March 2018, pages 1–26.
39 | doi: 10.1007/s10514-018-9711-z, url: https://doi.org/10.1007/s10514-018-9711-z
40 |
41 | J. Dentler, S. Kannan, M. A. O. Mendez and H. Voos,
42 | "A modularization approach for nonlinear model predictive control of distributed fast systems",
43 | 24th Mediterranean Conference on Control and Automation (MED), Athens, Greece, 2016, pp. 292-297.
44 | doi: 10.1109/MED.2016.7535973
45 |
46 | Jan Dentler and Somasundar Kannan and Miguel Angel Olivares Mendez and Holger Voos,
47 | "A real-time model predictive position control with collision avoidance for commercial low-cost quadrotors",
48 | Proceedings of 2016 IEEE Multi-Conference on Systems and Control (MSC 2016), Argentina, Buenos Aires, 2016
49 |
50 | If you are using the "Condensed Multiple Shooting Generalized Minimal Residuum Method (CMSCGMRES)" kernel contributed by the team of Prof. Dr. Toshiyuki OHTSUKA, please refer to:
51 |
52 | Ohtsuka, T.,
53 | “A Continuation/GMRES Method for Fast Computation of Nonlinear Receding Horizon Control,”
54 | Automatica, Vol. 40, No. 4, Apr. 2004, pp. 563-574.
55 |
56 | Seguchi, H., and Ohtsuka, T.,
57 | “Nonlinear Receding Horizon Control of an Underactuated Hovercraft,”
58 | International Journal of Robust and Nonlinear Control, Vol. 13, Nos. 3-4, Mar.-Apr. 2003, pp. 381-398.
59 |
60 | ## DENMPC features:
61 |
62 | Nonlinear model predictive control (e.g. a quadrotor with nonlinear system dynamics)
63 | Central control of single-agent systems (e.g. a single robot)
64 | Central control of multi-agent systems (e.g. multiple robots that are interacting)
65 | Object-oriented code to easily adapt it:
66 | Controller: Interface class for implementations of controllers, e.g.CMSCGMRES
67 | Agent: Interface class for implementations of agents, respective system or robot types, e.g. Quadrotor
68 | Constraint: Interface class for implementations of single-agent constraints
69 | Coupling: Interface class for implementations for coupling agents
70 | Open-source code
71 |
72 |
73 | ## Installation
74 |
75 | # Navigate to your ROS catkin workspace (e.g. catkin_ws):`
76 | cd catkin_ws/src
77 | #Clone repository
78 | git clone https://github.com/DentOpt/denmpc.git
79 | cd ..
80 | #Build package
81 | catkin_make
82 |
83 | ### To use the AR.Drone 2.0 scenario with the tum simulator
84 | Install:
85 |
86 | cd catkin_ws/src
87 | git clone https://github.com/DentOpt/ardrone_simulator_gazebo7.git
88 | cd ..
89 | catkin_make
90 |
91 | To run the AR.Drone 2.0 scenario in gazebo, run
92 |
93 | roslaunch cvg_sim\_gazebo ardrone_testworld.launch
94 |
95 | Launch drone (Takeoff) from commandline:
96 |
97 | rostopic pub -1 /ardrone/takeoff std_msgs/Empty
98 |
99 | The AR.Drone 2.0 simulator is configured to subscribe control commands under the topic "/cmd_vel".
100 | The AR.Drone 2.0 pose is published under "/pose"
101 |
102 |
103 | To control the AR.Drone 2.0 in with denmpc:
104 | either to track center of UAV:
105 |
106 | rosrun denmpc scenario_ardrone_pose_tracking_node
107 |
108 | or to track with sensor constraint:
109 |
110 | rosrun denmpc scenario_ardrone_sensor_tracking_node
111 |
112 | and to send desired pose use rqt or commandline, e.g
113 |
114 | rostopic pub /desiredpose geometry_msgs/PoseStamd '{header: {stamp: now, frame_id: "map"}, pose: {position: {x: 0.0, y: 0.0, z: 2.0}, orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}}}'
115 |
116 | ### To use the Turtlebot scenario
117 | Install:
118 |
119 | cd catkin_ws/src
120 | sudo apt-get install
121 | git clone https://github.com/ros/ros_tutorials.git #Install Turtlesim
122 | git clone https://github.com/DentOpt/denmpc.git -b tutorial_turtlesim #Install DENMPC branch
123 | cd ..
124 | catkin_make
125 |
126 | Run:
127 |
128 | roscore #run roscore
129 | rosrun turtlesim turtlesim_node #Run Turtlesim in separate tab
130 | rosrun denmpc scenario_scenario_node #Run denmpc in separate tab
131 |
132 | That's it!
133 | You will see how the turtle DENMPC moves from its initial position to the position x=1 y=1.
134 | You can give any desired position by publishing it to the /turtle1/desiredpose.
135 | For example, for the new target x=5, y=5 type
136 | `rostopic pub /turtle1/desiredpose turtlesim/Pose "{x: 5.0, y: 5.0, theta: 0.0, linear_velocity: 0.0, angular_velocity: 0.0}"`
137 |
138 |
139 |
--------------------------------------------------------------------------------
/doc/mainpage.dox:
--------------------------------------------------------------------------------
1 | /**
2 | \mainpage
3 | \htmlinclude manifest.html
4 |
5 | \b dentopt is ...
6 | \author Jan Dentler
7 |
8 | This package is providing an object oriented model predictive control (MPC) framework
9 | with the following features :
10 | - real-time control of nonlinear systems based on a "Condensed Multiple Shooting Generalized Minimal Residuum Method (CMSCGMRES)" developed by
11 | 1. Ohtsuka, T., “A Continuation/GMRES Method for Fast Computation of Nonlinear Receding Horizon Control,” Automatica, Vol. 40, No. 4, Apr. 2004, pp. 563-574.
12 | 2. Seguchi, H., and Ohtsuka, T., “Nonlinear Receding Horizon Control of an Underactuated Hovercraft,” International Journal of Robust and Nonlinear Control, Vol. 13, Nos. 3-4, Mar.-Apr. 2003, pp. 381-398.
13 | - central control of multi-agent systems
14 | - object oriented code to easily adapt it
15 | - open-source code
16 |
17 | \section codeapi Code API
18 |
19 | Controller:
20 | This base class containes the routines to implement and adjust controllers.
21 |
22 | Cmscgmres:
23 | This class containes the routines of the CMSCGMRES solver.
24 | This solver is based on a newton-step with approximation of the Hessian via a forward difference approach
25 |
26 | Agent:
27 | This base class containes the routines to implement and adjust agents.
28 |
29 | Constraint:
30 | This base class containes the routines to implement and adjust constraints.
31 |
32 | Coupling:
33 | This base class containes the routines to implement and adjust couplings.
34 |
35 | Scheduler:
36 | This class containes the routines to handle the agent and controller communication.
37 |
38 | */
39 |
--------------------------------------------------------------------------------
/doc/rosdoc.yaml:
--------------------------------------------------------------------------------
1 | - builder: doxygen
2 | name: C++ API
3 | output_dir: c++
4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
5 | exclude_patterns: ''
6 |
--------------------------------------------------------------------------------
/doc/wiki:
--------------------------------------------------------------------------------
1 | This package is providing an object oriented real-time nonlinear model predictive control (NMPC) framework developed at the Automation & Robotics Research Group http://wwwde.uni.lu/snt/research/automation_robotics_research_group at the University of Luxembourg.
2 |
3 | It is based on following contributions:
4 |
5 | * J. Dentler, S. Kannan, M. A. O. Mendez and H. Voos, <
>"A modularization approach for nonlinear model predictive control of distributed fast systems", <
>24th Mediterranean Conference on Control and Automation (MED), Athens, Greece, 2016, pp. 292-297.<
>doi: 10.1109/MED.2016.7535973
6 | * Jan Dentler and Somasundar Kannan and Miguel Angel Olivares Mendez and Holger Voos, <
>"A real-time model predictive position control with collision avoidance for commercial low-cost quadrotors", <
>Proceedings of 2016 IEEE Multi-Conference on Systems and Control (MSC 2016), Argentina, Buenos Aires, 2016
7 |
8 | The used kernel solver is a "Condensed Multiple Shooting Generalized Minimal Residuum Method (CMSCGMRES)" developed by Prof. Dr. Toshiyuki OHTSUKA:
9 |
10 | * Ohtsuka, T., <
>“A Continuation/GMRES Method for Fast Computation of Nonlinear Receding Horizon Control,” <
>Automatica, Vol. 40, No. 4, Apr. 2004, pp. 563-574.
11 | * Seguchi, H., and Ohtsuka, T., <
>“Nonlinear Receding Horizon Control of an Underactuated Hovercraft,” <
>International Journal of Robust and Nonlinear Control, Vol. 13, Nos. 3-4, Mar.-Apr. 2003, pp. 381-398.
12 |
13 | Its features include:
14 |
15 | 1. nonlinear model predictive control (e.g. a quadrotor with nonlinear system dynamics)
16 | 1. Central control of single agent systems (e.g. a single robot)
17 | 1. Central control of multi-agent systems (e.g. multiple robots that are interacting)
18 | 1. Object oriented code to easily adapt it:
19 | * Controller: Interface class for implementations of controllers, e.g.CMSCGMRES
20 | * Agent: Interface class for implementations of agents, respective system or robot types, e.g. Quadrotor
21 | * Constraint: Interface class for implementations of single agent constraints
22 | * Coupling: Interface class for implementations for coupling agents
23 | 1. Open-source code
24 |
25 |
--------------------------------------------------------------------------------
/msg/VrepInfo.msg:
--------------------------------------------------------------------------------
1 | std_msgs/Header headerInfo
2 | std_msgs/Int32 simulatorState
3 | std_msgs/Float32 simulationTime
4 | std_msgs/Float32 timeStep
5 |
--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
1 |
2 |
3 | denmpc
4 | 1.0.0
5 | A real-time nonlinear model predictive control package for single and multi-agent systems
6 |
7 | Jan Dentler
8 | Jan DENTLER
9 | GPLv3
10 |
11 | http://wiki.ros.org/wiki/dentopt_nmpc_controller
12 |
13 |
14 |
15 |
16 |
17 |
18 | message_generation
19 |
20 |
21 |
22 | message_runtime
23 |
24 |
25 | catkin
26 | roscpp
27 | rospy
28 | std_msgs
29 | roscpp
30 | rospy
31 | std_msgs
32 |
33 |
34 |
35 |
36 |
37 |
38 |
--------------------------------------------------------------------------------
/src/Agent.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Agent.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Agent Container
9 | *
10 | * Agent represents the container class for all types of agents
11 | * which can be any entity/system that can be described with an Differential equation.
12 | * E.g. robots, drones, plants, etc.
13 | */
14 |
15 | #include "Agent.h"
16 |
17 | Agent::Agent(){
18 | std::cout<<"Agent::Agent()"< init_x,
25 | // std::vector init_xdes,
26 | // std::vector init_u,
27 | // std::vector init_udes,
28 | // std::vector init_y,
29 | // std::vector init_ydes,
30 | // std::vector init_p){
31 | // std::cout<<"Agent::Agent(id,x,xdes,u,udes,y,ydes,p)"<setAgent1(this);
61 | coupling_.push_back(_couplingconstraint);
62 | }
63 | void Agent::addCouplingAsAgent2(Coupling* _couplingconstraint){
64 | _couplingconstraint->setAgent2(this);
65 | coupling_.push_back(_couplingconstraint);
66 | }
67 | void Agent::removeCoupling(Coupling* _couplingconstraint){
68 | //check arrays for the constraint and remove them
69 | coupling_.erase(std::remove(coupling_.begin(), coupling_.end(), _couplingconstraint),coupling_.end());
70 | }
71 |
72 |
73 | void Agent::addController(Controller* _controller){
74 | controller_.push_back(_controller);
75 | }
76 | void Agent::removeController(Controller* _controller){
77 | //check array for the controller and remove it
78 | controller_.erase(std::remove(controller_.begin(), controller_.end(), _controller), controller_.end());
79 | }
80 |
81 | //Set initial states to states
82 | void Agent::reset2initialstate(){
83 | setState (x_init_);
84 | setControl (u_init_);
85 | setOutput (y_init_);
86 | setDisturbance (d_init_);
87 | setDesiredState (xdes_init_);
88 | setDesiredControl (udes_init_);
89 | setDesiredOutput (ydes_init_);
90 | setParameter (p_init_);
91 | }
92 |
--------------------------------------------------------------------------------
/src/Agent.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Agent.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Agent Container
9 | *
10 | * Agent represents the container class for all types of agents
11 | * which can be any entity/system that can be described with an Differential equation.
12 | * E.g. robots, drones, plants, etc.
13 | */
14 |
15 | #ifndef _AGENT_H
16 | #define _AGENT_H
17 |
18 | //#define DEBUG_CGMRES
19 | //#define DEBUG_FUNCTION_TRACE
20 | //#define AGENT_INFO
21 |
22 | #define INPUTDATA_CHECK
23 |
24 | //#include "Coupling.h"
25 | #include
26 | #include
27 | #include //For vector
28 | #include //For string
29 | #include //For vector
30 | #include //For String Messages
31 | #include "MathLib.h"
32 | #include "Constraint.h"
33 | #include "Controller.h"
34 | #include "Indexing.h"
35 |
36 | using namespace MathLib;
37 |
38 | //Forward Declaration
39 | class Constraint;
40 | class Coupling;
41 | class Controller;
42 |
43 | class Agent:public AgentIndex{
44 | friend class Controller;
45 | protected:
46 | //=================================
47 | //Variables
48 | int id_; //ID of the agent
49 | std::string name_; //Name of the agent
50 | bool flag_record_; //Flag for recording
51 | bool flag_display_; //Flag for display
52 | //States
53 | double* x_; //States
54 | double* u_; //Controls
55 | double* y_; //Outputs
56 | double* d_; //Disturbance
57 | double* xdes_; //Desired States
58 | double* udes_; //Desired Controls
59 | double* ydes_; //Desired Outputs
60 | double* p_; //Parameters
61 |
62 | //Limits
63 | // double* x_limu_; //States upper limit
64 | // double* u_limu_; //Controls upper limit
65 | // double* y_limu_; //Outputs upper limit
66 | // double* x_liml_; //States lower limit
67 | // double* u_liml_; //Controls lower limit
68 | // double* y_liml_; //Outputs lower limit
69 | //Initial States
70 | double* x_init_; //States
71 | double* u_init_; //Controls
72 | double* y_init_; //Outputs
73 | double* d_init_; //Disturbance
74 | double* xdes_init_; //Desired States
75 | double* udes_init_; //Desired Controls
76 | double* ydes_init_; //Desired Outputs
77 | double* p_init_; //Parameters
78 | //State Dimensions
79 | int dim_x_;
80 | int dim_u_;
81 | int dim_y_;
82 | int dim_d_;
83 | int dim_xdes_;
84 | int dim_udes_;
85 | int dim_ydes_;
86 | int dim_p_;
87 | int dim_ineq_;
88 | int dim_eq_;
89 | int dim_l_;
90 | int dim_v_;
91 | //Flags
92 | bool saveflag_;
93 | bool printflag_;
94 | bool plotflag_;
95 | //Handles
96 | ros::NodeHandle ros_node_;
97 | std::vector ros_publishers_;
98 | std::vector ros_state_subscribers_;
99 | std::vector ros_desired_state_subscribers_;
100 | std::vector ros_control_subscribers_;
101 | std::vector ros_desired_control_subscribers_;
102 | std::vector ros_parameter_subscribers_;
103 | //Vector of constraints
104 |
105 | std::vector constraint_;
106 | std::vector coupling_;
107 | std::vector controller_;
108 |
109 |
110 | inline void setVector(int dim, double* vector,double* out,const char* error){
111 | for(int i=0;i SET TO 0
114 | if(std::isnan(vector[i])){
115 | out[i]=0;
116 | std::cout<<"!!! In "< vector,double* out,const char* error){
126 | for(int i=0;i SET TO 0
129 | if(std::isnan(vector[i])){
130 | out[i]=0;
131 | std::cout<<"!!! In "< init_x_,
148 | // std::vector init_xdes_,
149 | // std::vector init_u_,
150 | // std::vector init_udes_,
151 | // std::vector init_y_,
152 | // std::vector init_ydes_,
153 | // std::vector init_p_);
154 |
155 |
156 | ~Agent();
157 |
158 | //Getter Methods
159 | int getId() {return this->id_;};
160 | std::string getName() {return this->name_;};
161 | inline void getState(double* _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;i vector) {setVector(dim_u_,vector,u_,"Control");};
215 | inline void setState(std::vector vector) {setVector(dim_x_,vector,x_,"State");};
216 | inline void setOutput(std::vector vector) {setVector(dim_y_,vector,y_,"Output");};
217 | inline void setDisturbance(std::vector vector) {setVector(dim_d_,vector,d_,"Disturbance");};
218 | inline void setDesiredControl(std::vector vector) {setVector(dim_udes_,vector,udes_,"Desired Control");};
219 | inline void setDesiredState(std::vector vector) {setVector(dim_xdes_,vector,xdes_,"Desired State");};
220 | inline void setDesiredOutput(std::vector vector) {setVector(dim_ydes_,vector,ydes_,"Desired Output");};
221 | inline void setParameter(std::vector vector) {setVector(dim_p_,vector,p_,"Parameter");};
222 |
223 | inline void setInitialControl(double* vector) {setVector(dim_u_,vector,u_init_,"Initial Control");};
224 | inline void setInitialState(double* vector) {setVector(dim_x_,vector,x_init_,"Initial State");};
225 | inline void setInitialOutput(double* vector) {setVector(dim_y_,vector,y_init_,"Initial Output");};
226 | inline void setInitialDisturbance(double* vector) {setVector(dim_d_,vector,d_init_,"Initial Disturbance");};
227 | inline void setInitialDesiredControl(double* vector){setVector(dim_udes_,vector,udes_init_,"Initial Desired Control");};
228 | inline void setInitialDesiredState(double* vector) {setVector(dim_xdes_,vector,xdes_init_,"Initial Desired State");};
229 | inline void setInitialDesiredOutput(double* vector) {setVector(dim_ydes_,vector,ydes_init_,"Initial Desired Output");};
230 | inline void setInitialParameter(double* vector) {setVector(dim_p_,vector,p_init_,"Initial Parameter");};
231 |
232 | inline void setInitialControl(std::vector vector) {setVector(dim_u_,vector,u_init_,"Initial Control");};
233 | inline void setInitialState(std::vector vector) {setVector(dim_x_,vector,x_init_,"Initial State");};
234 | inline void setInitialOutput(std::vector vector) {setVector(dim_y_,vector,y_init_,"Initial Output");};
235 | inline void setInitialDisturbance(std::vector vector) {setVector(dim_d_,vector,d_init_,"Initial Disturbance");};
236 | inline void setInitialDesiredControl(std::vector vector){setVector(dim_udes_,vector,udes_init_,"Initial Desired Control");};
237 | inline void setInitialDesiredState(std::vector vector) {setVector(dim_xdes_,vector,xdes_init_,"Initial Desired State");};
238 | inline void setInitialDesiredOutput(std::vector vector) {setVector(dim_ydes_,vector,ydes_init_,"Initial Desired Output");};
239 | inline void setInitialParameter(std::vector vector) {setVector(dim_p_,vector,p_init_,"Initial Parameter");};
240 |
241 | //Methods
242 | /*
243 | virtual void record(){};
244 | virtual void print(){};
245 | virtual void start(){printf("Agent%i start: not defined",this->id);};
246 | virtual void pause(){printf("Agent%i pause: not defined",this->id);};
247 | virtual void stop() {printf("Agent%i stop: not defined",this->id);};
248 | // calculate plant actuation from plant output with controller -> Call Controller
249 | virtual void calculateActuation(double time){std::cout<<"calculateActuation"< Call Sensor/Observer
251 | virtual void calculateState(){std::cout<<"calculateState"<id_);};
258 | virtual void pause(){printf("Agent%i pause: not defined",this->id_);};
259 | virtual void stop() {printf("Agent%i stop: not defined",this->id_);};
260 | // calculate plant actuation from plant output with controller -> Call Controller
261 | void calculateActuation(double time){std::cout<<"Agent::calculateActuation"< Call Sensor/Observer
263 | void calculateState(){std::cout<<"Agent::calculateState"< Call Sensor/Observer
265 | // void calculateDisturbance(double t){
266 | // double tmp_d[dim_d_];
267 | // //Estimate disturbance
268 | // d(tmp_d,t,x_,u_,d_,p_,xdes_,udes_);
269 | // //set result of d as new disturbance
270 | // setVector(dim_d_,tmp_d,d_,"calculateDisturbance():d");
271 | // };
272 | // // simulate state
273 | // void simulateState(double time,double updateintervall){std::cout<<"Agent::simulateState"<id_=id;
30 | this->name_="general Constraint";
31 | }
32 | Constraint::Constraint(int id,Agent* _agent){
33 | this->id_=id;
34 | this->name_="general Constraint";
35 | agent_=_agent;
36 | agent_->addConstraint(this);
37 |
38 | }
39 |
--------------------------------------------------------------------------------
/src/Constraint.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Constraint.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Constraint Container
9 | *
10 | * This can be used to implement constraints on system states,
11 | * controls, etc. it covers stage costs, final costs, inequality constraints, equality constraints
12 | * E.g. Orientation Constraints
13 | */
14 |
15 | #ifndef _CONSTRAINT_H
16 | #define _CONSTRAINT_H
17 |
18 | #include "Agent.h"
19 | #include "Controller.h"
20 | #include
21 | #include
22 | #include //For printf
23 | #include
24 | #include "Indexing.h"
25 |
26 | using namespace MathLib;
27 |
28 | //Forward Declaration
29 | class Agent;
30 |
31 | class Constraint:public ConstraintIndex{
32 | friend class Controller;
33 | protected:
34 | double* pc_; //Parameters
35 | double* pc_init_; //Parameters
36 | int dim_x_;
37 | int dim_u_;
38 | int dim_y_;
39 | int dim_xdes_;
40 | int dim_udes_;
41 | int dim_ydes_;
42 | int dim_p_;
43 | int dim_pc_;
44 | int dim_ineq_;
45 | int dim_eq_;
46 | int dim_l_;
47 | int dim_v_;
48 |
49 | std::string name_;
50 | int id_;
51 |
52 | Agent* agent_;
53 |
54 | public:
55 |
56 | Agent* getAgent();
57 | int getId(){return this->id_;};
58 | std::string getName(){return this->name_;};
59 | Constraint(int id, Agent* agent1);
60 | Constraint(int id);
61 | Constraint();
62 |
63 | inline void getParameter(double* _vector) {for(int i=0;i& _vector) {for(int i=0;i&getParameter()const {std::vector _vector (p,p+sizeof(p)/sizeof(p[0]));return _vector;};
66 | inline int getState_Dim() {return dim_x_;};
67 | inline int getControl_Dim() {return dim_u_;};
68 | inline int getOutput_Dim() {return dim_y_;};
69 | inline int getDesiredState_Dim() {return dim_xdes_;};
70 | inline int getDesiredControl_Dim() {return dim_udes_;};
71 | inline int getDesiredOutput_Dim() {return dim_ydes_;};
72 | inline int getParameter_Dim() {return dim_pc_;};
73 | // inline int getConstraintParameter_Dim() {return dim_pc;};
74 | inline int getInequalityConstraint_Dim(){return dim_ineq_;};
75 | inline int getEqualityConstraint_Dim() {return dim_eq_;};
76 | inline int getStageCost_Dim() {return dim_l_;};
77 | inline int getFinalCost_Dim() {return dim_v_;};
78 |
79 | //Setter Methods
80 | int setId(int _id) {id_=_id;};
81 | std::string setName(std::string _name) {name_=_name;};
82 | inline void setParameter(double* _vector) {for(int i=0;i _vector) {for(int i=0;i _vector){for(int i=0;iid_=id;
23 | this->name_="general Coupling";
24 | agent1_=_agent1;
25 | agent2_=_agent2;
26 | // agent1->addCoupling1(this);
27 | // agent2->addCoupling2(this);
28 | }
29 |
--------------------------------------------------------------------------------
/src/Coupling.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Coupling.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Coupling Container
9 | *
10 | * This can be used to implement couplings between agents
11 | * It covers stage costs, final costs, inequality constraints, equality constraints
12 | * E.g. Collision Avoidance constraints
13 | */
14 |
15 | #ifndef _COUPLING_H
16 | #define _COUPLING_H
17 |
18 | #include "Indexing.h"
19 | #include "Agent.h"
20 | #include "Constraint.h"
21 | #include
22 | #include
23 | #include //For printf
24 | #include
25 |
26 | using namespace MathLib;
27 |
28 | //Forward Declaration
29 | class Agent;
30 | class Constraint;
31 |
32 | class Coupling : public CouplingIndex{
33 | friend class Controller;
34 | protected:
35 | int id_;
36 | std::string name_;
37 | double* pc_; //Parameters
38 | double* pc_init_; //Parameters
39 | //State Dimensions
40 | int dim_x1_;
41 | int dim_u1_;
42 | int dim_y1_;
43 | int dim_xdes1_;
44 | int dim_udes1_;
45 | int dim_ydes1_;
46 | int dim_p1_;
47 | int dim_x2_;
48 | int dim_u2_;
49 | int dim_y2_;
50 | int dim_xdes2_;
51 | int dim_udes2_;
52 | int dim_ydes2_;
53 | int dim_p2_;
54 | int dim_pc_;
55 |
56 | int dim_p_;
57 | int dim_ineq_;
58 | int dim_eq_;
59 | int dim_l_;
60 | int dim_v_;
61 | Agent* agent1_;
62 | Agent* agent2_;
63 |
64 | public:
65 | inline void setInitialParameter(double* _vector){for(int i=0;i& _vector) {for(int i=0;i& _vector) {for(int i=0;iid_;};
86 | std::string getName(){return this->name_;};
87 | Coupling(){};
88 | Coupling(int _id){id_=_id;};
89 | Coupling(int id_, Agent* agent1, Agent* agent2);
90 | //Stage costs
91 | virtual void l(double *out, double t, double *x1,double *x2, double *u1,double *u2, double *p1,double *p2, double *pc, double *xdes1,double *xdes2, double *udes1,double *udes2) {std::cout<<"Coupling "<::RosEvent_addAgent(Controller* controller, Agent* agent, double time):RosEvent(time){
26 | // agent_ =agent;
27 | // controller_=controller;
28 | // }
29 | // void RosEvent_addAgent::eventAction(){
30 | // std::cout<<"RosEvent_addAgent::eventAction() "<addAgent(agent_);
32 | // };
33 | //
34 | // RosEvent_removeAgent::RosEvent_removeAgent(Controller* controller, Agent* agent, double time):RosEvent(time){
35 | // agent_ =agent;
36 | // controller_=controller;
37 | // }
38 | // void RosEvent_removeAgent::eventAction(){
39 | // std::cout<<"RosEvent_removeAgent::eventAction() "<removeAgent(agent_);
41 | // };
42 | //
43 |
44 |
45 |
46 |
47 |
48 |
49 |
50 |
51 |
52 |
53 |
54 | /****************************************************
55 | * Time EVENT
56 | ****************************************************/
57 |
58 | /********************
59 | * Parameter Events
60 | */
61 | TimeEvent_changeParameters::TimeEvent_changeParameters(Agent* agent, double* p,double time):TimeEvent(time){
62 | agent_ =agent;
63 | constraint_=NULL;
64 | coupling_ =NULL;
65 | parameters_=p;
66 | }
67 | TimeEvent_changeParameters::TimeEvent_changeParameters(Constraint* constraint, double* pc,double time):TimeEvent(time){
68 | agent_ =NULL;
69 | constraint_ =constraint;
70 | coupling_ =NULL;
71 | parameters_ =pc;
72 | }
73 | TimeEvent_changeParameters::TimeEvent_changeParameters(Coupling* coupling, double* pc,double time):TimeEvent(time){
74 | agent_ =NULL;
75 | constraint_=NULL;
76 | coupling_ =coupling;
77 | parameters_=pc;
78 | }
79 | void TimeEvent_changeParameters::eventAction(){
80 | if(agent_!=NULL){
81 | std::cout<<"Event::TimeEvent_addConstraint "<setParameter(parameters_);
83 | agent_->setInitialParameter(parameters_);
84 | }else if(constraint_!=NULL){
85 | std::cout<<"Event::TimeEvent_addConstraint "<setParameter(parameters_);
87 | constraint_->setInitialParameter(parameters_);
88 | }if(coupling_!=NULL){
89 | std::cout<<"Event::TimeEvent_addConstraint "<setParameter(parameters_);
91 | coupling_->setInitialParameter(parameters_);
92 | }
93 | };
94 |
95 | /********************
96 | * Agent Events
97 | */
98 | TimeEvent_addAgent::TimeEvent_addAgent(Controller* controller, Agent* agent, double time):TimeEvent(time){
99 | agent_ =agent;
100 | controller_=controller;
101 | }
102 | void TimeEvent_addAgent::eventAction(){
103 | std::cout<<"Event::TimeEvent_addConstraint "<addAgent(agent_);
105 | };
106 |
107 | TimeEvent_removeAgent::TimeEvent_removeAgent(Controller* controller, Agent* agent, double time):TimeEvent(time){
108 | agent_ =agent;
109 | controller_=controller;
110 | }
111 | void TimeEvent_removeAgent::eventAction(){
112 | std::cout<<"Event::TimeEvent_removeConstraint "<removeAgent(agent_);
114 | };
115 |
116 |
117 | /********************
118 | * Coupling Event
119 | */
120 | TimeEvent_addCoupling::TimeEvent_addCoupling(Agent* agent1,Agent* agent2,Coupling* coupling,double time):TimeEvent(time){
121 | agent1_ =agent1;
122 | agent2_ =agent2;
123 | coupling_=coupling;
124 |
125 | }
126 | void TimeEvent_addCoupling::eventAction(){
127 | std::cout<<"Event::TimeEvent_addCoupling "<addCouplingAsAgent1(coupling_);
129 | agent2_->addCouplingAsAgent2(coupling_);
130 | };
131 |
132 | TimeEvent_removeCoupling::TimeEvent_removeCoupling(Agent* agent1,Agent* agent2,Coupling* coupling,double time):TimeEvent(time){
133 | agent1_ =agent1;
134 | agent2_ =agent2;
135 | coupling_=coupling;
136 | }
137 | void TimeEvent_removeCoupling::eventAction(){
138 | std::cout<<"Event::TimeEvent_removeCoupling "<removeCoupling(coupling_);
140 | agent2_->removeCoupling(coupling_);
141 | };
142 |
143 | /********************
144 | * Constraint Events
145 | */
146 |
147 | TimeEvent_addConstraint::TimeEvent_addConstraint(Agent* agent, Constraint* constraint,double time):TimeEvent(time){
148 | agent_ =agent;
149 | constraint_=constraint;
150 | }
151 | void TimeEvent_addConstraint::eventAction(){
152 | std::cout<<"Event::TimeEvent_addConstraint "<addConstraint(constraint_);
154 | };
155 |
156 | TimeEvent_removeConstraint::TimeEvent_removeConstraint(Agent* agent, Constraint* constraint,double time):TimeEvent(time){
157 | agent_ =agent;
158 | constraint_=constraint;
159 | }
160 | void TimeEvent_removeConstraint::eventAction(){
161 | std::cout<<"Event::TimeEvent_removeConstraint "<removeConstraint(constraint_);
163 | };
164 |
--------------------------------------------------------------------------------
/src/Event.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Event.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Event Container
9 | *
10 | * This can be used to implement events which handle the on-line
11 | * modularization of the control scenario, triggered by messages.
12 | * It covers adding and removing agents, constraints and couplings
13 | * from the scenario
14 | */
15 |
16 | #ifndef _EVENT_H
17 | #define _EVENT_H
18 |
19 | #include //For ros::
20 | #include "std_msgs/Bool.h"
21 |
22 | #include "denmpc/VrepInfo.h"
23 | #include
24 | #include
25 | //#include "YamlInterface.h"
26 | #include "Agent.h"
27 | #include "Constraint.h"
28 | #include "Controller.h"
29 | #include //For vector
30 | #include //For printf
31 |
32 | class Event{
33 | protected:
34 | bool flag_event_;
35 | public:
36 | Event(){
37 | flag_event_=false;
38 | }
39 | virtual bool check(double time) {std::cout<<"Implement Event::check()"<data;
66 | }
67 | RosEvent(std::string rostopicname,bool flag_event=false){
68 | bool flag_event_=flag_event;
69 | rostopicname_=rostopicname;
70 | msg_content_=false;
71 | sub_=node_.subscribe(rostopicname_.c_str(),1,&RosEvent::subCallback,this);
72 | }
73 |
74 | virtual void eventAction(){std::cout<<"Implement RosEvent::eventAction()"<";
92 | if(msg_content_){
93 | std::cout<<"add agent;"<addAgent(agent_);
95 | }
96 | else{
97 | std::cout<<"remove agent;"<removeAgent(agent_);
99 | }
100 | };
101 | };
102 |
103 | class RosEvent_addCoupling:public RosEvent{
104 | protected:
105 | Agent* agent1_;
106 | Agent* agent2_;
107 | Coupling* coupling_;
108 | public:
109 | RosEvent_addCoupling(Agent* agent1,Agent* agent2,Coupling* coupling, std::string rostopicname):RosEvent(rostopicname){
110 | agent1_ =agent1;
111 | agent2_ =agent2;
112 | coupling_ =coupling;
113 | }
114 | virtual void eventAction(){
115 | std::cout<<"RosEvent_addCoupling::eventAction() ->";
116 | if(msg_content_){
117 | std::cout<<"add Coupling;"<addCouplingAsAgent1(coupling_);
119 | agent2_->addCouplingAsAgent2(coupling_);
120 | }
121 | else{
122 | std::cout<<"remove Coupling;"<removeCoupling(coupling_);
124 | agent2_->removeCoupling(coupling_);
125 | }
126 | }
127 | };
128 |
129 | class RosEvent_addConstraint:public RosEvent{
130 | protected:
131 | Agent* agent_;
132 | Constraint* constraint_;
133 | public:
134 | RosEvent_addConstraint(Agent* agent, Constraint* constraint, std::string rostopicname):RosEvent(rostopicname){
135 | agent_=agent;
136 | constraint_=constraint;
137 | }
138 | virtual void eventAction(){
139 | std::cout<<"RosEvent_addConstraint::eventAction() ->";
140 | if(msg_content_){
141 | std::cout<<"add Constraint;"<addConstraint(constraint_);
143 | }
144 | else{
145 | std::cout<<"remove Constraint;"<removeConstraint(constraint_);
147 | }
148 | }
149 | };
150 |
151 |
152 |
153 |
154 |
155 | /****************************************************
156 | * Time EVENT
157 | ****************************************************/
158 |
159 | class TimeEvent:public Event{
160 | protected:
161 | double time_;
162 | public:
163 | TimeEvent(double time=0,bool flag_event=false){
164 | time_=time;
165 | flag_event_=flag_event;
166 | }
167 | bool check(double time){
168 | if((flag_event_==false)&&(time>time_)){
169 | flag_event_=true;
170 | eventAction();
171 | return true;
172 | }
173 | else{
174 | return false;
175 | }
176 | };
177 | virtual void eventAction(){std::cout<<"Implement TimeEvent::eventAction()"<
18 |
19 | /*---- print array ----*/
20 | void MathLib::mprint(std::ostream& ss, unsigned nstate, double *a, const char* name)
21 | {
22 | ss<a, const char* name)
75 | {
76 | printf("%s[%lux1]=\n",name,a.size());
77 | unsigned i, j, tmp ;
78 | printf("\t ");
79 | for(j = 0; j < a.size(); j++) {
80 | printf("%9u ",j);
81 | }
82 | printf("\n");
83 | printf("\t[0,:]: ");
84 | for(j = 0; j < a.size(); j++) {
85 | printf("%+8.7f,",a[j]);
86 | }
87 | printf("\n");
88 | }
89 |
90 |
91 | /*---- print matrix ----*/
92 | void MathLib::mprint(unsigned nhor, unsigned nstate, double **a, const char* name)
93 | {
94 | printf("%s[%ux%u]=\n",name,nhor,nstate);
95 | unsigned i, j, tmp ;
96 | printf("\t ");
97 | for(j = 0; j < nstate; j++) {
98 | printf("%9u ",j);
99 | }
100 | printf("\n");
101 | for(i = 0; i < nhor; i++){
102 | printf("\t[%2u,:]: ",i);
103 | for(j = 0; j < nstate; j++) {
104 | printf("%+8.7f,",a[i][j]);
105 | }
106 | printf("\n");
107 | }
108 | }
109 |
110 | void MathLib::PoseStamped2State(std::vector& state, const geometry_msgs::PoseStamped msgPose, double laststateupdatetimestamp, double& newstatetimestamp){
111 | tf::Quaternion qpresent;
112 | //Initialize temporary variables
113 | double yaw,pitch,roll,dtinput;
114 | dtinput=msgPose.header.stamp.toSec()-laststateupdatetimestamp;
115 |
116 | #ifdef DEBUG
117 | printf("PoseStamped2Vector... start \n");
118 | printf("msg timestamp: %f, ",presentpose.header.stamp.toSec());
119 | printf("previous timestamp: %f, ",lastpose.header.stamp.toSec());
120 | printf("time difference: %f\n",dtinput);
121 | #endif
122 |
123 | if(dtinput>0){
124 | //Approximate UAV Lateral Velocities
125 | state[3]=(msgPose.pose.position.x-state[0])/dtinput;//dx
126 | state[4]=(msgPose.pose.position.y-state[1])/dtinput;//dy
127 | state[5]=(msgPose.pose.position.z-state[2])/dtinput;//dz
128 | //Set UAV Position x
129 | state[0]=msgPose.pose.position.x;//x
130 | state[1]=msgPose.pose.position.y;//y
131 | state[2]=msgPose.pose.position.z;//z
132 |
133 | //Set Euler Angles to phi
134 | tf::quaternionMsgToTF(msgPose.pose.orientation, qpresent);
135 | //Consider Yaw Pitch Roll sequence within the statevector
136 | tf::Matrix3x3(qpresent).getEulerYPR(yaw, pitch, roll);//yaw,pitch,roll
137 |
138 | //Approximate UAV Lateral Velocities
139 | state[9] =atan2(sin(roll -state[6]),cos(roll -state[6]))/dtinput; //droll
140 | state[10]=atan2(sin(pitch-state[7]),cos(pitch-state[7]))/dtinput; //dpitch
141 | state[11]=atan2(sin(yaw -state[8]),cos(yaw -state[8]))/dtinput; //dyaw
142 | //Approximate UAV Lateral Velocities
143 | state[6]=roll; //roll
144 | state[7]=pitch; //pitch
145 | state[8]=yaw; //yaw
146 |
147 | #ifdef DEBUG
148 | //Print position and lateral velocity
149 | printf("Vector \n");;
150 | printf("x: %+2.5f,",state[0]);
151 | printf("y: %+2.5f,",state[1]);
152 | printf("z: %+2.5f,",state[2]);
153 | printf("dx: %+2.5f,",state[3]);
154 | printf("dy: %+2.5f,",state[4]);
155 | printf("dz: %+2.5f\n",state[5]);
156 | printf("roll: %+2.5f,",state[0]);
157 | printf("pitch: %+2.5f,",state[0]);
158 | printf("yaw: %+2.5f,",state[0]);
159 | printf("droll: %+2.5f,",state[0]);
160 | printf("dpitch:%+2.5f,",state[0]);
161 | printf("dyaw: %+2.5f\n",state[0]);
162 | #endif
163 |
164 | }
165 | newstatetimestamp=msgPose.header.stamp.toSec();
166 | }
167 |
--------------------------------------------------------------------------------
/src/MathLib.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file MathLib.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief General Purpose Library, contains
9 | * -----------------------------------------------------------------------
10 | * Some Fundamental Functions for RHC
11 | * T. Ohtsuka '97/10/30~'97/10/31 (rhfunc.c)
12 | * '00/01/27 (rhfuncu.c)
13 | * -----------------------------------------------------------------------
14 | * MathLib contains elemental vector operations
15 | */
16 |
17 | #ifndef MATHLIB_H_
18 | #define MATHLIB_H_
19 |
20 | #include
21 | #include
22 | #include
23 | #include
24 | #include
25 | #include
26 | #include
27 |
28 | namespace MathLib{
29 |
30 | /*---- print array ----*/
31 | void mprint(unsigned nstate, double *a, const char* name);
32 |
33 | void mprint(std::vectora, const char* name);
34 |
35 | /*---- print matrix ----*/
36 | void mprint(unsigned nhor, unsigned nstate, double **a, const char* name);
37 |
38 | /*---- print array ----*/
39 | void mprint(std::ostream& ss, unsigned nstate, double *a, const char* name);
40 | /*---- print matrix ----*/
41 | void mprint(std::ostream& ss, unsigned nhor, unsigned nstate, double **a, const char* name);
42 |
43 |
44 |
45 | /*---- a[n] -> b[n] ----*/
46 | inline void minit(double *b, unsigned n){
47 | for(int j = 0; j < n; j++){
48 | b[j] =0;
49 | }
50 | }
51 |
52 |
53 | /*---- a[n] -> b[n] ----*/
54 | inline void mmov(double *b, unsigned n, std::vector a)
55 | { unsigned j;
56 | for(j = 0; j < n; j++){
57 | b[j] = a[j] ;
58 | }
59 | }
60 |
61 | /*---- a[n] -> b[n] ----*/
62 | inline void mmov(double *b, unsigned n, double *a )
63 | { unsigned j;
64 | for(j = 0; j < n; j++){
65 | b[j] = a[j] ;
66 | }
67 | }
68 |
69 | /*---- a[m][n] -> b[m][n] ----*/
70 | inline void mmov( double **b, unsigned m, unsigned n, double **a )
71 | {
72 | unsigned i, j, tmp ;
73 | for(i = 0; i < m; i++)
74 | for(j = 0; j < n; j++){
75 | b[i][j] = a[i][j];
76 | }
77 | }
78 |
79 | /*---- a[m][n] -> b[m][n] ----*/
80 | inline void mmov( double *b, unsigned m, unsigned n, double *a )
81 | {
82 | unsigned i, j, tmp ;
83 | for(i = 0; i < m; i++)
84 | for(j = 0; j < n; j++){
85 | tmp = i*n + j ;
86 | b[tmp] = a[tmp] ;
87 | }
88 | }
89 | /*---- a[m][n] + b[m][n] -> c[m][n] ----*/
90 | inline void madd(double *c, unsigned m, unsigned n, double *a, double *b )
91 | {
92 | unsigned i, j, tmp ;
93 | for(i = 0; i < m; i++)
94 | for(j = 0; j < n; j++) {
95 | tmp = i*n + j ;
96 | c[tmp] = a[tmp] + b[tmp] ;
97 | }
98 | }
99 | /*---- a[m][n] + b[m][n] -> c[m][n] ----*/
100 | inline void madd(unsigned n, double *a, double *b,double *c)
101 | {
102 | unsigned j;
103 | for(j = 0; j < n; j++) {
104 | c[j] = a[j] + b[j] ;
105 | }
106 | }
107 | /*---- a[m] - b[m] -> c[m] ----*/
108 | inline void msub( double *c, unsigned m,double *a, double *b )
109 | {
110 | unsigned i, j;
111 | for(i = 0; i < m; i++){
112 | c[i] = a[i] - b[i] ;
113 | }
114 | }
115 | /*---- a[m][n] - b[m][n] -> c[m][n] ----*/
116 | inline void msub( double *c, unsigned m, unsigned n, double *a, double *b )
117 | {
118 | unsigned i, j, tmp ;
119 | for(i = 0; i < m; i++)
120 | for(j = 0; j < n; j++) {
121 | tmp = i*n + j ;
122 | c[tmp] = a[tmp] - b[tmp] ;
123 | }
124 | }
125 | /*---- k * a[m][n] -> b[m][n] ----*/
126 | inline void mmulsc( double *b, unsigned m, unsigned n, double *a, double k )
127 | {
128 | unsigned i, j, tmp;
129 | for(i = 0; i < m; i++)
130 | for(j = 0; j < n; j++){
131 | tmp = i*n + j ;
132 | b[tmp] = k * a[tmp];
133 | }
134 | }
135 | /*---- a[m][n] / k -> b[m][n] ----*/
136 | inline void mdivsc( double *b, unsigned m, unsigned n, double *a, double k )
137 | {
138 | unsigned i, j, tmp;
139 | for(i = 0; i < m; i++)
140 | for(j = 0; j < n; j++){
141 | tmp = i*n + j ;
142 | b[tmp] = a[tmp] / k;
143 | }
144 | }
145 |
146 |
147 |
148 |
149 |
150 |
151 |
152 |
153 | /*---- a[n] -> b[n] ----*/
154 | inline void mmov(unsigned n, std::vector a,double *b)
155 | { unsigned j;
156 | for(j = 0; j < n; j++){
157 | b[j] = a[j] ;
158 | }
159 | }
160 |
161 | /*---- a[n] -> b[n] ----*/
162 | inline void mmov(unsigned n, double *a,double *b )
163 | { unsigned j;
164 | for(j = 0; j < n; j++){
165 | b[j] = a[j] ;
166 | }
167 | }
168 |
169 | /*---- a[m][n] -> b[m][n] ----*/
170 | inline void mmov( unsigned m, unsigned n, double **a, double **b )
171 | {
172 | unsigned i, j, tmp ;
173 | for(i = 0; i < m; i++)
174 | for(j = 0; j < n; j++){
175 | b[i][j] = a[i][j];
176 | }
177 | }
178 |
179 | /*---- a[m][n] -> b[m][n] ----*/
180 | inline void mmov( unsigned m, unsigned n, double *a, double *b )
181 | {
182 | unsigned i, j, tmp ;
183 | for(i = 0; i < m; i++)
184 | for(j = 0; j < n; j++){
185 | tmp = i*n + j ;
186 | b[tmp] = a[tmp] ;
187 | }
188 | }
189 | /*---- a[m][n] + b[m][n] -> c[m][n] ----*/
190 | inline void madd(unsigned m, unsigned n, double *a, double *b,double *c )
191 | {
192 | unsigned i, j, tmp ;
193 | for(i = 0; i < m; i++)
194 | for(j = 0; j < n; j++) {
195 | tmp = i*n + j ;
196 | c[tmp] = a[tmp] + b[tmp] ;
197 | }
198 | }
199 | /*---- a[m] - b[m] -> c[m] ----*/
200 | inline void msub( unsigned m,double *a, double *b,double *c )
201 | {
202 | unsigned i, j;
203 | for(i = 0; i < m; i++){
204 | c[i] = a[i] - b[i] ;
205 | }
206 | }
207 | /*---- a[m][n] - b[m][n] -> c[m][n] ----*/
208 | inline void msub( unsigned m, unsigned n, double *a, double *b,double *c )
209 | {
210 | unsigned i, j, tmp ;
211 | for(i = 0; i < m; i++)
212 | for(j = 0; j < n; j++) {
213 | tmp = i*n + j ;
214 | c[tmp] = a[tmp] - b[tmp] ;
215 | }
216 | }
217 | /*---- k * a[m][n] -> b[m][n] ----*/
218 | inline void mmulsc( unsigned m, unsigned n, double *a, double k,double *b )
219 | {
220 | unsigned i, j, tmp;
221 | for(i = 0; i < m; i++){
222 | for(j = 0; j < n; j++){
223 | tmp = i*n + j ;
224 | b[tmp] = k * a[tmp];
225 | }
226 | }
227 | }
228 | /*---- k * a[m][n] -> b[m][n] ----*/
229 | inline void mmulsc( unsigned n, double *a, double k,double *b )
230 | {
231 | unsigned j;
232 | for(j = 0; j < n; j++){
233 | b[j] = k * a[j];
234 | }
235 | }
236 | /*---- a[m][n] / k -> b[m][n] ----*/
237 | inline void mdivsc( unsigned m, unsigned n, double *a, double k,double *b )
238 | {
239 | unsigned i, j, tmp;
240 | for(i = 0; i < m; i++){
241 | for(j = 0; j < n; j++){
242 | tmp = i*n + j ;
243 | b[tmp] = a[tmp] / k;
244 | }
245 | }
246 | }
247 |
248 |
249 |
250 |
251 |
252 |
253 |
254 | /*---- Inner Product of a[m] and b[m] ----*/
255 | inline double mvinner( unsigned m, double *a, double *b)
256 | {
257 | unsigned i;
258 | double tmp;
259 | tmp =0.;
260 | for(i=0; i initvector)
311 | {
312 | double *v;
313 | v = (double *)malloc( ( n * sizeof(double) ) );
314 | //v = new double [n];
315 | if(!v) {
316 | printf("defvector(): allocation failure. \n");
317 | exit(1);
318 | }
319 | if(n==(initvector.size())){
320 | for(unsigned i=0;i& array, geometry_msgs::PoseStamped msgPose,double& rostimestamp){
529 | rostimestamp=msgPose.header.stamp.toSec();
530 | array[0]=msgPose.pose.position.x;
531 | array[1]=msgPose.pose.position.y;
532 | array[2]=msgPose.pose.position.z;
533 | array[3]=0;
534 | array[4]=0;
535 | array[5]=0;
536 | array[6]=0;
537 | }
538 |
539 | inline void PoseStamped2Array(std::vector& array, geometry_msgs::PoseStamped msgPose,double& rostimestamp){
540 | rostimestamp=msgPose.header.stamp.toSec();
541 | array[0]=msgPose.pose.position.x;
542 | array[1]=msgPose.pose.position.y;
543 | array[2]=msgPose.pose.position.z;
544 | array[3]=msgPose.pose.orientation.x;
545 | array[4]=msgPose.pose.orientation.y;
546 | array[5]=msgPose.pose.orientation.z;
547 | array[6]=msgPose.pose.orientation.w;
548 | }
549 |
550 | void PoseStamped2State(std::vector& state, const geometry_msgs::PoseStamped msgPose, double laststateupdatetimestamp, double& newstatetimestamp);
551 |
552 |
553 |
554 | }
555 |
556 |
557 |
558 |
559 |
560 | #endif
561 |
562 |
--------------------------------------------------------------------------------
/src/Scheduler.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Scheduler.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Scheduler Container
9 | *
10 | * Scheduler is managing the communication and control update intervalls.
11 | * It also allows switching between control, simulation and V-Rep mode.
12 | */
13 |
14 | #include "Scheduler.h"
15 |
16 | /**
17 | * Scheduler implementation
18 | */
19 | //Scheduler::Scheduler(int argc, char **argv, vector _agentlist, vector constraintlist){
20 | Scheduler::Scheduler(
21 | int argc,
22 | char **argv,
23 | std::vector controllerlist,
24 | std::vector eventlist){
25 | //Store agentlist_
26 | controllerlist_=controllerlist;
27 | eventlist_ =eventlist;
28 |
29 | //Initialize ROS
30 | ros::init(argc, argv, "fccf");
31 | //Printing starting screen
32 | printf("//*********************************************\n");
33 | printf("// Starting Fast Cooperative Control Framework \n");
34 | printf("//*********************************************\n");
35 |
36 | //Initialization
37 | //Printing initialization screen
38 | printf("// Initialization \n");
39 | printf("//---------------------------------------------\n");
40 |
41 | }
42 |
43 | void Scheduler::controlloop(){
44 | //Get systemtime of framework start [s]
45 | ros_time_calibration_=ros::Time::now().toSec();
46 | //Set the update loop intervall [1/s]
47 | ros::Rate loop_frequency(1/time_intervall_);
48 | //Index
49 | unsigned i;
50 | //Printing loop start screen
51 | printf("//---------------------------------------------\n");
52 | printf("// Start ROS Control Loop \n");
53 |
54 |
55 | //Starting Agents
56 | for(int it_controller=0;it_controllerstartAgents();
59 | }
60 |
61 |
62 | //Starting fccf loop
63 | //--------------------------------------------------------------
64 | for(counter_fccfloop_=0;ros::ok();counter_fccfloop_++){
65 | //Calculate system time in framework
66 | time_framework_=ros::Time::now().toSec()-ros_time_calibration_;
67 | //Print time in framework
68 | std::cout<<"/*************************************************************************\\"<check(time_framework_)){
79 | flag_event_happened=true;
80 | }
81 | }
82 | //Reinit if event happened
83 | if(flag_event_happened){
84 | flag_event_happened=false;
85 | for(int it_controller=0;it_controllerinit();
87 | }
88 | }
89 | //Control loop
90 | for(int it_controller=0;it_controllergetMeasurements();
93 | ptr_controller->computeAction(time_framework_);
94 | ptr_controller->applyAction();
95 | }
96 |
97 | //Sleep until endtime of loop intervall
98 | loop_frequency.sleep();
99 | //Run ROS Processes
100 | ros::spinOnce();
101 | }
102 | printf("//**********************************************\n");
103 | printf("// Finishing Fast Cooperative Control Framework \n");
104 | printf("//**********************************************\n");
105 |
106 | }
107 | void Scheduler::vrepsimrunning(const denmpc::VrepInfo& msgInfo){
108 | // printf("\n%u\n",msgInfo.simulatorState.data);
109 | int info=msgInfo.simulatorState.data;
110 | bool old_vrep_flag_stop =vrep_flag_stop_;
111 | bool old_vrep_flag_pause=vrep_flag_pause_;
112 | //Bit 0 True for simulation not stopped
113 | //Bit 1 True for simulation paused
114 | vrep_flag_stop_=!(info&0b001);
115 | vrep_flag_pause_=(info&0b010);
116 | vrep_flag_realtime_=(info&0b100);
117 | vrep_time_=msgInfo.simulationTime.data;
118 | //printf("Vrep Flag: Stop:%d Pause:%d\n",vrep_flag_stop_,vrep_flag_pause_);
119 | if((vrep_flag_stop_)&&(!old_vrep_flag_stop)){
120 | printf( "!!!!!!!!!!!!!!!!!!\n");
121 | printf( "!! vrep stopped !!\n");
122 | printf( "!!!!!!!!!!!!!!!!!!\n");
123 | }
124 | if((vrep_flag_pause_)&&(!old_vrep_flag_pause)){
125 | printf( "!!!!!!!!!!!!!!!!!\n");
126 | printf( "!! vrep paused !!\n");
127 | printf( "!!!!!!!!!!!!!!!!!\n");
128 | }
129 | }
130 | void Scheduler::vrepcontrolloop(){
131 | //Get Vrep simulation
132 | ros::Subscriber rosSubscriberVrepInfo;
133 | ros::NodeHandle rosNodeVrepInfo;
134 | //Get info about Vrep Simulation
135 | rosSubscriberVrepInfo = rosNodeVrepInfo.subscribe(rosNodeVrepInfo.resolveName("vrep/info"),1,&Scheduler::vrepsimrunning,this);
136 | //Printing loop start screen
137 | printf("//---------------------------------------------\n");
138 | printf("// Start VREP Control Loop \n");
139 | ros::Duration(0.2).sleep(); //Wait for subscriber
140 | ros::spinOnce(); //Check Vrep state
141 | //Set the update loop intervall [1/s]
142 | ros::Rate loop_frequency(1/time_intervall_);
143 | //Index
144 | unsigned i;
145 |
146 |
147 |
148 | //Starting Agents
149 | for(int it_controller=0;it_controllerstartAgents();
152 | }
153 |
154 |
155 |
156 | //Starting fccf loop
157 | //--------------------------------------------------------------
158 | for(counter_fccfloop_=0;ros::ok();counter_fccfloop_++){
159 | //Wait if simulation is stopped
160 | while(ros::ok()&&((vrep_flag_stop_==true)||(vrep_flag_pause_==true))){
161 | ros::spinOnce();
162 | };
163 | printf("//---------------------------------------------\n");
164 | //Calculate system time in framework
165 | time_framework_=vrep_time_;
166 | //Print time in framework
167 | printf("loop: %7i ",counter_fccfloop_);
168 | printf("time: %7.4f\n",time_framework_);
169 | double tmp_start=ros::Time::now().toSec();
170 |
171 | //Check if event happend
172 | bool flag_event_happened=false;
173 | for(int it_event=0;it_eventcheck(time_framework_)){
175 | flag_event_happened=true;
176 | }
177 | }
178 | //Reinit if event happened
179 | if(flag_event_happened){
180 | flag_event_happened=false;
181 | for(int it_controller=0;it_controllerinit();
183 | }
184 | }
185 |
186 | //Control loop
187 | for(int it_controller=0;it_controllergetMeasurements();
190 | ptr_controller->computeAction(time_framework_);
191 | ptr_controller->applyAction();
192 | }
193 |
194 | //Sleep until endtime of loop intervall
195 | loop_frequency.sleep();
196 | //Run ROS Processes
197 | ros::spinOnce();
198 | }
199 | printf("//**********************************************\n");
200 | printf("// Finishing Fast Cooperative Control Framework \n");
201 | printf("//**********************************************\n");
202 | }
203 |
204 | void Scheduler::simulationloop(double simtime){
205 | //Index
206 | unsigned i;
207 | //Printing loop start screen
208 | printf("//---------------------------------------------\n");
209 | printf("// Start SIMULATION Control Loop \n");
210 |
211 | //Starting Agents
212 | for(int it_controller=0;it_controllerstartAgents();
215 | }
216 |
217 | time_framework_=0;
218 | //Starting fccf loop
219 | //--------------------------------------------------------------
220 | for(counter_fccfloop_=0;time_framework_check(time_framework_)){
230 | flag_event_happened=true;
231 | }
232 | }
233 | //Reinit if event happened
234 | if(flag_event_happened){
235 | flag_event_happened=false;
236 | for(int it_controller=0;it_controllerinit();
238 | }
239 | }
240 | //Control loop
241 | for(int it_controller=0;it_controllercomputeAction(time_framework_);
244 | ptr_controller->applyAction();
245 | ptr_controller->integrateStateExplicitEuler(time_framework_,time_intervall_);
246 | }
247 |
248 | time_framework_+=time_intervall_;
249 | }
250 | printf("//***********************************************\n");
251 | printf("// Finishing Fast Cooperative Control Simulation \n");
252 | printf("//***********************************************\n");
253 | }
254 |
255 |
--------------------------------------------------------------------------------
/src/Scheduler.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Scheduler.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Scheduler Container
9 | *
10 | * Scheduler is managing the communication and control update intervalls.
11 | * It also allows switching between control, simulation and V-Rep mode.
12 | */
13 |
14 | #ifndef _SCHEDULER_H
15 | #define _SCHEDULER_H
16 |
17 | #include //For ros::
18 |
19 | #include "denmpc/VrepInfo.h"
20 | #include
21 | #include "Event.h"
22 | #include "Controller.h"
23 | #include "Agent.h"
24 | #include "Constraint.h"
25 | #include "Controller.h"
26 | #include //For vector
27 | #include //For printf
28 |
29 |
30 |
31 | class Scheduler {
32 | protected:
33 | /*Control update intervall*/
34 | double time_intervall_;
35 | /*Global time in framework*/
36 | double time_framework_;
37 | /*Framework loop counter*/
38 | int counter_fccfloop_;
39 | /*std::vector of all agents*/
40 | std::vector agentlist_;
41 | std::vector eventlist_;
42 | std::vector controllerlist_;
43 | /*Framework calibration time*/
44 | double ros_time_calibration_;
45 |
46 | Controller* ptr_controller;
47 |
48 | /*ROS control loop scheduler*/
49 | void controlloop();
50 |
51 | /*Simulation control loop scheduler*/
52 | void simulationloop(double simtime);
53 |
54 | /*VREP control loop scheduler (via ros)*/
55 | void vrepcontrolloop();
56 | void vrepsimrunning(const denmpc::VrepInfo& msgInfo);
57 | float vrep_time_;
58 | bool vrep_flag_stop_;
59 | bool vrep_flag_pause_;
60 | bool vrep_flag_realtime_;
61 |
62 | public:
63 | // Scheduler(int argc, char **argv, std::vector _agentlist, std::vector constraintlist);
64 | Scheduler(
65 | int argc,
66 | char **argv,
67 | std::vector controllerlist,
68 | std::vector eventlist
69 | );
70 | void run_control(double dt){
71 | time_intervall_=dt;
72 | controlloop();
73 | }
74 | void run_vrep(double dt){
75 | time_intervall_=dt;
76 | vrepcontrolloop();
77 | }
78 | void run_simulation(double dt, double simtime){
79 | time_intervall_=dt;
80 | simulationloop(simtime);
81 | }
82 |
83 |
84 | };
85 |
86 | #endif //_Scheduler_H
87 |
--------------------------------------------------------------------------------
/src/controller/Cmscgmres.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Cmscgmres.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Condensed Multiple Shooting Generalized Minimal Residuum Method (CMSCGMRES)
9 | *
10 | * This is a modification of the CMSCGMRES developed by
11 | * Ohtsuka, T., “A Continuation/GMRES Method for Fast Computation of Nonlinear Receding Horizon Control,” Automatica, Vol. 40, No. 4, Apr. 2004, pp. 563-574.
12 | * Seguchi, H., and Ohtsuka, T., “Nonlinear Receding Horizon Control of an Underactuated Hovercraft,” International Journal of Robust and Nonlinear Control, Vol. 13, Nos. 3-4, Mar.-Apr. 2003, pp. 381-398.
13 | *
14 | * The CMSCGMRES has been extended by an orthogonalization within the GMRES Solver which allows
15 | * suboptimal solutions of infeasible problems
16 | * and a security measures for infeasible cases.
17 | */
18 |
19 | #ifndef CMSCGMRES_H_
20 | #define CMSCGMRES_H_
21 |
22 | #include "Controller.h"
23 |
24 | //=================================================//
25 | // Preprocessor Commands to turn of / on Debugging //
26 | //=================================================//
27 |
28 | //#define TRACE_ON
29 | //#define RESET_DU
30 | //#define RESET_DX
31 | //#define DEBUG_CGMRES
32 | //#define CONTROLLER_INFO
33 | //#define CGMRES_INIT_INFO
34 | //#define STATE_INFO
35 | //#define DEBUG_CGMRES
36 |
37 | class Cmscgmres : public Controller{
38 | protected:
39 | /*-------------- Global Variables Defined by User -------------- */
40 | std::string type_;
41 |
42 | //========================================//
43 | // Controller Parameters //
44 | //========================================//
45 | double alpha_;
46 | double zeta_;
47 | double rtol_;
48 | double hdir_;
49 | double htau_;
50 | int kmax_;
51 | int nhor_;
52 |
53 | //===============================================//
54 | // Controller Global Variables / Pointers / Flags//
55 | //===============================================//
56 | double tsim0_;
57 | int controlcounter_;
58 | int dim_f_, dim_g_, dim_x_,dim_p_;
59 | double onemhdir_, hdirbht_, htbhdir_, onemzetahdir_, onemzetahdir2_;
60 | double sum_;
61 | double t_, ts_;
62 | double tauf_;
63 | double *tau_;
64 | double **U_, **X_, **Uk_, **Fk_, **Hk_, **U1_;
65 | double **F_, **G_, **F1s_, **G1s_, **H1s_, **F1_, **F2_, **G1_, **H1_;
66 | double /* *x,*/ *x1s_, *bvec_, *duvec_, *dxvec_, *dutmp_, *errvec_;
67 | double *lmd0_, *hu0_;
68 | double thor_;
69 | double ht_;
70 | double* x0_;
71 | double* u0_;
72 | double* udes_;
73 | double* p_;
74 | double* x1_;
75 | double** f3_;
76 | //Function Pointers
77 | typedef void (Cmscgmres::*funcptr_cgmresfunc)(unsigned, double *, double *);
78 | typedef void (Cmscgmres::*funcPtr_func) (double,double*,double*,double*);
79 |
80 | //========================================//
81 | // CMSCGMRES SYSTEM FUNCTIONS INTERFACE //
82 | //========================================//
83 |
84 | /**-------------- hufunc --------------
85 | * contains dHd[u,a_slack]
86 | *------------------------------------
87 | * @param time
88 | * @param states
89 | * @param adjoint states
90 | * @param inputs
91 | * @param dHdu \huhu
92 | * @return void
93 | */
94 | inline void hufunc(double t, double* x, double* lambda, double* optvar, double* dHduout){
95 | // double* u=optvar;
96 | // double* mu=optvar+u_conc_dim;
97 | dHdu(dHduout,t,x,optvar,d_conc_,p_conc_,xdes_conc_,udes_conc_,lambda);
98 | };
99 |
100 | /**---- lpfunc ----
101 | * contains -dHdx with linp=[x,optvar]
102 | *------------------------------------
103 | * @param time
104 | * @param adjoint states
105 | * @param [states,inputs,desiredstates]
106 | * @param derivate of adjointstates
107 | * @return void
108 | */
109 | inline void lpfunc(double t, double* lambda, double* linp, double* lambdaprime){
110 | // //linp=[x,optvar] //Primal Barrier => linp=[x,optvar]
111 | // double* x=linp;
112 | // double* optvar=linp +dim_x_conc_;
113 | minusdHdx(lambdaprime,t,linp,linp+dim_x_conc_,d_conc_,p_conc_,xdes_conc_,udes_conc_,lambda);
114 | };
115 |
116 | /**---- dxpfunc= System ODE ----
117 | * @param time
118 | * @param states
119 | * @param inputs
120 | * @param derivate of states
121 | * @return void
122 | */
123 | inline void xpfunc(double t, double* x, double* u, double* dfdx){
124 | //this->function_fsys_ohtsuka(dfdx,t,x,u,timevarpar);
125 | f(dfdx,t,x,u,d_conc_,p_conc_);
126 | };
127 |
128 | /**---- Phix = Final costs function ----
129 | * @param time
130 | * @param states
131 | * @param endcosts
132 | * @return void
133 | */
134 | inline void phix(double t, double* x, double* out){
135 | //this->function_dvdx(phx1,x,xdes);
136 | //v(out,t,x,p,xdes);
137 | dvdx(out,t,x,p_conc_,xdes_conc_);
138 | };
139 |
140 |
141 | //========================================//
142 | // MAIN CMSCGMRES ROUTINES //
143 | //========================================//
144 |
145 | /*---- k Givens Roations used in GMRES ----
146 | * rotate with c, s so that [[s,c];[-s c]]*[a;b]=[r;0]
147 | * @param k : number of rows/columns
148 | * @param c : Spur Elements
149 | * @param s : Digonal Elements
150 | * @param v : [r,0];
151 | * @return void
152 | */
153 | inline void givrot(unsigned k, double *c, double *s, double *v);
154 |
155 | /**--------------------------------------------------------
156 | * Linear Equation Subroutine
157 | * GMRES (Generalized Minimum Residual)
158 | * Cf. C.T.Kelley: Iterative Methods for Linear and Nonlinear Equations
159 | * T.Ohtsuka '00/01/24
160 | * @param axfunc(unsigned n, double *x, double *ax): a function that gives A*x
161 | * @param n: dim x
162 | * @param b: the right-hand-side of the equation, A*x = b
163 | * @param x: initial guess, to be replaced with the solution
164 | * @param kmax: number of iteration
165 | * @param err: residual norms, |b - A*x_i| (i=1,...,n+1)
166 | * @return void
167 | */
168 | inline void nfgmres(funcptr_cgmresfunc axfunc, unsigned n, double *b, double *x, unsigned kmax, double *err);
169 |
170 | /**--------------------------------------------------------
171 | * Simultaneous Ordinaly Diferential Equation Subroutine
172 | * Runge-Kutta_Gill Method (Original by T.Murayama)
173 | *
174 | * ---- Variation of rkg() : dx/dt is also returned. ----
175 | * ---- (for Start of the Adams method) ----
176 | * ----------------------------------------------------------
177 | * @param void (*func)()
178 | * @param time
179 | * @param states
180 | * @param inputs
181 | * @param stepsize
182 | * @param state dimension
183 | * @param ans
184 | * @param fxy
185 | * @return void
186 | */
187 | inline void nfrkginpex(funcPtr_func func,double x,double* y,double* u,double h,unsigned dim,double* ans,double* fxy);
188 |
189 | /**--------------------------------------------------------
190 | * Simultaneous Ordinaly Diferential Equation Subroutine
191 | * Adams Method (Predictor-Corrector Method)
192 | * Predictor: Adams-Bashforth
193 | * Corrector: Adams-Moulton
194 | * T.Ohtsuka '92/10/24
195 | */
196 | inline void nfadamsinp(funcPtr_func func,double x,double* y,double* u,double* f1,double* f2,double* f3,double h,unsigned dim,double* ans);
197 |
198 | /**--------------------------------------------------------
199 | * Simultaneous Ordinary Differential Equation Subroutine Euler Method (Forward Difference)
200 | * ans[i] = y[i] + h * func(x,y,u,fval);
201 | * ----------------------------------------------------------
202 | * @param evaluation function
203 | * @param time[k]
204 | * @param states[k]
205 | * @param inputs[k]
206 | * @param stepsize
207 | * @param state dimension
208 | * @param states[k+1]
209 | * @return void
210 | */
211 | inline void nfeulerinp(funcPtr_func func, double x, double* y, double* u, double h, unsigned dim, double* ans);
212 |
213 | /**--------Initial conditions for dHdudt ---------
214 | * @param time
215 | * @param dudt0
216 | * @param dHdudt
217 | * @return void
218 | */
219 | inline void dhu0func(unsigned dimu, double *du0, double *dhu);
220 |
221 | /**--------Compute u, that fulfills optimality at initial time tau0=t ---------
222 | * @param states
223 | * @param inputs
224 | * @return void
225 | */
226 | inline void getOptimalInitialValues(double* x, double* u);
227 |
228 | /**-------------- dHdu of present horizon ---------
229 | * @param time
230 | * @param states
231 | * @param inputs
232 | * @param dHdu(t,x,u)
233 | * @return void
234 | */
235 | inline void errfunc(double t, double *x, double **u, double **hu);
236 |
237 | /**-------------- unew --------------
238 | * calculate lefthand side:
239 | * DhF(U; x; t : dU; dx; 1)
240 | *----------------------------------
241 | * @param dimension of u
242 | * @param derivate of inputs
243 | * @param lefthand side
244 | * @return void
245 | */
246 | inline void adufunc(unsigned n, double *du, double *adu);
247 |
248 | /**-------------- unew --------------
249 | * calculate and save new controller actuation in u
250 | *----------------------------------
251 | * @param time
252 | * @param states
253 | * @param file for saving error
254 | * @param file for saving costs
255 | * @return void
256 | */
257 | inline void unew(double t, double* x, double* x1, double* u);
258 |
259 | /**-------------- Ffunc --------------
260 | * calculate F(U,X,x,t): optimization variable derivative
261 | *----------------------------------
262 | * @param **Uk
263 | * @param **Xk
264 | * @param *xk
265 | * @param tk
266 | * @param **Fk
267 | * @return void
268 | */
269 | inline void Ffunc(double **Uk, double **Xk, double *xk, double tk, double **Fk);
270 |
271 | /**-------------- Gfunc --------------
272 | * calculate G(U,X,x,t): state derivative
273 | *----------------------------------
274 | * @param **Uk
275 | * @param **Xk
276 | * @param *xk
277 | * @param tk
278 | * @param **Fk
279 | * @return void
280 | */
281 | inline void Gfunc(double **Uk, double **Xk, double *xk, double tk, double **Gk);
282 |
283 | /**-------------- Hfunc --------------
284 | * calculate H(U,X,x,t)
285 | *----------------------------------
286 | * @param **Uk
287 | * @param **Zk
288 | * @param *xk
289 | * @param tk
290 | * @param **Hk
291 | * @return void
292 | */
293 | inline void Hfunc(double **Uk, double **Zk, double *xk, double tk, double **Hk);
294 |
295 |
296 | //========================================//
297 | // Internal initialization routines //
298 | //========================================//
299 |
300 | /**-------------- initVectors --------------
301 | * sets arrays with current values
302 | *------------------------------------
303 | * @return void
304 | */
305 | inline void initVectors();
306 |
307 | /**-------------- allocateMemory --------------
308 | * allocates Memory for the controller
309 | *------------------------------------
310 | * @return void
311 | */
312 | void allocateMemory();
313 |
314 | /**-------------- freeMemory --------------
315 | * frees Memory for the controller
316 | *------------------------------------
317 | * @return void
318 | */
319 | void freeMemory();
320 |
321 |
322 | public:
323 | //========================================//
324 | // MAIN ACCES METHODS //
325 | //========================================//
326 |
327 | /**-------------- Constructor --------------
328 | * @param _listofagents
329 | * @param _kmax
330 | * @param _thor
331 | * @param _ht
332 | * @param _nhor
333 | * @param _rtol
334 | * @param _zeta
335 | * @param _alpha
336 | * @param _hdir
337 | * @return void
338 | */
339 | Cmscgmres(std::vector _listofagents,int _id,
340 | int _kmax=10,double _thor=1, double _ht=0.1,
341 | int _nhor=10,double _rtol= 1e-8,double _zeta=10,
342 | double _alpha=2,double _hdir=0.001);
343 |
344 | /**Destructor */
345 | ~Cmscgmres();
346 |
347 | /**-------------- Compute controller action --------------
348 | * @param _time
349 | * @return void
350 | */
351 | void computeAction(double _time);
352 |
353 | /**-------------- Init controller --------------
354 | * allocate memory and initialize controls
355 | * according to the optimality conditions
356 | *----------------------------------
357 | * @param _time
358 | * @return void
359 | */
360 | void init();
361 |
362 |
363 | //========================================//
364 | // SETTER METHODS FOR SOLVER PARAMETERS //
365 | //========================================//
366 |
367 | /**-------------- set Solver Tolerance --------------
368 | * @param _tol
369 | * @return void
370 | */
371 | void setTolerance(double _tol){
372 | rtol_=_tol;
373 | init();
374 | }
375 |
376 | /**-------------- set Horizon Expansion Factor --------------
377 | * @param _alpha
378 | * @return void
379 | */
380 | void setHorizonExpansionFactor(double _alpha){
381 | alpha_=_alpha;
382 | init();
383 | }
384 |
385 | /**-------------- set Maximum number of CGMRES iterations --------------
386 | * @param _kmax
387 | * @return void
388 | */
389 | void setMaximumNumberofIterations(double _kmax){
390 | kmax_=_kmax;
391 | init();
392 | }
393 |
394 | /**-------------- set Prediction Horizon length --------------
395 | * @param _thor
396 | * @return void
397 | */
398 | void setHorizonLength(double _thor){
399 | thor_=_thor;
400 | init();
401 | }
402 |
403 | /**-------------- set Number of Diskretisation points of the horizon --------------
404 | * @param _nhor
405 | * @return void
406 | */
407 | void setHorizonDiskretization(double _nhor){
408 | nhor_=_nhor;
409 | init();
410 | }
411 |
412 | /**-------------- set the discretisation step of the hessian approximation --------------
413 | * @param _hdir
414 | * @return void
415 | */
416 | void setForwardDifferenceStep(double _hdir){
417 | hdir_=_hdir;
418 | onemhdir_ = 1 - hdir_ / ht_;
419 | hdirbht_ = hdir_ / ht_;
420 | htbhdir_ = ht_ / hdir_;
421 | onemzetahdir_= 1 - zeta_*hdir_;
422 | init();
423 | }
424 |
425 | /**-------------- set the update interval, with which the CMSCGMRES is calculating --------------
426 | * @param _updateintervall
427 | * @return void
428 | */
429 | void setUpdateIntervall(double _updateintervall){
430 | ht_=_updateintervall;
431 | onemhdir_ = 1 - hdir_ / ht_;
432 | hdirbht_ = hdir_ / ht_;
433 | htbhdir_ = ht_ / hdir_;
434 | onemzetahdir_= 1 - zeta_*hdir_;
435 | init();
436 | }
437 |
438 | /**-------------- set the desired solver convergence factor of the dynamics --------------
439 | * @param _zeta
440 | * @return void
441 | */
442 | void setContiunationConvergenceFactor(double _zeta){
443 | zeta_=_zeta;
444 | onemzetahdir_= 1 - zeta_*hdir_;
445 | init();
446 | }
447 |
448 |
449 | //=======================//
450 | // VECTOR MANIPULATION //
451 | //=======================//
452 | /**---- a[m][n] -> b[m][n] ----*/
453 | void mmov( int n, double *a, double *b );
454 | /**---- a[m][n] -> b[m][n] ----*/
455 | void mmov( int m, int n, double *a, double *b );
456 | /**---- a[m][n] + b[m][n] -> c[m][n] ----*/
457 | void madd( int m, int n, double *a, double *b, double *c );
458 | /**---- a[m][n] - b[m][n] -> c[m][n] ----*/
459 | void msub( int m, int n, double *a, double *b, double *c );
460 | /**---- k * a[m][n] -> b[m][n] ----*/
461 | void mmulsc( int m, int n, double *a, double k, double *b );
462 | /**---- a[m][n] -> -a[m][n] ----*/
463 | void mminus( int m, int n, double *a, double *b );
464 | /**---- a[m][n] / k -> b[m][n] ----*/
465 | void mdivsc( int m, int n, double *a, double k, double *b );
466 | /**---- Inner Product of a[m] and b[m] ----*/
467 | double mvinner( int m, double *a, double *b );
468 | /**---- Define an n-Dimensional Vector ----*/
469 | double *defvector(int n);
470 | /**---- UnDefine an n-Dimensional Vector ----*/
471 | void freevector(double *v);
472 | /**---- Define an n by m Matrix ----*/
473 | double **defmatrix(int n, int m);
474 | /**---- Undefine an n by m Matrix ----*/
475 | void freematrix(double **a);
476 |
477 | };
478 |
479 |
480 |
481 |
482 |
483 | #endif
484 |
--------------------------------------------------------------------------------
/src/controller/Controller.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file Controller.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Controller Container
9 | *
10 | * Controller represents a Container Class for MPC-Controllers.
11 | * It generates the functions required for optimization, such as
12 | * the Hamiltonian and its derivatives.
13 | * The generation is based on a concatenation of all controlled agents, constraints and couplings.
14 | * The concatenation is in respect to the chosen constraint handling:
15 | * 1. Auxiliary Variable Method as implemented by Ohtsuka
16 | * where the inequality is transformed to an equality constraint with the help of slack variables
17 | * Ohtsuka, T., “A Continuation/GMRES Method for Fast Computation of Nonlinear Receding Horizon Control,” Automatica, Vol. 40, No. 4, Apr. 2004, pp. 563-574.
18 | * 2. Simple Primal Barrier Method
19 | * where the inequality is transformed into stage cost with the help of a logarithmic barrier function.
20 | */
21 |
22 | #ifndef _CONTROLLER_H
23 | #define _CONTROLLER_H
24 |
25 | #include "Agent.h"
26 | #include "Constraint.h"
27 | #include "Coupling.h"
28 | #include
29 | #include
30 | #include
31 | #include "std_msgs/Float32.h"
32 |
33 | using namespace MathLib;
34 |
35 | //Forward Declarations
36 | class Agent;
37 | class Constraint;
38 | class Coupling;
39 |
40 | /* ===============================================
41 | * Controller is concatenating Hamiltonian
42 | * according to coptimization variables
43 | * optvar=[ agent1_u, ..., agentN_u, \
44 | * agent1_lambda, ..., agentN_lambda,\
45 | * agent1c1_lambda, ..., agent1cM_lambda,
46 | * ...,
47 | * agentNc1_lambda, ..., agentNcM_lambda,
48 | * constraint1_lambda, ..., constraintN_lambda,
49 | *
50 | *
51 | * ]
52 | * x=[agent1_x,agent2_x,...,agentN_x]...
53 | * =============================================== */
54 |
55 | //Defining numbers for Constraint handling methods
56 | #define METHOD_PRIMALBARRIER 0
57 | #define METHOD_AUXILIARYVARIABLE 1
58 | #define METHOD_ACTIVESET 2
59 | #define METHOD_EXTERIORPENALTY 3
60 |
61 | class Controller{
62 | friend class Cmscgmres;
63 | protected:
64 | int id_;
65 |
66 | //Constraint handling factors
67 | double primalbarrierfactor_;
68 | double exteriorpenaltyfactor_;
69 | double lambda_init_;
70 | double mu_init_;
71 | double mui_init_;
72 | double slack_init_;
73 | double slack_penalty_init_;
74 |
75 | //Lists and Pointers
76 | std::vector agentlist_;
77 | Agent* tmp_agent_ptr_;
78 | Constraint* tmp_constraint_ptr_;
79 | Coupling* tmp_coupling_ptr_;
80 |
81 | //Dimensions
82 | int dim_optvar_conc_; //Concatenation of optimization variables
83 | int dim_x_conc_; //Concatenation of states
84 | int dim_u_conc_; //Concatenation of controls
85 | int dim_xdes_conc_; //Concatenation of desired states
86 | int dim_udes_conc_; //Concatenation of desired controls
87 | int dim_d_conc_; //Concatenation of disturbance
88 | int dim_p_conc_; //Concatenation of parameters
89 | int dim_lambda_conc_; //Concatenation of state lagrange mutlipliers
90 | int dim_eqcon_conc_; //Concatenation of equality lagrange multipliers
91 | int dim_ineqcon_conc_; //Concatenation of inequality lagrange multipliers
92 |
93 | //Arrays
94 | double* optvar_conc_; //Concatenation of optimization variables
95 | double* x_conc_;
96 | double* u_conc_;
97 | double* d_conc_;
98 | double* p_conc_;
99 | double* udes_conc_;
100 | double* xdes_conc_;
101 | double* lambda_conc_;
102 | double* mu_conc_;
103 | double* mui_conc_;
104 | double* slack_conc_;
105 | double* slack_penalty_conc_;
106 |
107 | int constrainthandlingmethod_;
108 | bool* workingset_;
109 |
110 | //Flags
111 | bool flag_memoryalreadyallocated_; //True if memory is allocated
112 | bool flag_show_controllertrace_; //Show interior vector and matrix values of controller
113 | bool flag_show_controllerinfo_; //Show controller info
114 | bool flag_show_controllerstates_; //Show controller states
115 | bool flag_save_controllerlog_; //Save controllers
116 |
117 | //Log
118 | std::string log_filename_;
119 | std::stringstream log_stringstream_;
120 | std::ofstream log_file_;
121 | ros::NodeHandle node_;
122 | ros::Publisher comp_time_publisher_;
123 |
124 | public:
125 |
126 | void startAgents();
127 |
128 | void setInitialEqualityConstraintLagrangeMultiplier (double _mu_init) {mu_init_=_mu_init;}
129 | void setInitialInequalityConstraintLagrangeMultiplier(double _mui_init) {mui_init_=_mui_init;}
130 | void setInitialAuxiliarySlackVariables (double _slack_init) {slack_init_=_slack_init;}
131 |
132 | void activateInfo_ControllerTrace(){flag_show_controllertrace_=true;}
133 | void activateInfo_Controller(){flag_show_controllerinfo_=true;}
134 | void activateInfo_ControllerStates(){flag_show_controllerstates_=true;}
135 | void startLogging2File(){flag_save_controllerlog_=true;}
136 | void deactivateInfo_ControllerTrace(){flag_show_controllertrace_=false;}
137 | void deactivateInfo_Controller(){flag_show_controllerinfo_=false;}
138 | void deactivateInfo_ControllerStates(){flag_show_controllerstates_=false;}
139 | void stopLogging2File(){flag_save_controllerlog_=false;}
140 |
141 | int getXdesConc_Dim(){return dim_xdes_conc_;}
142 | void setXdesConc(double* vector){
143 | MathLib::mmov(xdes_conc_,dim_xdes_conc_,vector);
144 | }
145 | void getXdesConc(double* vector){
146 | MathLib::mmov(vector,dim_xdes_conc_,xdes_conc_);
147 | }
148 | int getXConc_Dim(){return dim_x_conc_;}
149 | void setXConc(double* vector){
150 | MathLib::mmov(x_conc_,dim_x_conc_,vector);
151 | }
152 | void getXConc(double* vector){
153 | MathLib::mmov(vector,dim_x_conc_,x_conc_);
154 | }
155 |
156 |
157 | //Constructor
158 | Controller(std::vector _listofagents, int _id);
159 | //Destructor
160 | ~Controller();
161 | //Initialization: Concatenation of the Vectors
162 | void initConcatenation();
163 | // //get initial values and set them to the arrays
164 | // void initArrayValues();
165 | //Get Measurments from Agents
166 | void getMeasurements();
167 | //Compute action
168 | virtual void computeAction(double _time){std::cout<<"void Controller::computeAction()"< Call Estimator/Observer
191 | void calculateState();
192 | //Compute active set
193 | void setActiveSetWorkingSet(double* mui);
194 | /*=============================================
195 | * Systemfunction Composition
196 | *============================================= */
197 | // void dHduPrimalBarrier(double *out,double t, double *x, double *u, double *p){
198 | // double* xdes =p +p_conc_dim;
199 | // double* udes =xdes+xdes_conc_dim;
200 | // double* lambda =x +dim_x_conc_;
201 | // double* mu =u +dim_u_conc_;
202 | // double* mui =mu +eqcon_conc_dim_;
203 | // dHdu (out, t,x,u,p,xdes,udes,lambda,mu,mui);
204 | // dcdumu (out+dim_u_conc_, t,x,u,p,xdes,udes,mu);
205 | // dcidumui(out+dim_u_conc_+eqcon_conc_dim_,t,x,u,p,xdes,udes,mui);
206 | //
207 | // }
208 | // void dHdu(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui);
209 | // void dHdx(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui);
210 | /***************************************************************************************
211 | * Hamiltonian=l+lambda*f+mui*ci+mu*c
212 | * =>dHdu=dldu+lambda*dfdu+mui*dcidu+mu*dcdu
213 | * 0=dHdu for optimality
214 | ***************************************************************************************/
215 | inline void dHdu(double *out,double t, double *x, double *optcon, double *d, double *p, double *xdes, double *udes, double* lambda){
216 | switch(constrainthandlingmethod_){
217 | case METHOD_PRIMALBARRIER:{
218 | double* tmp_ptr_u =optcon;
219 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
220 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui]
221 | // double* x=linp;
222 | // double* u=linp +dim_x_conc_;
223 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
224 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
225 | dHduPrimalBarrier(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu);
226 | break;
227 | }
228 | case METHOD_AUXILIARYVARIABLE:{
229 | double* tmp_ptr_u =optcon;
230 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
231 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
232 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
233 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
234 | // double* x=linp;
235 | // double* u=linp +dim_x_conc_;
236 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
237 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
238 | dHduAuxiliaryVariable(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui,tmp_ptr_slack);
239 | break;
240 | }
241 | case METHOD_ACTIVESET:{
242 | double* tmp_ptr_u =optcon;
243 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
244 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
245 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
246 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
247 | // double* x=linp;
248 | // double* u=linp +dim_x_conc_;
249 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
250 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
251 | dHduActiveSet(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui);
252 | break;
253 | }
254 | case METHOD_EXTERIORPENALTY:{
255 | double* tmp_ptr_u =optcon;
256 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
257 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
258 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
259 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
260 | // double* x=linp;
261 | // double* u=linp +dim_x_conc_;
262 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
263 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
264 | dHduExteriorPenalty(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui);
265 | break;
266 | }
267 | }
268 | };
269 |
270 | /***************************************************************************************
271 | * Hamiltonian=l+lambda*f+mui*ci+mu*c
272 | * =>dHdx=dldx+lambda*dfdx+mui*dcidx+mu*dcdx
273 | * dlambda=-dHdx for optimality
274 | ***************************************************************************************/
275 | inline void minusdHdx(double *out,double t, double *x, double *optcon, double *d, double *p, double *xdes, double *udes, double* lambda){
276 |
277 | switch(constrainthandlingmethod_){
278 | case METHOD_PRIMALBARRIER:{
279 | double* tmp_ptr_u =optcon;
280 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
281 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui]
282 | // double* x=linp;
283 | // double* u=linp +dim_x_conc_;
284 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
285 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
286 | minusdHdxPrimalBarrier(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu);
287 | break;
288 | }
289 | case METHOD_AUXILIARYVARIABLE:{
290 | double* tmp_ptr_u =optcon;
291 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
292 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
293 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
294 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
295 | // double* x=linp;
296 | // double* u=linp +dim_x_conc_;
297 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
298 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
299 | minusdHdxAuxiliaryVariable(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui,tmp_ptr_slack);
300 | break;
301 | }
302 | case METHOD_ACTIVESET:{
303 | double* tmp_ptr_u =optcon;
304 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
305 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
306 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
307 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
308 | // double* x=linp;
309 | // double* u=linp +dim_x_conc_;
310 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
311 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
312 | minusdHdxActiveSet(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui);
313 | break;
314 | }
315 | case METHOD_EXTERIORPENALTY:{
316 | double* tmp_ptr_u =optcon;
317 | double* tmp_ptr_mu =tmp_ptr_u +dim_u_conc_;
318 | double* tmp_ptr_mui =tmp_ptr_mu +dim_eqcon_conc_;
319 | double* tmp_ptr_slack=tmp_ptr_mui +dim_ineqcon_conc_;
320 | // //linp=[x,optvar] //Primal Barrier => linp=[x,u,mu,mui,slack]
321 | // double* x=linp;
322 | // double* u=linp +dim_x_conc_;
323 | // double* mu=linp+dim_x_conc_+dim_u_conc_;
324 | // this->minusdHdxPrimalBarrier(lambdaprime,t,x,u,p_conc_,xdes,udes,lambda,mu);
325 | minusdHdxExteriorPenalty(out,t,x,tmp_ptr_u,d,p,xdes,udes,lambda,tmp_ptr_mu,tmp_ptr_mui);
326 | break;
327 | }
328 | }
329 |
330 | };
331 |
332 | /***************************************************************************************
333 | * Hamiltonian=l+lambda*f+mui*ci+mu*c
334 | * =>dHdu=dldu+lambda*dfdu+mui*dcidu+mu*dcdu
335 | * 0=dHdu for optimality
336 | ***************************************************************************************/
337 | inline void dHduPrimalBarrier(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu){
338 | double tmp_u[dim_u_conc_];
339 | memset(tmp_u, 0, dim_u_conc_*sizeof (double));
340 | // for(int it_control=0; it_controldHdu=[dldu+lambda*dfdu+mui*dcidu+mu*dcdu;ci;cia;dciada
395 | * 0=dHdu for optimality
396 | ***************************************************************************************/
397 | inline void dHduAuxiliaryVariable(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui, double* slack){
398 | double tmp_u[dim_u_conc_];
399 | memset(tmp_u, 0, dim_u_conc_*sizeof (double));
400 | // for(int it_control=0; it_controlminusdHdx=-[dldx+lambda*dfdx+mui*dcidx+mu*dcdx]
433 | ***************************************************************************************/
434 | inline void minusdHdxAuxiliaryVariable(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui, double* slack){
435 | double tmp_x[dim_x_conc_];
436 | memset(tmp_x, 0, dim_x_conc_*sizeof (double));
437 | // for(int it_control=0; it_controldHdu=[dldu+lambda*dfdu+mui*dcidu+mu*dcdu;ci;cia;dciada
460 | * 0=dHdu for optimality
461 | ***************************************************************************************/
462 | inline void dHduActiveSet(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
463 | double tmp_u[dim_u_conc_];
464 | memset(tmp_u, 0, dim_u_conc_*sizeof (double));
465 | // for(int it_control=0; it_controlminusdHdx=-[dldx+lambda*dfdx+mui*dcidx+mu*dcdx]
498 | ***************************************************************************************/
499 | inline void minusdHdxActiveSet(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
500 | double tmp_x[dim_x_conc_];
501 | memset(tmp_x, 0, dim_x_conc_*sizeof (double));
502 | // for(int it_control=0; it_controldHdu=[dldu+lambda*dfdu+mui*dcidu+mu*dcdu;ci;cia;dciada
539 | * 0=dHdu for optimality
540 | ***************************************************************************************/
541 | inline void dHduExteriorPenalty(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
542 | double tmp_u[dim_u_conc_];
543 | memset(tmp_u, 0, dim_u_conc_*sizeof (double));
544 | // for(int it_control=0; it_control0)
566 | tmp_ci[it_constraint]=exteriorpenaltyfactor_;
567 | else
568 | tmp_ci[it_constraint]=0;
569 | }
570 | //Get derivative of inequality constraint with calculated factors
571 | dcidumui (tmp_u,t,x,u,p,xdes,udes,tmp_ci);
572 | for(int it_control=0; it_controlminusdHdx=-[dldx+lambda*dfdx+mui*dcidx+mu*dcdx]
578 | ***************************************************************************************/
579 | inline void minusdHdxExteriorPenalty(double *out,double t, double *x, double *u, double *d, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
580 | double tmp_x[dim_x_conc_];
581 | memset(tmp_x, 0, dim_x_conc_*sizeof (double));
582 | // for(int it_control=0; it_control0){
599 | tmp_ci[it_constraint]=exteriorpenaltyfactor_;
600 | }
601 | else{
602 | tmp_ci[it_constraint]=0;
603 | }
604 | }
605 | //Get derivative of inequality constraint with calculated factors
606 | dcidxmui (tmp_x,t,x,u,p,xdes,udes,tmp_ci);
607 | for(int it_state=0; it_statedHdu=dldu+lambda*dfdu+mui*dcidu+mu*dcdu
616 | // * 0=dHdu for optimality
617 | // ***************************************************************************************/
618 | // void dHdu(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
619 | // double tmp_u[dim_u_conc_];
620 | // //dldu
621 | // dldu (out,t,x,u,p,xdes,udes);
622 | // //dfdu*lambda
623 | // dfdulambda (tmp_u,t,x,u,p,lambda);
624 | // for(int it_control=0; it_controldHdx=dldx+lambda*dfdx+mui*dcidx+mu*dcdx
642 | // * dlambda=-dHdx for optimality
643 | // ***************************************************************************************/
644 | // void dHdx(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes, double* lambda, double* mu, double* mui){
645 | // double tmp_x[dim_x_conc_];
646 | // dldx (out,t,x,u,p,xdes,udes);
647 | // dfdxlambda (tmp_x,t,x,u,p,lambda);
648 | // for(int it_state=0; it_state
12 |
13 | /*******************************************************
14 | * Constructor *
15 | *******************************************************/
16 | Ardrone_20170227::Ardrone_20170227(int id):Agent(id){
17 | //Set initial values
18 | dim_x_=9;
19 | dim_xdes_=9;
20 | dim_u_=4;
21 | dim_udes_=4;
22 | dim_y_=0;
23 | dim_ydes_=0;
24 | dim_p_=19;
25 | dim_d_=0;
26 | dim_l_=1;
27 | dim_v_=1;
28 | dim_eq_=0;
29 | dim_ineq_=4;
30 | //Allocate vectors
31 | x_ =defvector(dim_x_);
32 | xdes_ =defvector(dim_xdes_);
33 | u_ =defvector(dim_u_);
34 | udes_ =defvector(dim_udes_);
35 | y_ =defvector(dim_y_);
36 | ydes_ =defvector(dim_ydes_);
37 | d_ =defvector(dim_d_);
38 | p_ =defvector(dim_p_);
39 | x_init_ =defvector(dim_x_);
40 | xdes_init_ =defvector(dim_xdes_);
41 | u_init_ =defvector(dim_u_);
42 | udes_init_ =defvector(dim_udes_);
43 | y_init_ =defvector(dim_y_);
44 | ydes_init_ =defvector(dim_ydes_);
45 | d_init_ =defvector(dim_d_);
46 | p_init_ =defvector(dim_p_);
47 |
48 |
49 | //Initialize Parameters
50 | /* Agent parameters:
51 | * { lateralA,lateralB,verticalA,verticalB,orientationalA,orientationalB,
52 | * qposlat,qposlat,qposvert,qposorient,qposorient,qvlat,qvlat,qvvert,qvorient,
53 | * ruvlat,ruvlat,ruvver,ruorient
54 | * } dim[19]
55 | */
56 | double ardrone0_init_p[]={
57 | -0.5092,1.458,3,3,1,1.3, //Model Parameters: Ardrone
58 | 1.0,1.0,1.0, 1.0,1.0, 0.0,0.0,0.0,0.0, //State Penalty: Position
59 | 1.0,1.0,1.0,1.0 //Input Penalty
60 | };
61 | double ardrone0_init_x[]= {0,0,0, 1,0, 0.0,0.0,0.0,0.0};
62 | double ardrone0_init_xdes[]={0,0,0, 1,0, 0.0,0.0,0.0,0.0};
63 | this->setInitialState(ardrone0_init_x);
64 | this->setInitialDesiredState(ardrone0_init_xdes);
65 | this->setInitialParameter(ardrone0_init_p);
66 |
67 | //Creating control publisher
68 | ros::Publisher* pub0= new ros::Publisher();
69 | //Starting Advertising
70 | *pub0=ros_node_.advertise("cmd_vel", 1);
71 | //Adding publisher to array
72 | ros_publishers_.push_back(pub0);
73 |
74 | //Creating subscriber
75 | ros::Subscriber* sub0= new ros::Subscriber();
76 | //Initialize subscriber0_old_msg_ attribute
77 | subscriber0_old_msg_.pose.position.x=0;
78 | subscriber0_old_msg_.pose.position.y=0;
79 | subscriber0_old_msg_.pose.position.z=0;
80 | subscriber0_old_msg_.pose.orientation.w=1;
81 | subscriber0_old_msg_.pose.orientation.x=0;
82 | subscriber0_old_msg_.pose.orientation.y=0;
83 | subscriber0_old_msg_.pose.orientation.z=0;
84 | //Starting subscription
85 | *sub0=ros_node_.subscribe("pose", 1, &Ardrone_20170227::subStateCallback,this);
86 | //Adding subscriber to array
87 | ros_state_subscribers_.push_back(sub0);
88 |
89 | //Creating subscriber
90 | ros::Subscriber* sub1= new ros::Subscriber();
91 | //Initialize subscriber0_old_msg_ attribute
92 | subscriber1_old_msg_.pose.position.x=0;
93 | subscriber1_old_msg_.pose.position.y=0;
94 | subscriber1_old_msg_.pose.position.z=0;
95 | subscriber1_old_msg_.pose.orientation.w=1;
96 | subscriber1_old_msg_.pose.orientation.x=0;
97 | subscriber1_old_msg_.pose.orientation.y=0;
98 | subscriber1_old_msg_.pose.orientation.z=0;
99 | //Starting subscription
100 | *sub1=ros_node_.subscribe("desiredpose", 1, &Ardrone_20170227::subDesiredStateCallback,this);
101 | //Adding subscriber to array
102 | ros_desired_state_subscribers_.push_back(sub1);
103 |
104 | }
105 | Ardrone_20170227::Ardrone_20170227(
106 | std::string state_subscriber_topic,
107 | std::string desired_state_subscriber_topic,
108 | std::string control_publish_topic,
109 | double* init_x=NULL,
110 | double* init_xdes=NULL,
111 | double* init_u=NULL,
112 | double* init_udes=NULL,
113 | double* init_p=NULL,
114 | double* init_d=NULL,
115 | int id=0
116 | ):Ardrone_20170227(id){
117 | if(init_x!=NULL){this->setInitialState (init_x);};
118 | if(init_xdes!=NULL){this->setInitialDesiredState (init_xdes);};
119 | if(init_u!=NULL){this->setInitialControl (init_u);};
120 | if(init_udes!=NULL){this->setInitialDesiredControl(init_udes);};
121 | if(init_p!=NULL){this->setInitialParameter (init_p);};
122 | this->reset2initialstate();
123 | this->setStateSubscriberRosTopicName(state_subscriber_topic);
124 | this->setDesiredStateSubscriberRosTopicName(desired_state_subscriber_topic);
125 | this->setPublisherRosTopicName(control_publish_topic);
126 | };
127 |
128 | /********************************************************
129 | * System dynamics *
130 | ********************************************************/
131 | void Ardrone_20170227::f(double *out,double t, double *x, double *u, double *d, double *p){
132 | #ifdef DEBUG_FUNCTION_TRACE
133 | cout<<"exec Ardrone_20170227::f(...)"<
15 |
16 | /*******************************************************
17 | * Agent *
18 | *******************************************************/
19 |
20 | class Ardrone_20170227:public Agent{
21 | geometry_msgs::PoseStamped subscriber0_old_msg_;
22 | geometry_msgs::PoseStamped subscriber1_old_msg_;
23 |
24 | void start(){printf("Agent%i: Ardrone_20170227 started",this->id_);};
25 | void pause(){printf("Agent%i: Ardrone_20170227 paused",this->id_);};
26 | void stop() {printf("Agent%i: Ardrone_20170227 stopped",this->id_);};
27 |
28 | public:
29 | Ardrone_20170227(int id=0);
30 | Ardrone_20170227(
31 | std::string pose,
32 | std::string desiredpose,
33 | std::string cmd_vel,
34 | double* init_x,
35 | double* init_xdes,
36 | double* init_u,
37 | double* init_udes,
38 | double* init_p,
39 | double* init_d,
40 | int id
41 | );
42 | void setStateSubscriberRosTopicName(std::string rostopicname){
43 | ros_state_subscribers_[0]->shutdown();
44 | *ros_state_subscribers_[0]=ros_node_.subscribe(rostopicname, 1,&Ardrone_20170227::subStateCallback,this);
45 | };
46 | void subStateCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
47 | std::vectortmp(dim_x_,0);
48 | double dt=(msg->header.stamp.nsec-subscriber0_old_msg_.header.stamp.nsec)*1.0e-9;
49 | if(dt>0){
50 | tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w);
51 | tf::Quaternion quat_old(subscriber0_old_msg_.pose.orientation.x,subscriber0_old_msg_.pose.orientation.y,subscriber0_old_msg_.pose.orientation.z,subscriber0_old_msg_.pose.orientation.w);
52 | double tmp1,tmp2,yaw,yaw_old;
53 | tf::Matrix3x3(quat).getEulerYPR(yaw,tmp1,tmp2);
54 | tf::Matrix3x3(quat_old).getEulerYPR(yaw_old,tmp1,tmp2);
55 |
56 | double dx=(msg->pose.position.x-subscriber0_old_msg_.pose.position.x)/dt;
57 | double dy=(msg->pose.position.y-subscriber0_old_msg_.pose.position.y)/dt;
58 | double dz=(msg->pose.position.z-subscriber0_old_msg_.pose.position.z)/dt;
59 | double dyaw=(yaw-yaw_old)/dt;
60 |
61 | tmp[0]=msg->pose.position.x;
62 | tmp[1]=msg->pose.position.y;
63 | tmp[2]=msg->pose.position.z;
64 | tmp[3]=cos(yaw);
65 | tmp[4]=sin(yaw);
66 | tmp[5]=dx*tmp[3]+dy*tmp[4];
67 | tmp[6]=-dx*tmp[4]+dy*tmp[3];
68 | tmp[7]=dz;
69 | tmp[8]=dyaw;
70 | this->setState(tmp);
71 | subscriber0_old_msg_=*msg;
72 | }
73 | };
74 | void setDesiredStateSubscriberRosTopicName(std::string rostopicname){
75 | ros_desired_state_subscribers_[0]->shutdown();
76 | *ros_desired_state_subscribers_[0]=ros_node_.subscribe(rostopicname, 1,&Ardrone_20170227::subDesiredStateCallback,this);
77 | };
78 | void subDesiredStateCallback(const geometry_msgs::PoseStamped::ConstPtr& msg){
79 | std::vector tmp(dim_x_,0);
80 | double dt=(msg->header.stamp.nsec-subscriber1_old_msg_.header.stamp.nsec)*1.0e-9;
81 | if(dt>0){
82 | tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w);
83 | tf::Quaternion quat_old(subscriber1_old_msg_.pose.orientation.x,subscriber1_old_msg_.pose.orientation.y,subscriber1_old_msg_.pose.orientation.z,subscriber1_old_msg_.pose.orientation.w);
84 | double tmp1,tmp2,yaw,yaw_old;
85 | tf::Matrix3x3(quat).getEulerYPR(yaw,tmp1,tmp2);
86 | tf::Matrix3x3(quat_old).getEulerYPR(yaw,tmp1,tmp2);
87 |
88 | double dx=(msg->pose.position.x-subscriber1_old_msg_.pose.position.x)/dt;
89 | double dy=(msg->pose.position.y-subscriber1_old_msg_.pose.position.y)/dt;
90 | double dz=(msg->pose.position.z-subscriber1_old_msg_.pose.position.z)/dt;
91 | double dyaw=(yaw-yaw_old)/dt;
92 |
93 | tmp[0] = msg->pose.position.x;
94 | tmp[1] = msg->pose.position.y;
95 | tmp[2] = msg->pose.position.z;
96 | tmp[3]=cos(yaw);
97 | tmp[4]=sin(yaw);
98 | tmp[5]=dx*tmp[3]+dy*tmp[4];
99 | tmp[6]=-dx*tmp[4]+dy*tmp[3];
100 | tmp[7]=dz;
101 | tmp[8]=dyaw;
102 | this->setDesiredState(tmp);
103 | subscriber1_old_msg_=*msg;
104 | }
105 | }
106 |
107 | void setPublisherRosTopicName(std::string rostopicname){
108 | ros_publishers_[0]->shutdown();
109 | *ros_publishers_[0]=ros_node_.advertise(rostopicname, 1);
110 | };
111 | void rosPublishActuation(){
112 | geometry_msgs::Twist msg;
113 | msg.linear.x = u_[0];
114 | msg.linear.y = u_[1];
115 | msg.linear.z = u_[2];
116 | msg.angular.x = 0;
117 | msg.angular.y = 0;
118 | msg.angular.z = u_[3];
119 | ros_publishers_[0]->publish(msg);
120 | }
121 | void f(double *out,double t, double *x, double *u, double *d, double *p);
122 | void dfdxlambda(double *out,double t, double *x, double *u, double *d, double *p, double *lambda);
123 | void dfdulambda(double *out,double t, double *x, double *u, double *d, double *p, double *lambda);
124 | void l(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes);
125 | void dldx(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes);
126 | void dldu(double *out,double t, double *x, double *u, double *p, double *xdes, double *udes);
127 | void v(double *out,double t, double *x, double *p, double *xdes);
128 | void dvdx(double *out,double t, double *x, double *p, double *xdes);
129 | void ci(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes);
130 | void dcidxmui(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *mui);
131 | void dcidumui(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *mui);
132 | void cia(double *out,double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *slack);
133 | void dciadxmui(double *out,double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *mui, double *slack);
134 | void dciadumui(double *out,double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *mu, double *slack);
135 | void dciadamui(double *out,double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes, double *mui, double *slack);
136 | };
137 | #endif
138 |
--------------------------------------------------------------------------------
/src/user_generated/CollisionAvoidanceCoupling_20170227.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file CollisionAvoidanceCoupling_20170227.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Exported with Mathematica Code Generator by Jan Dentler
9 | */
10 |
11 | #include "CollisionAvoidanceCoupling_20170227.h"
12 |
13 | CollisionAvoidanceCoupling_20170227::CollisionAvoidanceCoupling_20170227(int id){
14 | //Set initial values
15 | id_ =id;
16 | dim_x1_ =6;
17 | dim_xdes1_=6;
18 | dim_u1_ =4;
19 | dim_udes1_=4;
20 | dim_y1_ =6;
21 | dim_ydes1_=0;
22 | dim_p1_ =8;
23 | dim_x2_ =6;
24 | dim_xdes2_=6;
25 | dim_u2_ =4;
26 | dim_udes2_=4;
27 | dim_y2_ =6;
28 | dim_ydes2_=0;
29 | dim_p2_ =8;
30 | dim_pc_ =3;
31 | dim_l_ =1;
32 | dim_v_ =0;
33 | dim_eq_ =0;
34 | dim_ineq_ =0;
35 | pc_ =defvector(dim_pc_);
36 |
37 | pc_init_ =defvector(dim_pc_);
38 |
39 | }
40 |
41 | CollisionAvoidanceCoupling_20170227::CollisionAvoidanceCoupling_20170227(double* pc, int id):CollisionAvoidanceCoupling_20170227(id){
42 | this->setParameter(pc);
43 | this->setInitialParameter(pc);
44 | }
45 |
46 | CollisionAvoidanceCoupling_20170227::CollisionAvoidanceCoupling_20170227(Agent* agent1, Agent* agent2, double* pc, int id):CollisionAvoidanceCoupling_20170227(id){
47 | agent1->addCouplingAsAgent1(this);
48 | agent2->addCouplingAsAgent2(this);
49 | this->setParameter(pc);
50 | this->setInitialParameter(pc);
51 | }
52 | void CollisionAvoidanceCoupling_20170227::l(double *out,double t, double *x1, double *x2, double *u1, double *u2, double *p1, double *p2, double *pc, double *xdes1, double *xdes2, double *udes1, double *udes2){
53 | #ifdef DEBUG_FUNCTION_TRACE
54 | cout<<"exec l()"<
12 |
13 | /*******************************************************
14 | * Constructor *
15 | *******************************************************/
16 | OrientationConstraint_20170227::OrientationConstraint_20170227(int id):Constraint(id){
17 | dim_x_=5;
18 | dim_xdes_=5;
19 | dim_u_=4;
20 | dim_udes_=4;
21 | dim_y_=0;
22 | dim_ydes_=0;
23 | dim_p_=0;
24 | dim_pc_=7;
25 | dim_l_=1;
26 | dim_v_=0;
27 | dim_eq_=0;
28 | dim_ineq_=0;
29 | pc_=defvector(dim_pc_);
30 | pc_init_=defvector(dim_pc_);
31 | }
32 | OrientationConstraint_20170227::OrientationConstraint_20170227(Agent* agent,int id):OrientationConstraint_20170227(id){
33 | agent_=agent;
34 | agent_->addConstraint(this);
35 | }
36 |
37 | /********************************************************
38 | * Stage costs *
39 | ********************************************************/
40 | void OrientationConstraint_20170227::l(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes){
41 | double o[12];
42 | o[0] = -x[0];
43 | o[1] = -x[2];
44 | o[2] = xdes[2] + o[1];
45 | o[3] = cos(pc[2]);
46 | o[4] = x[3]*xdes[0];
47 | o[5] = -o[4];
48 | o[6] = x[3]*x[0];
49 | o[7] = x[4]*xdes[1];
50 | o[8] = -o[7];
51 | o[9] = x[4]*x[1];
52 | o[10] = pc[1] + o[5] + o[6] + o[8] + o[9];
53 | o[11] = sin(pc[2]);
54 | out[0] = pc[0]*(xdes[0] + o[0] + pc[5]*pow(x[3]*(-xdes[1] + x[1]) + x[4]*(xdes[0] + o[0]),2.) + pc[4]*pow(pc[3] + o[3]*o[10] + o[2]*o[11],2.) + pc[6]*\
55 | pow(o[2]*o[3] - o[10]*o[11],2.));
56 | }
57 | void OrientationConstraint_20170227::dldx(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes){
58 | double o[28];
59 | o[0] = -x[0];
60 | o[1] = xdes[0] + o[0];
61 | o[2] = x[4]*o[1];
62 | o[3] = -xdes[1];
63 | o[4] = x[1] + o[3];
64 | o[5] = x[3]*o[4];
65 | o[6] = o[2] + o[5];
66 | o[7] = sin(pc[2]);
67 | o[8] = -x[2];
68 | o[9] = xdes[2] + o[8];
69 | o[10] = cos(pc[2]);
70 | o[11] = o[9]*o[10];
71 | o[12] = x[3]*xdes[0];
72 | o[13] = -o[12];
73 | o[14] = x[3]*x[0];
74 | o[15] = x[4]*xdes[1];
75 | o[16] = -o[15];
76 | o[17] = x[4]*x[1];
77 | o[18] = pc[1] + o[13] + o[14] + o[16] + o[17];
78 | o[19] = o[7]*o[18];
79 | o[20] = -o[19];
80 | o[21] = o[11] + o[20];
81 | o[22] = o[10]*o[18];
82 | o[23] = o[7]*o[9];
83 | o[25] = pc[3] + o[22] + o[23];
84 | o[26] = -xdes[0];
85 | o[27] = x[0] + o[26];
86 | out[0] = pc[0]*(-1. - 2.*x[4]*pc[5]*o[6] - 2.*x[3]*pc[6]*o[7]*o[21] + 2.*x[3]*pc[4]*o[10]*o[25]);
87 | out[1] = pc[0]*(2.*x[3]*pc[5]*o[6] - 2.*x[4]*pc[6]*o[7]*o[21] + 2.*x[4]*pc[4]*o[10]*o[25]);
88 | out[2] = pc[0]*(-2.*pc[6]*o[10]*o[21] - 2.*pc[4]*o[7]*o[25]);
89 | out[3] = pc[0]*(2.*pc[5]*o[4]*o[6] - 2.*pc[6]*o[7]*o[21]*o[27] + 2.*pc[4]*o[10]*o[25]*o[27]);
90 | out[4] = pc[0]*(2.*pc[5]*o[1]*o[6] - 2.*pc[6]*o[4]*o[7]*o[21] + 2.*pc[4]*o[4]*o[10]*o[25]);
91 | }
92 | void OrientationConstraint_20170227::dldu(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes){
93 | out[0] = 0;
94 | out[1] = 0;
95 | out[2] = 0;
96 | out[3] = 0;
97 | }
98 |
--------------------------------------------------------------------------------
/src/user_generated/OrientationConstraint_20170227.h:
--------------------------------------------------------------------------------
1 | /**
2 | * @file OrientationConstraint_20170227.h
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief Exported with Mathematica Code Generator by Jan Dentler
9 | */
10 |
11 |
12 | #ifndef ORIENTATIONCONSTRAINT_20170227_H_
13 | #define ORIENTATIONCONSTRAINT_20170227_H_
14 |
15 | #include
16 |
17 | /*******************************************************
18 | * Constructor *
19 | *******************************************************/
20 |
21 | class OrientationConstraint_20170227:public Constraint{
22 | public:
23 | OrientationConstraint_20170227(int id=0);
24 | OrientationConstraint_20170227(Agent* agent,int id=0);
25 | void l(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes);
26 | void dldx(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes);
27 | void dldu(double *out, double t, double *x, double *u, double *p, double *pc, double *xdes, double *udes);
28 | };
29 | #endif
30 |
--------------------------------------------------------------------------------
/src/user_scenarios/ardrone_pose_tracking.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file scenario.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief User Scenario
9 | *
10 | * Scenario is defining containing int main() and defining the control scenario
11 | */
12 |
13 | #include "Scheduler.h"
14 | #include "Agent.h"
15 | #include "Constraint.h"
16 | #include "Coupling.h"
17 |
18 | #include "Ardrone_20170227.h"
19 | #include "OrientationConstraint_20170227.h"
20 | #include "CollisionAvoidanceCoupling_20170227.h"
21 |
22 | #include "Cmscgmres.h"
23 | #include "Event.h"
24 | #include "std_msgs/Bool.h"
25 |
26 | int main(int argc, char **argv){
27 |
28 | //Initialize ros node
29 | ros::init(argc, argv, "controller");
30 |
31 | /****** Initialize Agent Instances ******/
32 | std::vector agentlist;
33 |
34 | //Initialize: Ardrone instance: ardrone1
35 | Ardrone_20170227* ardrone0=new Ardrone_20170227(agentlist.size());
36 | agentlist.push_back(ardrone0); /*add to agentlist*/
37 |
38 | /****** Initialize Coupling Instances ******/
39 | std::vector controllerlist;
40 |
41 | //Initialize: Controller
42 | Cmscgmres* controller1=new Cmscgmres(agentlist,controllerlist.size());
43 | controller1->activateInfo_ControllerStates();
44 | controllerlist.push_back(controller1);
45 |
46 | /****** Initialize Events ******/
47 | std::vector eventlist;
48 |
49 | /****** Initialize Scheduler ******/
50 | Scheduler scheduler(argc,argv,controllerlist, eventlist);
51 | scheduler.run_control(0.01);
52 | };
53 |
54 |
55 |
56 |
--------------------------------------------------------------------------------
/src/user_scenarios/ardrone_sensor_tracking.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file scenario.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief User Scenario
9 | *
10 | * Scenario is defining containing int main() and defining the control scenario
11 | */
12 |
13 | #include "Scheduler.h"
14 | #include "Agent.h"
15 | #include "Constraint.h"
16 | #include "Coupling.h"
17 |
18 | #include "Ardrone_20170227.h"
19 | #include "OrientationConstraint_20170227.h"
20 | #include "CollisionAvoidanceCoupling_20170227.h"
21 |
22 | #include "Cmscgmres.h"
23 | #include "Event.h"
24 | #include "std_msgs/Bool.h"
25 |
26 | int main(int argc, char **argv){
27 |
28 | //Initialize ros node
29 | ros::init(argc, argv, "controller");
30 |
31 | /****** Initialize Agent Instances ******/
32 | std::vector agentlist;
33 |
34 | //Initialize: Ardrone instance: ardrone1
35 | Ardrone_20170227* ardrone0=new Ardrone_20170227(agentlist.size());
36 |
37 | double ardrone0_init_p[]={ //without tracking position
38 | -0.5092,1.458,-1,1,-5,1.3, //Model Parameters: Ardrone
39 | 0.0,0.0,0.0, 0.0,0.0, 0.0,0.0,0.0,0.0, //State Penalty: no tracking
40 | 1.0,1.0,0.3,1.0 //Input penalty
41 | };
42 | ardrone0->setInitialParameter(ardrone0_init_p);
43 | agentlist.push_back(ardrone0); /*add to agentlist*/
44 |
45 | //AddConstraint
46 | std::vector constraintlist;
47 | Constraint* constraint= new OrientationConstraint_20170227(ardrone0);
48 | //constraint_init_p{k0, ds, beta, ddist, kforw, kside, kup}
49 | double constraint0_init_p[]={1,0.17,0.5,1.5,1,1,1};
50 | constraint->setInitialParameter(constraint0_init_p);
51 | constraint->setParameter(constraint0_init_p);
52 |
53 | /****** Initialize Coupling Instances ******/
54 | std::vector controllerlist;
55 |
56 | //Initialize: Controller
57 | Cmscgmres* controller1=new Cmscgmres(agentlist,controllerlist.size());
58 | controller1->activateInfo_ControllerStates();
59 | controllerlist.push_back(controller1);
60 |
61 |
62 | /****** Initialize Events ******/
63 | std::vector eventlist;
64 |
65 | /****** Initialize Scheduler ******/
66 | Scheduler scheduler(argc,argv,controllerlist, eventlist);
67 | scheduler.run_control(0.01);
68 | };
69 |
70 |
71 |
72 |
--------------------------------------------------------------------------------
/src/user_scenarios/scenario.cpp:
--------------------------------------------------------------------------------
1 | /**
2 | * @file scenario.cpp
3 | * @Author Jan Dentler (jan.dentler@uni.lu)
4 | * University of Luxembourg
5 | * @date 27.February, 2017
6 | * @time 23:23h
7 | * @license GPLv3
8 | * @brief User Scenario
9 | *
10 | * Scenario is defining containing int main() and defining the control scenario
11 | */
12 |
13 | #include "Scheduler.h"
14 | #include "Agent.h"
15 | #include "Constraint.h"
16 | #include "Coupling.h"
17 |
18 | #include "Ardrone_20170227.h"
19 | #include "OrientationConstraint_20170227.h"
20 | #include "CollisionAvoidanceCoupling_20170227.h"
21 |
22 | #include "Cmscgmres.h"
23 | #include "Event.h"
24 | #include "std_msgs/Bool.h"
25 |
26 | int main(int argc, char **argv){
27 |
28 | //Initialize ros node
29 | ros::init(argc, argv, "controller");
30 |
31 | /****** Initialize Agent Instances ******/
32 | std::vector agentlist;
33 |
34 | //Initialize: Ardrone instance: ardrone1
35 | Ardrone_20170227* ardrone0=new Ardrone_20170227(agentlist.size());
36 |
37 | double ardrone0_init_p[]={ //without tracking position
38 | -0.5092,1.458,-1,1,-5,1.3, //Model Parameters: Ardrone
39 | 0.0,0.0,0.0, 0.0,0.0, 0.0,0.0,0.0,0.0, //State Penalty: no tracking
40 | 1.0,1.0,0.3,1.0 //Input penalty
41 | };
42 | double ardrone0_init_x[]= {0,0,2, 1,0, 0.0,0.0,0.0,0.0};
43 | double ardrone0_init_xdes[]={0,0,2, 1,0, 0.0,0.0,0.0,0.0};
44 | ardrone0->setInitialState(ardrone0_init_x);
45 | ardrone0->setInitialDesiredState(ardrone0_init_xdes);
46 | ardrone0->setInitialParameter(ardrone0_init_p);
47 | // ardrone0->setStateSubscriberRosTopicName ("/Ardrone2SimpleLinModel_HASHMARK_0/pose");
48 | // ardrone0->setDesiredStateSubscriberRosTopicName("/Ardrone2SimpleLinModel_HASHMARK_0/desiredpose");
49 | // ardrone0->setPublisherRosTopicName ("/Ardrone2SimpleLinModel_HASHMARK_0/ext_ctrl_ch");
50 | agentlist.push_back(ardrone0); /*add to agentlist*/
51 |
52 | //AddConstraint
53 | std::vector constraintlist;
54 | Constraint* constraint= new OrientationConstraint_20170227(ardrone0);
55 |
56 | //constraint_init_p{k0, ds, beta, ddist, kforw, kside, kup}
57 | double constraint0_init_p[]={1,0.17,0.5,1.5,1,1,1};
58 | constraint->setInitialParameter(constraint0_init_p);
59 | constraint->setParameter(constraint0_init_p);
60 |
61 | /****** Initialize Coupling Instances ******/
62 | std::vector controllerlist;
63 |
64 | //Initialize: Controller
65 | Cmscgmres* controller1=new Cmscgmres(agentlist,controllerlist.size());
66 | // controller1->setHorizonDiskretization(10);
67 | // controller1->setHorizonLength(1);
68 | // controller1->setTolerance(1e-8);
69 | // controller1->setUpdateIntervall(0.01);
70 | // controller1->setMaximumNumberofIterations(10);
71 | // controller1->activateInfo_ControllerTrace();
72 | // controller1->activateInfo_Controller();
73 | // controller1->startLogging2File();
74 | controller1->activateInfo_ControllerStates();
75 | controllerlist.push_back(controller1);
76 |
77 |
78 | /****** Initialize Events ******/
79 | std::vector eventlist;
80 |
81 | /****** Initialize Scheduler ******/
82 | Scheduler scheduler(argc,argv,controllerlist, eventlist);
83 | scheduler.run_control(0.01);
84 | // scheduler.run_vrep(0.01);
85 | // scheduler.run_simulation(0.1,10);
86 |
87 | };
88 |
89 |
90 |
91 |
--------------------------------------------------------------------------------