hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

YOPO

Browse Source
This commit is contained in:
TJU_Lu
2025-06-16 23:18:04 +08:00
commit 94ceefc057
509 changed files with 105903 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
.gitignore vendored Normal file
View File

@@ -0,0 +1,16 @@
.idea/
Controller/devel/
Controller/build/
Controller/.catkin_workspace
Simulator/devel/
Simulator/build/
Simulator/logs/
Simulator/.catkin_tools/
YOPO/saved/*
dataset/
yopo_sim/
!YOPO/saved/YOPO_1/

1
Controller/src/.gitignore vendored Normal file
View File

@@ -0,0 +1 @@
/so3_control/logger/*.csv

69
Controller/src/CMakeLists.txt Normal file
View File

@@ -0,0 +1,69 @@
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 3.0.2)
project(Project)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()

35
Controller/src/readme.md Normal file
View File

@@ -0,0 +1,35 @@
# UAV Simulator
### Build:
```bash
catkin_make
```
### MODE 1. PID Position Controller and Simulator
Work with traditional planner
```
source devel/setup.bash
roslaunch so3_quadrotor_simulator simulator_position_control.launch
```
### MODE 2. Attitude Controller with Disturbance Observer
Work with our learning-based planner (without position controller)
```
source devel/setup.bash
roslaunch so3_quadrotor_simulator simulator_attitude_control.launch
```
### Others
pub disturbance
```
rostopic pub /force_disturbance
```
takeoff and land (used in realworld flight)
```
rosservice call /network_controller_node/takeoff_land
```
### acknowledgment
This repo is modified from https://github.com/HKUST-Aerial-Robotics/Fast-Planner, thanks for their excellent work!

49
Controller/src/so3_control/CMakeLists.txt Executable file
View File

@@ -0,0 +1,49 @@
cmake_minimum_required(VERSION 2.8.3)
project(so3_control)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
nav_msgs
quadrotor_msgs
tf
nodelet
cmake_utils
)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
catkin_package()
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_library(SO3Control src/SO3Control.cpp)
add_library(so3_control_nodelet src/so3_control_nodelet.cpp)
target_link_libraries(so3_control_nodelet
${catkin_LIBRARIES}
SO3Control
)
# 普通PID控制器
add_executable(control_example src/control_example.cpp)
target_link_libraries(control_example
${catkin_LIBRARIES}
)
# 接收加速度直接转为姿态，并叠加干扰观测器作为内环输入
add_executable(network_control_node src/NetworkControl.cpp src/network_control_node.cpp)
target_link_libraries(network_control_node
${catkin_LIBRARIES}
SO3Control
)

4
Controller/src/so3_control/config/corrections_hummingbird.yaml Executable file
View File

@@ -0,0 +1,4 @@
corrections:
z: 0.0
r: 0.0
p: 0.0

4
Controller/src/so3_control/config/corrections_pelican.yaml Executable file
View File

@@ -0,0 +1,4 @@
corrections:
z: 0.0
r: 0.0
p: 0.0

6
Controller/src/so3_control/config/gains.yaml Executable file
View File

@@ -0,0 +1,6 @@
# Gains for Laser-based Pelican
gains:
pos: {x: 5.0, y: 5.0, z: 15.0}
vel: {x: 5.0, y: 5.0, z: 5.0}
rot: {x: 3.5, y: 3.5, z: 1.0}
ang: {x: 0.4, y: 0.4, z: 0.1}

6
Controller/src/so3_control/config/gains_hummingbird.yaml Executable file
View File

@@ -0,0 +1,6 @@
# Vision Gain for Hummingbird
gains:
pos: {x: 2.0, y: 2.0, z: 3.5}
vel: {x: 1.8, y: 1.8, z: 2.0}
rot: {x: 1.0, y: 1.0, z: 0.3}
ang: {x: 0.07, y: 0.07, z: 0.02}

6
Controller/src/so3_control/config/gains_pelican.yaml Executable file
View File

@@ -0,0 +1,6 @@
# Gains for Laser-based Pelican
gains:
pos: {x: 5.0, y: 5.0, z: 15.0}
vel: {x: 5.0, y: 5.0, z: 5.0}
rot: {x: 3.5, y: 3.5, z: 1.0}
ang: {x: 0.4, y: 0.4, z: 0.1}

110
Controller/src/so3_control/include/so3_control/HGDO.h Normal file
View File

@@ -0,0 +1,110 @@
#ifndef HGDO_H_
#define HGDO_H_
#include "ros/ros.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
inline void get_dcm_from_q(Eigen::Matrix3d &dcm, const Eigen::Quaterniond &q) {
float a = q.w();
float b = q.x();
float c = q.y();
float d = q.z();
float aSq = a*a;
float bSq = b*b;
float cSq = c*c;
float dSq = d*d;
dcm(0, 0) = aSq + bSq - cSq - dSq;
dcm(0, 1) = 2 * (b * c - a * d);
dcm(0, 2) = 2 * (a * c + b * d);
dcm(1, 0) = 2 * (b * c + a * d);
dcm(1, 1) = aSq - bSq + cSq - dSq;
dcm(1, 2) = 2 * (c * d - a * b);
dcm(2, 0) = 2 * (b * d - a * c);
dcm(2, 1) = 2 * (a * b + c * d);
dcm(2, 2) = aSq - bSq - cSq + dSq;
}
class HGDO
{
public:
HGDO(){};
HGDO(double control_dt){
control_dt_ = control_dt;
};
double Sat(double &Input)
{
double D = 1.0;
double abs_input = abs(Input);
if (abs_input <= D)
{
return Input;
}
else
{
return Input / abs_input;
}
}
// 用这个,放到控制器的定时器和控制器同步,注意有些全局变量在FxTDO
// 模型预测，这个z2_nhgdo作为北西天系，（用来算姿态的）加速度直接加z2_nhgdo作为实际的加速度
// U_input: 上一时刻期望加速度（控制量姿态对应的）
// vel: 当前时刻的速度
// dis: 当前时刻干扰返回值,就是3个方向加速度
void HGDO_ext_force_ob(const Eigen::Vector3d &U_input, const Eigen::Vector3d &vel, Eigen::Vector3d &dis)
{
Eigen::Vector3d dz1, dz2;
Eigen::Vector3d U = U_input;
// U(2) -= g;
for (int i = 0; i < 3; i++)
{
dz1(i) = U(i) + z2_hgdo(i) - (alpha_1 / theta) * (z1_hgdo(i) - vel(i));
dz2(i) = -(alpha_2 / pow(theta, 2)) * (z1_hgdo(i) - vel(i));
}
for (int i = 0; i < 3; i++)
{
z1_hgdo(i) += dz1(i) * control_dt_;
z2_hgdo(i) += dz2(i) * control_dt_;
}
dis = z2_hgdo;
}
// 自适应干扰观测，立文讲先不用
void AHGDO_ext_force_ob(Eigen::Vector3d &U_input, Eigen::Vector3d &vel, Eigen::Vector3d &dis)
{
Eigen::Vector3d dz1, dz2;
Eigen::Vector3d U = U_input;
// U(2) += g;
for (int i = 0; i < 3; i++)
{
double ob = (z1_nhgdo(i) - vel(i)) / d_bound;
dz1(i) = U(i) + z2_nhgdo(i) - (alpha_1 / theta_1) * (z1_nhgdo(i) - vel(i)) - (alpha_1 * d_bound * (1 / theta_2 - 1 / theta_1)) * Sat(ob);
dz2(i) = -(alpha_2 / pow(theta_1, 2)) * (z1_nhgdo(i) - vel(i)) - (alpha_2 * d_bound * (1 / pow(theta_2, 2) - 1 / pow(theta_2, 2))) * Sat(ob);
}
for (int i = 0; i < 3; i++)
{
z1_nhgdo(i) += dz1(i) * control_dt_;
z2_nhgdo(i) += dz2(i) * control_dt_;
}
dis = z2_nhgdo;
}
private:
double g = 9.81;
double control_dt_{0.02};
Eigen::Vector3d z1_hgdo = Eigen::Vector3d::Zero();
Eigen::Vector3d z2_hgdo = Eigen::Vector3d::Zero();
Eigen::Vector3d z1_nhgdo = Eigen::Vector3d::Zero();
Eigen::Vector3d z2_nhgdo = Eigen::Vector3d::Zero();
double theta = 0.2;
double theta_1 = 0.2;
double theta_2 = 0.5;
double alpha_1 = 3.0;
double alpha_2 = 2.0;
double d_bound = 0.01;
};
#endif

156
Controller/src/so3_control/include/so3_control/NetworkControl.h Normal file
View File

@@ -0,0 +1,156 @@
#ifndef NETWORK_CONTROL_H_
#define NETWORK_CONTROL_H_
#include <Eigen/Eigen>
#include <math.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <quadrotor_msgs/SO3Command.h>
#include <tf/transform_datatypes.h>
#include <ros/ros.h>
#include <so3_control/SO3Control.h>
#include <so3_control/HGDO.h>
#include <so3_control/mavros_interface.h>
#include <quadrotor_msgs/SetTakeoffLand.h>
#include <string>
#include <iostream>
#include <boost/filesystem.hpp>
#include <regex>
#include <fstream>
#include <thread>
#include <mutex>
#include <algorithm>
#define ONE_G 9.81
class NetworkControl
{
public:
NetworkControl(ros::NodeHandle &node){
nh_ = node;
so3_controller_.setMass(mass_);
disturbance_observer_ = HGDO(control_dt_);
nh_.param("is_simulation", is_simulation_, false);
nh_.param("use_disturbance_observer", use_disturbance_observer_, false);
nh_.param("hover_thrust", hover_thrust_, 0.4);
nh_.param("kx_xy", kx_xy, 5.7);
nh_.param("kx_z", kx_z, 6.2);
nh_.param("kv_xy", kv_xy, 3.4);
nh_.param("kv_z", kv_z, 4.0);
nh_.param("record_log", record_log_, false);
nh_.param("logger_file_name", logger_file_name, std::string("/home/lu/"));
printf("kx: (%f, %f, %f), kv: (%f, %f, %f) \n", kx_xy, kx_xy, kx_z, kv_xy, kv_xy, kv_z);
so3_command_pub_ = nh_.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);
position_cmd_sub_ = nh_.subscribe("position_cmd", 1, &NetworkControl::network_cmd_callback, this, ros::TransportHints().tcpNoDelay());
odom_sub_ = nh_.subscribe("odom", 1, &NetworkControl::odom_callback, this, ros::TransportHints().tcpNoDelay());
imu_sub_ = nh_.subscribe("imu", 1, &NetworkControl::imu_callback, this, ros::TransportHints().tcpNoDelay());
takeoff_land_control_timer = nh_.createTimer(ros::Duration(control_dt_), &NetworkControl::timerCallback, this);
takeoff_land_srv = nh_.advertiseService("takeoff_land", &NetworkControl::takeoff_land_srv_handle, this);
if (is_simulation_) {
ros::Duration(2.0).sleep();
std::thread(&NetworkControl::simulateTakeoff, this).detach();
}
};
~NetworkControl(){};
private:
ros::NodeHandle nh_;
ros::Publisher so3_command_pub_;
ros::Subscriber position_cmd_sub_, odom_sub_, imu_sub_;
ros::ServiceServer takeoff_land_srv;
ros::Timer takeoff_land_control_timer;
std::mutex mutex_;
double mass_ = 0.98;
double control_dt_ = 0.02;
double hover_thrust_ = 0.4;
double kx_xy, kx_z, kv_xy, kv_z;
double cur_yaw_ = 0;
Eigen::Vector3d cur_pos_ = Eigen::Vector3d(0, 0, 0);
Eigen::Vector3d cur_vel_ = Eigen::Vector3d(0, 0, 0);
Eigen::Vector3d cur_acc_ = Eigen::Vector3d(0, 0, 0);
Eigen::Quaterniond cur_att_ = Eigen::Quaterniond::Identity();
Eigen::Vector3d dis_acc_ = Eigen::Vector3d(0, 0, 0);
Eigen::Vector3d last_des_acc_ = Eigen::Vector3d(0, 0, 0);
double last_thrust_ = 0;
Eigen::Vector3d des_pos_ = Eigen::Vector3d(0, 0, 0);
Eigen::Vector3d des_vel_ = Eigen::Vector3d(0, 0, 0);
Eigen::Vector3d des_acc_ = Eigen::Vector3d(0, 0, 0);
double des_yaw_ = 0;
double des_yaw_dot_ = 0;
bool is_simulation_ = false;
bool state_init_ = false;
bool ref_valid_ = false;
bool ctrl_valid_ = false;
bool position_cmd_init_ = false;
bool takeoff_cmd_init_ = false;
bool use_disturbance_observer_ = false;
bool record_log_ = false;
SO3Control so3_controller_;
HGDO disturbance_observer_;
Mavros_Interface mavros_interface_;
std::ofstream logger;
std::string logger_file_name;
void initLogRecorder();
void recordLog(Eigen::Vector3d &cur_v, Eigen::Vector3d &cur_a, Eigen::Vector3d &des_a, Eigen::Vector3d &dis_a, double cur_yaw, double des_yaw);
Eigen::Vector3d publishHoverSO3Command(Eigen::Vector3d des_pos_, Eigen::Vector3d des_vel_, Eigen::Vector3d des_acc_, double des_yaw_, double des_yaw_dot_);
void get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des);
void pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw);
void limite_acc(Eigen::Vector3d &acc);
void network_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr &cmd);
void odom_callback(const nav_msgs::Odometry::ConstPtr &odom);
void imu_callback(const sensor_msgs::Imu &imu);
void timerCallback(const ros::TimerEvent&);
// mavros interface
bool takeoff_land_srv_handle(quadrotor_msgs::SetTakeoffLand::Request &req,
quadrotor_msgs::SetTakeoffLand::Response &res){
std::thread t(&NetworkControl::takeoff_land_thread, this, std::ref(req));
t.detach();
res.res = true;
return true;
}
bool arm_disarm_vehicle(bool arm);
void takeoff_land_thread(quadrotor_msgs::SetTakeoffLand::Request &req);
void simulateTakeoff() {
ros::ServiceClient client = nh_.serviceClient<quadrotor_msgs::SetTakeoffLand>("takeoff_land");
quadrotor_msgs::SetTakeoffLand srv;
srv.request.takeoff = true;
srv.request.takeoff_altitude = 2.0;
if (client.call(srv)) {
ROS_INFO("Takeoff called successfully");
} else {
ROS_ERROR("Failed to call takeoff service");
}
}
};
#endif

42
Controller/src/so3_control/include/so3_control/SO3Control.h Executable file
View File

@@ -0,0 +1,42 @@
#ifndef __SO3_CONTROL_H__
#define __SO3_CONTROL_H__
#include <Eigen/Geometry>
class SO3Control
{
public:
SO3Control();
void setMass(const double mass);
void setGravity(const double g);
void setPosition(const Eigen::Vector3d& position);
void setVelocity(const Eigen::Vector3d& velocity);
void setAcc(const Eigen::Vector3d& acc);
void calculateControl(const Eigen::Vector3d& des_pos,
const Eigen::Vector3d& des_vel,
const Eigen::Vector3d& des_acc, const double des_yaw,
const double des_yaw_dot, const Eigen::Vector3d& kx,
const Eigen::Vector3d& kv);
const Eigen::Vector3d& getComputedForce(void);
const Eigen::Quaterniond& getComputedOrientation(void);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// Inputs for the controller
double mass_;
double g_;
Eigen::Vector3d pos_;
Eigen::Vector3d vel_;
Eigen::Vector3d acc_;
// Outputs of the controller
Eigen::Vector3d force_;
Eigen::Quaterniond orientation_;
};
#endif

194
Controller/src/so3_control/include/so3_control/mavros_interface.h Normal file
View File

@@ -0,0 +1,194 @@
#ifndef MAVROS_INTERFACE_H_
#define MAVROS_INTERFACE_H_
#include "ros/ros.h"
#include <stdio.h>
#include <stdlib.h>
#include <string>
#include <iostream>
#include <mavros_msgs/AttitudeTarget.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
class Mavros_Interface
{
public:
Mavros_Interface() : _nh("~cmd")
{
_state.reset();
std::string base_name = "/mavros";
// char id_str[10];
// sprintf(id_str, "%d", id);
// base_name += id_str;
std::string att_target_pub_name;
att_target_pub_name = base_name + "/setpoint_raw/attitude";
att_target_pub = _nh.advertise<mavros_msgs::AttitudeTarget>(att_target_pub_name.c_str(), 10);
std::string state_sub_name;
state_sub_name = base_name + "/state";
state_sub = _nh.subscribe(state_sub_name.c_str(), 10, &Mavros_Interface::state_cb, this);
std::string set_mode_s_name;
set_mode_s_name = base_name + "/set_mode";
set_mode_client = _nh.serviceClient<mavros_msgs::SetMode>(set_mode_s_name.c_str());
std::string arm_disarm_s_name;
arm_disarm_s_name = base_name + "/cmd/arming";
arm_disarm_client = _nh.serviceClient<mavros_msgs::CommandBool>(arm_disarm_s_name.c_str());
}
~Mavros_Interface() {}
typedef struct mavros_state_t
{
ros::Time header;
bool has_armed;
bool offboard_enabled;
void reset()
{
has_armed = false;
offboard_enabled = false;
}
mavros_state_t()
{
reset();
}
};
void state_cb(const mavros_msgs::State &state_data)
{
mavros_msgs::State temp_data = state_data;
_state.header = state_data.header.stamp;
_state.has_armed = state_data.armed;
if (state_data.mode == "OFFBOARD")
{
_state.offboard_enabled = true;
}
else
{
_state.offboard_enabled = false;
}
}
void get_status(bool &arm_state, bool &offboard_enabled)
{
arm_state = _state.has_armed;
offboard_enabled = _state.offboard_enabled;
}
bool set_arm_and_offboard()
{
ros::Rate _ofb_check_rate(1);
int try_arm_ofb_times = 0;
while (!_state.offboard_enabled || !_state.has_armed)
{
if (_state.offboard_enabled)
{
ros::Rate _arm_check_rate(1);
while (!_state.has_armed)
{
mavros_msgs::CommandBool arm_srv;
arm_srv.request.value = true;
if (arm_disarm_client.call(arm_srv))
{
ROS_INFO("vehicle ARMED");
}
try_arm_ofb_times = try_arm_ofb_times + 1;
if (try_arm_ofb_times >= 3)
{
ROS_ERROR("try 3 times, cannot armed uav, give up!");
return false;
}
_arm_check_rate.sleep();
}
}
else
{
ROS_INFO("not in OFFBOARD mode");
mavros_msgs::SetMode set_mode_srv;
set_mode_srv.request.base_mode = 0;
set_mode_srv.request.custom_mode = "OFFBOARD";
if (!set_mode_client.call(set_mode_srv))
{
return false;
}
ROS_INFO("switch to OFFBOARD mode");
_ofb_check_rate.sleep();
}
}
return true;
}
/* void set_arm(const ros::TimerEvent& event) {
if(_state.offboard_enabled) {
mavros_msgs::CommandBool arm_srv;
arm_srv.request.value = true;
if (arm_disarm_client.call(arm_srv)) {
ROS_INFO("vehicle ARMED");
}
} else {
ROS_INFO("not in OFFBOARD mode");
}
} */
bool set_disarm()
{
ros::Rate _arm_check_rate(1);
while (_state.has_armed)
{
mavros_msgs::CommandBool arm_srv;
arm_srv.request.value = false;
if (!arm_disarm_client.call(arm_srv))
{
return false;
}
ROS_INFO("vehicle DISARMED");
_arm_check_rate.sleep();
}
return true;
}
void pub_att_thrust_cmd(const Eigen::Quaterniond &q_d, const double &thrust_d)
{
/*
目前mavros用的是北东地的坐标系，为了和pid通用所以没有改mavros而是在这里改为北东地
*/
mavros_msgs::AttitudeTarget at_cmd;
at_cmd.header.stamp = ros::Time::now();
at_cmd.type_mask = at_cmd.IGNORE_ROLL_RATE | at_cmd.IGNORE_PITCH_RATE | at_cmd.IGNORE_YAW_RATE;
at_cmd.thrust = (float)thrust_d;
at_cmd.orientation.w = q_d.w();
at_cmd.orientation.x = q_d.x();
at_cmd.orientation.y = -q_d.y();
at_cmd.orientation.z = -q_d.z();
att_target_pub.publish(at_cmd);
}
// for simulation
void set_arm_and_offboard_manually()
{
_state.has_armed = true;
_state.offboard_enabled = true;
}
void set_disarm_manually()
{
_state.has_armed = false;
}
private:
ros::NodeHandle _nh;
ros::Publisher att_target_pub;
ros::Subscriber state_sub;
ros::ServiceClient set_mode_client;
ros::ServiceClient arm_disarm_client;
mavros_state_t _state;
};
#endif

19
Controller/src/so3_control/launch/controller_network.launch Normal file
View File

@@ -0,0 +1,19 @@
<launch>
<arg name="hover_thrust" default="0.38" />
<node pkg="so3_control" type="network_control_node" name="network_controller_node" output="screen">
<param name="is_simulation" value="false"/>
<param name="use_disturbance_observer" value="true"/>
<param name="hover_thrust" value="$(arg hover_thrust)"/>
<param name="kx_xy" value="5.7"/>
<param name="kx_z" value="6.2"/>
<param name="kv_xy" value="3.4"/>
<param name="kv_z" value="4.0"/>
<remap from="~odom" to="/vins_estimator/imu_propagate"/>
<remap from="~imu" to="/mavros/imu/data_raw"/>
<remap from="~position_cmd" to="/so3_control/pos_cmd"/>
<remap from="~so3_cmd" to="so3_cmd"/>
<param name="record_log" value = "true"/>
<param name="logger_file_name" value = "$(find so3_control)/logger/"/>
</node>
</launch>

28
Controller/src/so3_control/logger/log_plot.py Executable file
View File

@@ -0,0 +1,28 @@
import sys
import numpy as np
import matplotlib.pyplot as plt
if __name__ == '__main__':
file_path = sys.argv[1]
temp = np.loadtxt(file_path, dtype=str, delimiter=",")
print('load file:', file_path)
label = temp[0, :].astype(str)
data = temp[1:, :].astype(np.float64)
timestamp = (data[:, 0] - data[0, 0])
print('data length:', len(data))
# print(timestamp)
while 1:
for i in range(len(label)):
if i != 0:
print('[', i, ']: ', label[i])
print('plot the curve')
print('example: 1 2 3')
wtp = input('want to plot(\'q\' to quit):')
if wtp == 'q':
break
wtp = [int(n) for n in wtp.split()]
for i in range(len(wtp)):
plt.plot(timestamp, data[:, wtp[i]], linewidth=2.5, linestyle='-', marker='.', label=label[wtp[i]])
plt.xlabel('t')
plt.legend()
plt.show()

14
Controller/src/so3_control/mainpage.dox Executable file
View File

@@ -0,0 +1,14 @@
/**
\mainpage
\htmlinclude manifest.html
\b so3_control
<!--
Provide an overview of your package.
-->
-->
*/

7
Controller/src/so3_control/nodelet_plugin.xml Executable file
View File

@@ -0,0 +1,7 @@
<library path="lib/libso3_control_nodelet">
<class name="so3_control/SO3ControlNodelet" type="SO3ControlNodelet" base_class_type="nodelet::Nodelet">
<description>
so3_control
</description>
</class>
</library>

35
Controller/src/so3_control/package.xml Executable file
View File

@@ -0,0 +1,35 @@
<package>
<version>0.0.0</version>
<name>so3_control</name>
<description >
so3_control
</description>
<maintainer email="todo@todo.todo">Kartik Mohta</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/so3_control</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>quadrotor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>cmake_utils</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>quadrotor_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>cmake_utils</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugin.xml"/>
</export>
</package>

437
Controller/src/so3_control/src/NetworkControl.cpp Normal file
View File

@@ -0,0 +1,437 @@
#include "so3_control/NetworkControl.h"
void NetworkControl::initLogRecorder()
{
// Use file count as name to avoid date confusion
std::cout << "logger_file_name: " << logger_file_name << std::endl;
int max_number = -1;
boost::filesystem::path dir(logger_file_name);
if (boost::filesystem::exists(dir) && boost::filesystem::is_directory(dir)) {
std::regex filename_pattern(R"(^(\d+)_.*$)"); // 匹配以数字开头，并以"_"分隔的文件名
for (const auto& entry : boost::filesystem::directory_iterator(dir)) {
if (boost::filesystem::is_regular_file(entry)) {
std::string filename = entry.path().filename().string();
std::smatch match;
if (std::regex_match(filename, match, filename_pattern)) {
int number = std::stoi(match[1]); // 提取第一个"_"前的数字
max_number = std::max(max_number, number);
}
}
}
} else {
std::cerr << "Error: invalid logger path" << std::endl;
}
std::string fileCountStr = std::to_string(max_number + 1);
std::string temp_file_name = logger_file_name + fileCountStr + "_Net_logger_";
time_t timep;
timep = time(0);
char tmp[64];
strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S", localtime(&timep));
temp_file_name += tmp;
temp_file_name += ".csv";
if (logger.is_open())
{
logger.close();
}
logger.open(temp_file_name.c_str(), std::ios::out);
std::cout << "logger: " << temp_file_name << std::endl;
if (!logger.is_open())
{
std::cout << "cannot open the logger." << std::endl;
}
else
{
logger << "timestamp" << ',';
logger << "cur_px" << ',';
logger << "cur_py" << ',';
logger << "cur_pz" << ',';
logger << "cur_vx" << ',';
logger << "cur_vy" << ',';
logger << "cur_vz" << ',';
logger << "cur_ax" << ',';
logger << "cur_ay" << ',';
logger << "cur_az" << ',';
logger << "des_px" << ',';
logger << "des_py" << ',';
logger << "des_pz" << ',';
logger << "des_vx" << ',';
logger << "des_vy" << ',';
logger << "des_vz" << ',';
logger << "des_ax" << ',';
logger << "des_ay" << ',';
logger << "des_az" << ',';
logger << "dis_ax" << ',';
logger << "dis_ay" << ',';
logger << "dis_az" << ',';
logger << "px4_ax" << ',';
logger << "px4_ay" << ',';
logger << "px4_az" << ',';
logger << "thrust" << ',';
logger << "cur_yaw" << ',';
logger << "des_yaw" << std::endl;
}
}
void NetworkControl::recordLog(Eigen::Vector3d &cur_v, Eigen::Vector3d &cur_a, Eigen::Vector3d &des_a, Eigen::Vector3d &dis_a, double cur_yaw, double des_yaw)
{
if (logger.is_open())
{
logger << ros::Time::now().toNSec() << ',';
logger << cur_pos_(0) << ',';
logger << cur_pos_(1) << ',';
logger << cur_pos_(2) << ',';
logger << cur_v(0) << ',';
logger << cur_v(1) << ',';
logger << cur_v(2) << ',';
logger << cur_a(0) << ',';
logger << cur_a(1) << ',';
logger << cur_a(2) << ',';
logger << des_pos_(0) << ',';
logger << des_pos_(1) << ',';
logger << des_pos_(2) << ',';
logger << des_vel_(0) << ',';
logger << des_vel_(1) << ',';
logger << des_vel_(2) << ',';
logger << des_a(0) << ',';
logger << des_a(1) << ',';
logger << des_a(2) << ',';
logger << dis_a(0) << ',';
logger << dis_a(1) << ',';
logger << dis_a(2) << ',';
logger << des_a(0) - dis_a(0) << ',';
logger << des_a(1) - dis_a(1) << ',';
logger << des_a(2) - dis_a(2) << ',';
logger << last_thrust_ << ',';
logger << cur_yaw << ',';
logger << des_yaw << std::endl;
}
}
Eigen::Vector3d NetworkControl::publishHoverSO3Command(Eigen::Vector3d des_pos, Eigen::Vector3d des_vel,
Eigen::Vector3d des_acc, double des_yaw, double des_yaw_dot)
{
Eigen::Vector3d kx(kx_xy, kx_xy, kx_z);
Eigen::Vector3d kv(kv_xy, kv_xy, kv_z);
so3_controller_.calculateControl(des_pos, des_vel, des_acc, des_yaw, des_yaw_dot, kx, kv);
Eigen::Vector3d force = so3_controller_.getComputedForce();
Eigen::Quaterniond orientation = so3_controller_.getComputedOrientation();
quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command); //! @note memory leak?
so3_command->header.stamp = ros::Time::now();
so3_command->force.x = force(0);
so3_command->force.y = force(1);
so3_command->force.z = force(2);
so3_command->orientation.x = orientation.x();
so3_command->orientation.y = orientation.y();
so3_command->orientation.z = orientation.z();
so3_command->orientation.w = orientation.w();
so3_command->kR[0] = 1.5;
so3_command->kR[1] = 1.5;
so3_command->kR[2] = 1.0;
so3_command->kOm[0] = 0.13;
so3_command->kOm[1] = 0.13;
so3_command->kOm[2] = 0.1;
so3_command->aux.current_yaw = cur_yaw_;
so3_command->aux.enable_motors = true;
so3_command_pub_.publish(so3_command);
double thrust_norm = force.norm() / (mass_ * ONE_G) * hover_thrust_;
mavros_interface_.pub_att_thrust_cmd(orientation, thrust_norm);
last_thrust_ = thrust_norm;
double thrust = force.norm() / mass_;
Eigen::Matrix3d Cbn;
get_dcm_from_q(Cbn, orientation);
Eigen::Vector3d att_acc = Eigen::Vector3d(0, 0, thrust);
att_acc = Cbn * att_acc;
att_acc(2) -= ONE_G;
// std::cout<<"att_acc"<<att_acc.transpose()<<std::endl;
return att_acc;
}
void NetworkControl::get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des)
{
Eigen::Vector3d force_ = mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
force_.noalias() += mass_ * ref_acc;
// Limit control angle to theta degree
double theta = M_PI / 4;
double c = cos(theta);
Eigen::Vector3d f;
f.noalias() = force_ - mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
double nf = f.norm();
double A = c * c * nf * nf - f(2) * f(2);
double B = 2 * (c * c - 1) * f(2) * mass_ * ONE_G;
double C = (c * c - 1) * mass_ * mass_ * ONE_G * ONE_G;
double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
force_.noalias() = s * f + mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
}
Eigen::Vector3d b1c, b2c, b3c;
Eigen::Vector3d b1d(cos(ref_yaw), sin(ref_yaw), 0);
if (force_.norm() > 1e-6)
b3c.noalias() = force_.normalized();
else
b3c.noalias() = Eigen::Vector3d(0, 0, 1);
b2c.noalias() = b3c.cross(b1d).normalized();
b1c.noalias() = b2c.cross(b3c).normalized();
Eigen::Matrix3d R;
R << b1c, b2c, b3c;
quat_des = Eigen::Quaterniond(R);
force_des = force_;
}
// 世界系的期望加速度：ref_acc（加上g）、期望yaw：ref_yaw
void NetworkControl::pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw)
{
Eigen::Vector3d force;
Eigen::Quaterniond quat_des;
get_Q_from_ACC(ref_acc, ref_yaw, quat_des, force);
quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command);
so3_command->header.stamp = ros::Time::now();
so3_command->force.x = force(0);
so3_command->force.y = force(1);
so3_command->force.z = force(2);
so3_command->orientation.x = quat_des.x();
so3_command->orientation.y = quat_des.y();
so3_command->orientation.z = quat_des.z();
so3_command->orientation.w = quat_des.w();
so3_command->kR[0] = 1.5;
so3_command->kR[1] = 1.5;
so3_command->kR[2] = 1.0;
so3_command->kOm[0] = 0.13;
so3_command->kOm[1] = 0.13;
so3_command->kOm[2] = 0.1;
so3_command->aux.current_yaw = cur_yaw;
so3_command->aux.enable_motors = true;
so3_command_pub_.publish(so3_command);
double thrust_norm = force.norm() / (mass_ * ONE_G) * hover_thrust_;
mavros_interface_.pub_att_thrust_cmd(quat_des, thrust_norm);
last_thrust_ = thrust_norm;
}
void NetworkControl::limite_acc(Eigen::Vector3d &acc){
return; // limited if needed
acc[0] = std::max(-8.0, std::min(acc[0], 8.0));
acc[1] = std::max(-8.0, std::min(acc[1], 8.0));
acc[2] = std::max(-4.0, std::min(acc[2], 4.0));
}
void NetworkControl::network_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr &cmd)
{
if (!ctrl_valid_)
return;
bool arm_state = false;
bool ofb_enable = false;
mavros_interface_.get_status(arm_state, ofb_enable);
if (!arm_state || !ofb_enable)
return;
position_cmd_init_ = true;
Eigen::Vector3d des_acc = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
limite_acc(des_acc);
double des_yaw = cmd->yaw;
disturbance_observer_.HGDO_ext_force_ob(last_des_acc_, cur_vel_, dis_acc_);
// ROS_INFO_THROTTLE(0.5, "dis_acc: %.3f, %.3f, %.3f", dis_acc_.x(), dis_acc_.y(), dis_acc_.z());
// std::cout << "dis_acc: " << dis_acc_.transpose() << std::endl;
Eigen::Vector3d att_acc;
if (cmd->trajectory_flag == quadrotor_msgs::PositionCommand::TRAJECTORY_STATUS_READY)
{
if (use_disturbance_observer_)
att_acc = des_acc - dis_acc_;
else
att_acc = des_acc;
pub_SO3_command(att_acc, des_yaw, cur_yaw_);
// std::cout<<"acc: "<<des_acc.transpose()<<" yaw:"<<des_yaw<<std::endl;
if (record_log_)
recordLog(cur_vel_, cur_acc_, des_acc, dis_acc_, cur_yaw_, des_yaw);
}
else
{
Eigen::Vector3d des_pos = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
Eigen::Vector3d des_vel = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
double des_yaw = cmd->yaw;
double des_yaw_dot = cmd->yaw_dot;
att_acc = publishHoverSO3Command(des_pos, des_vel, des_acc, des_yaw, des_yaw_dot);
if (record_log_)
recordLog(cur_vel_, cur_acc_, att_acc, dis_acc_, cur_yaw_, des_yaw);
}
last_des_acc_ = att_acc;
}
void NetworkControl::odom_callback(const nav_msgs::Odometry::ConstPtr &odom)
{
cur_yaw_ = tf::getYaw(odom->pose.pose.orientation);
cur_vel_ = Eigen::Vector3d(odom->twist.twist.linear.x, odom->twist.twist.linear.y, odom->twist.twist.linear.z);
cur_pos_ = Eigen::Vector3d(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z);
cur_att_.w() = odom->pose.pose.orientation.w;
cur_att_.x() = odom->pose.pose.orientation.x;
cur_att_.y() = odom->pose.pose.orientation.y;
cur_att_.z() = odom->pose.pose.orientation.z;
// if(!is_simulation_)
// cur_acc_ = Eigen::Vector3d(odom->twist.twist.angular.x, odom->twist.twist.angular.y, odom->twist.twist.angular.z);
so3_controller_.setPosition(cur_pos_);
so3_controller_.setVelocity(cur_vel_);
if (!state_init_)
ROS_INFO("Odom Recived! Ready to TakeOff...");
state_init_ = true;
}
void NetworkControl::imu_callback(const sensor_msgs::Imu &imu)
{
Eigen::Vector3d acc(imu.linear_acceleration.x,
imu.linear_acceleration.y,
imu.linear_acceleration.z);
Eigen::Vector3d acc_world = cur_att_ * acc;
acc_world(2) -= 9.8;
cur_acc_ = acc_world;
// so3_controller_.setAcc(acc_world);
}
void NetworkControl::timerCallback(const ros::TimerEvent &)
{
if (!state_init_ || !ref_valid_)
return;
if (position_cmd_init_ && ctrl_valid_)
return;
mutex_.lock();
Eigen::Vector3d des_pos_temp = des_pos_;
mutex_.unlock();
Eigen::Vector3d att_acc = publishHoverSO3Command(des_pos_temp, des_vel_, des_acc_, des_yaw_, des_yaw_dot_);
if (takeoff_cmd_init_)
{
disturbance_observer_.HGDO_ext_force_ob(last_des_acc_, cur_vel_, dis_acc_);
// ROS_INFO_THROTTLE(1.0, " dis_acc: (%f, %f, %f)", dis_acc_.x(), dis_acc_.y(), dis_acc_.z());
}
last_des_acc_ = att_acc;
if (record_log_)
recordLog(cur_vel_, cur_acc_, att_acc, dis_acc_, cur_yaw_, des_yaw_);
takeoff_cmd_init_ = true;
}
void NetworkControl::takeoff_land_thread(quadrotor_msgs::SetTakeoffLand::Request &req)
{
mutex_.lock();
float takeoff_altitude = req.takeoff_altitude;
des_pos_ = cur_pos_;
des_pos_(2) -= 0.2;
des_yaw_ = cur_yaw_;
mutex_.unlock();
ref_valid_ = true;
if (req.takeoff)
{
std::cout << "takeoff process start" << std::endl;
if (!arm_disarm_vehicle(true))
{
std::cout << "Service failed because cannot Arm!" << std::endl;
return;
}
sleep(1);
double takeoff_vel = 0.8;
double takeoff_ddz = takeoff_vel * control_dt_;
ros::Rate takeoff_loop(1 / control_dt_);
std::cout << "takeoff altitude: " << takeoff_altitude << " m" << std::endl;
std::cout << "takeoff velocity: " << takeoff_vel << " m/s" << std::endl;
ros::Time start_takeoff_task_time = ros::Time::now();
while (ros::ok() && ros::Time::now() - start_takeoff_task_time < ros::Duration(8.0))
{
mutex_.lock();
des_pos_(2) += takeoff_ddz;
mutex_.unlock();
if (des_pos_(2) > takeoff_altitude)
{
ROS_INFO("TakeOff Done! Ready to Flight...");
ctrl_valid_ = true;
break;
}
takeoff_loop.sleep();
}
}
else
{
ctrl_valid_ = false;
double land_vel = -0.4;
double land_ddz = land_vel * control_dt_;
ros::Rate land_loop(1 / control_dt_);
ros::Time start_land_task_time = ros::Time::now();
while (ros::ok() && ros::Time::now() - start_land_task_time < ros::Duration(8.0))
{
mutex_.lock();
des_pos_(2) += land_ddz;
mutex_.unlock();
if (fabs(cur_pos_(2)) < 0.1f && fabs(cur_vel_(2)) < 1.0f)
{
ROS_INFO("detect land: disarm");
arm_disarm_vehicle(false);
break;
}
land_loop.sleep();
}
}
ROS_INFO("take off thread out");
return;
}
bool NetworkControl::arm_disarm_vehicle(bool arm)
{
if (arm)
{
if (!state_init_){
ROS_WARN("State timeout, will not arm!");
return false;
}
ROS_INFO("UAV will be armed!");
if (is_simulation_)
mavros_interface_.set_arm_and_offboard_manually();
else if (mavros_interface_.set_arm_and_offboard())
ROS_INFO("Arm done!");
else{
ROS_ERROR("Arm failure!");
return false;
}
if (record_log_)
initLogRecorder();
}
else
{
ROS_INFO("UAV will be disarmed!");
if (is_simulation_)
mavros_interface_.set_disarm_manually();
else if (mavros_interface_.set_disarm())
ROS_INFO("Disarm done!");
else {
ROS_ERROR("Disarm failure!");
return false;
}
if (record_log_)
logger.close();
}
return true;
}

124
Controller/src/so3_control/src/SO3Control.cpp Executable file
View File

@@ -0,0 +1,124 @@
#include <iostream>
#include <so3_control/SO3Control.h>
#include <ros/ros.h>
SO3Control::SO3Control()
: mass_(0.5)
, g_(9.81)
{
acc_.setZero();
}
void
SO3Control::setMass(const double mass)
{
mass_ = mass;
}
void
SO3Control::setGravity(const double g)
{
g_ = g;
}
void
SO3Control::setPosition(const Eigen::Vector3d& position)
{
pos_ = position;
}
void
SO3Control::setVelocity(const Eigen::Vector3d& velocity)
{
vel_ = velocity;
}
void
SO3Control::calculateControl(const Eigen::Vector3d& des_pos,
const Eigen::Vector3d& des_vel,
const Eigen::Vector3d& des_acc,
const double des_yaw, const double des_yaw_dot,
const Eigen::Vector3d& kx,
const Eigen::Vector3d& kv)
{
// ROS_INFO("Error %lf %lf %lf", (des_pos - pos_).norm(),
// (des_vel - vel_).norm(), (des_acc - acc_).norm());
bool flag_use_pos = !(std::isnan(des_pos(0)) || std::isnan(des_pos(1)) || std::isnan(des_pos(2)));
bool flag_use_vel = !(std::isnan(des_vel(0)) || std::isnan(des_vel(1)) || std::isnan(des_vel(2)));
bool flag_use_acc = !(std::isnan(des_acc(0)) || std::isnan(des_acc(1)) || std::isnan(des_acc(2)));
Eigen::Vector3d totalError(Eigen::Vector3d::Zero());
if ( flag_use_pos ) totalError.noalias() += des_pos - pos_;
if ( flag_use_vel ) totalError.noalias() += des_vel - vel_;
if ( flag_use_acc ) totalError.noalias() += des_acc - acc_;
Eigen::Vector3d ka(fabs(totalError[0]) > 3 ? 0 : (fabs(totalError[0]) * 0.0),
fabs(totalError[1]) > 3 ? 0 : (fabs(totalError[1]) * 0.0),
fabs(totalError[2]) > 3 ? 0 : (fabs(totalError[2]) * 0.0));
// std::cout << des_pos.transpose() << std::endl;
// std::cout << des_vel.transpose() << std::endl;
// std::cout << des_acc.transpose() << std::endl;
// std::cout << des_yaw << std::endl;
// std::cout << pos_.transpose() << std::endl;
// std::cout << vel_.transpose() << std::endl;
// std::cout << acc_.transpose() << std::endl;
force_ = mass_ * g_ * Eigen::Vector3d(0, 0, 1);
if ( flag_use_pos ) force_.noalias() += kx.asDiagonal() * (des_pos - pos_);
if ( flag_use_vel ) force_.noalias() += kv.asDiagonal() * (des_vel - vel_);
if ( flag_use_acc ) force_.noalias() += mass_ * ka.asDiagonal() * (des_acc - acc_) + mass_ * (des_acc);
// Limit control angle to 45 degree
double theta = M_PI / 4;
double c = cos(theta);
Eigen::Vector3d f;
f.noalias() = force_ - mass_ * g_ * Eigen::Vector3d(0, 0, 1);
if (Eigen::Vector3d(0, 0, 1).dot(force_ / force_.norm()) < c)
{
double nf = f.norm();
double A = c * c * nf * nf - f(2) * f(2);
double B = 2 * (c * c - 1) * f(2) * mass_ * g_;
double C = (c * c - 1) * mass_ * mass_ * g_ * g_;
double s = (-B + sqrt(B * B - 4 * A * C)) / (2 * A);
force_.noalias() = s * f + mass_ * g_ * Eigen::Vector3d(0, 0, 1);
}
// Limit control angle to 45 degree
Eigen::Vector3d b1c, b2c, b3c;
Eigen::Vector3d b1d(cos(des_yaw), sin(des_yaw), 0);
if (force_.norm() > 1e-6)
b3c.noalias() = force_.normalized();
else
b3c.noalias() = Eigen::Vector3d(0, 0, 1);
b2c.noalias() = b3c.cross(b1d).normalized();
b1c.noalias() = b2c.cross(b3c).normalized();
Eigen::Matrix3d R;
R << b1c, b2c, b3c;
orientation_ = Eigen::Quaterniond(R);
}
const Eigen::Vector3d&
SO3Control::getComputedForce(void)
{
return force_;
}
const Eigen::Quaterniond&
SO3Control::getComputedOrientation(void)
{
return orientation_;
}
void
SO3Control::setAcc(const Eigen::Vector3d& acc)
{
acc_ = acc;
}

78
Controller/src/so3_control/src/control_example.cpp Normal file
View File

@@ -0,0 +1,78 @@
#include <Eigen/Eigen>
#include <quadrotor_msgs/PositionCommand.h>
#include <ros/ros.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "quad_sim_example");
ros::NodeHandle nh("~");
ros::Publisher cmd_pub = nh.advertise<quadrotor_msgs::PositionCommand>("/position_cmd", 10);
ros::Duration(2.0).sleep();
while (ros::ok())
{
/*** example 1: position control ***/
std::cout << "\033[42m"
<< "Position Control to (2,0,1) meters"
<< "\033[0m" << std::endl;
for (int i = 0; i < 500; i++)
{
quadrotor_msgs::PositionCommand cmd;
cmd.position.x = 2.0;
cmd.position.y = 0.0;
cmd.position.z = 1.0;
cmd_pub.publish(cmd);
ros::Duration(0.01).sleep();
ros::spinOnce();
}
/*** example 2: velocity control ***/
std::cout << "\033[42m"
<< "Velocity Control to (-1,0,0) meters/second"
<< "\033[0m" << std::endl;
for (int i = 0; i < 500; i++)
{
quadrotor_msgs::PositionCommand cmd;
cmd.position.x = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.position.y = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.position.z = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.velocity.x = -1.0;
cmd.velocity.y = 0.0;
cmd.velocity.z = 0.0;
cmd_pub.publish(cmd);
ros::Duration(0.01).sleep();
ros::spinOnce();
}
/*** example 3: accelleration control ***/
std::cout << "\033[42m"
<< "Accelleration Control to (1,0,0) meters/second^2"
<< "\033[0m" << std::endl;
for (int i = 0; i < 500; i++)
{
quadrotor_msgs::PositionCommand cmd;
cmd.position.x = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.position.y = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.position.z = std::numeric_limits<float>::quiet_NaN(); // lower-order commands must be disabled by nan
cmd.velocity.x = std::numeric_limits<float>::quiet_NaN();
cmd.velocity.y = std::numeric_limits<float>::quiet_NaN();
cmd.velocity.z = std::numeric_limits<float>::quiet_NaN();
cmd.acceleration.x = 1.0;
cmd.acceleration.y = 0.0;
cmd.acceleration.z = 0.0;
cmd_pub.publish(cmd);
ros::Duration(0.01).sleep();
ros::spinOnce();
}
}
return 0;
}

11
Controller/src/so3_control/src/network_control_node.cpp Normal file
View File

@@ -0,0 +1,11 @@
#include "so3_control/NetworkControl.h"
#include "ros/ros.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "network_ctrl_node");
ros::NodeHandle node("~");
NetworkControl controller(node);
ros::spin();
return 0;
}

350
Controller/src/so3_control/src/so3_control_nodelet.cpp Executable file
View File

@@ -0,0 +1,350 @@
#include <Eigen/Geometry>
#include <nav_msgs/Odometry.h>
#include <nodelet/nodelet.h>
#include <quadrotor_msgs/Corrections.h>
#include <quadrotor_msgs/PositionCommand.h>
#include <quadrotor_msgs/SO3Command.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <so3_control/SO3Control.h>
#include <std_msgs/Bool.h>
#include <tf/transform_datatypes.h>
#include <string>
#include <iostream>
#include <fstream>
class SO3ControlNodelet : public nodelet::Nodelet
{
public:
SO3ControlNodelet()
: position_cmd_updated_(false)
, position_cmd_init_(false)
, des_yaw_(0)
, des_yaw_dot_(0)
, current_yaw_(0)
, enable_motors_(true)
, // FIXME
use_external_yaw_(false)
{
}
void onInit(void);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
void publishSO3Command(void);
void position_cmd_callback(
const quadrotor_msgs::PositionCommand::ConstPtr& cmd);
void odom_callback(const nav_msgs::Odometry::ConstPtr& odom);
void enable_motors_callback(const std_msgs::Bool::ConstPtr& msg);
void corrections_callback(const quadrotor_msgs::Corrections::ConstPtr& msg);
void imu_callback(const sensor_msgs::Imu& imu);
void initLogRecorder();
void recordLog();
SO3Control controller_;
ros::Publisher so3_command_pub_;
ros::Subscriber odom_sub_;
ros::Subscriber position_cmd_sub_;
ros::Subscriber enable_motors_sub_;
ros::Subscriber corrections_sub_;
ros::Subscriber imu_sub_;
bool position_cmd_updated_, position_cmd_init_;
std::string frame_id_;
bool record_log_{false};
bool cur_acc_init_{false}, cur_odom_init_{false};
Eigen::Vector3d des_pos_, des_vel_, des_acc_, kx_, kv_;
Eigen::Vector3d cur_pos_, cur_vel_, cur_acc_;
double des_yaw_, des_yaw_dot_;
double current_yaw_;
bool enable_motors_;
bool use_external_yaw_;
double kR_[3], kOm_[3], corrections_[3];
double init_x_, init_y_, init_z_;
std::ofstream logger;
std::string logger_file_name;
};
void
SO3ControlNodelet::publishSO3Command(void)
{
controller_.calculateControl(des_pos_, des_vel_, des_acc_, des_yaw_,
des_yaw_dot_, kx_, kv_);
const Eigen::Vector3d& force = controller_.getComputedForce();
const Eigen::Quaterniond& orientation = controller_.getComputedOrientation();
quadrotor_msgs::SO3Command::Ptr so3_command(
new quadrotor_msgs::SO3Command); //! @note memory leak?
so3_command->header.stamp = ros::Time::now();
so3_command->header.frame_id = frame_id_;
so3_command->force.x = force(0);
so3_command->force.y = force(1);
so3_command->force.z = force(2);
so3_command->orientation.x = orientation.x();
so3_command->orientation.y = orientation.y();
so3_command->orientation.z = orientation.z();
so3_command->orientation.w = orientation.w();
for (int i = 0; i < 3; i++)
{
so3_command->kR[i] = kR_[i];
so3_command->kOm[i] = kOm_[i];
}
so3_command->aux.current_yaw = current_yaw_;
so3_command->aux.kf_correction = corrections_[0];
so3_command->aux.angle_corrections[0] = corrections_[1];
so3_command->aux.angle_corrections[1] = corrections_[2];
so3_command->aux.enable_motors = enable_motors_;
so3_command->aux.use_external_yaw = use_external_yaw_;
so3_command_pub_.publish(so3_command);
}
void
SO3ControlNodelet::position_cmd_callback(
const quadrotor_msgs::PositionCommand::ConstPtr& cmd)
{
des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
des_acc_ = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y,
cmd->acceleration.z);
if ( cmd->kx[0] > 1e-5 || cmd->kx[1] > 1e-5 || cmd->kx[2] > 1e-5 )
{
kx_ = Eigen::Vector3d(cmd->kx[0], cmd->kx[1], cmd->kx[2]);
}
if ( cmd->kv[0] > 1e-5 || cmd->kv[1] > 1e-5 || cmd->kv[2] > 1e-5 )
{
kv_ = Eigen::Vector3d(cmd->kv[0], cmd->kv[1], cmd->kv[2]);
}
des_yaw_ = cmd->yaw;
des_yaw_dot_ = cmd->yaw_dot;
position_cmd_updated_ = true;
position_cmd_init_ = true;
publishSO3Command();
if (record_log_ && cur_acc_init_ && cur_odom_init_)
{
recordLog();
}
}
void
SO3ControlNodelet::odom_callback(const nav_msgs::Odometry::ConstPtr& odom)
{
const Eigen::Vector3d position(odom->pose.pose.position.x,
odom->pose.pose.position.y,
odom->pose.pose.position.z);
const Eigen::Vector3d velocity(odom->twist.twist.linear.x,
odom->twist.twist.linear.y,
odom->twist.twist.linear.z);
cur_odom_init_ = true;
current_yaw_ = tf::getYaw(odom->pose.pose.orientation);
cur_pos_ = position;
cur_vel_ = velocity;
controller_.setPosition(position);
controller_.setVelocity(velocity);
if (position_cmd_init_)
{
// We set position_cmd_updated_ = false and expect that the
// position_cmd_callback would set it to true since typically a position_cmd
// message would follow an odom message. If not, the position_cmd_callback
// hasn't been called and we publish the so3 command ourselves
// TODO: Fallback to hover if position_cmd hasn't been received for some
// time
if (!position_cmd_updated_)
publishSO3Command();
position_cmd_updated_ = false;
}
else if ( init_z_ > -9999.0 )
{
des_pos_ = Eigen::Vector3d(init_x_, init_y_, init_z_);
des_vel_ = Eigen::Vector3d(0,0,0);
des_acc_ = Eigen::Vector3d(0,0,0);
publishSO3Command();
}
}
void
SO3ControlNodelet::enable_motors_callback(const std_msgs::Bool::ConstPtr& msg)
{
if (msg->data)
ROS_INFO("Enabling motors");
else
ROS_INFO("Disabling motors");
enable_motors_ = msg->data;
}
void
SO3ControlNodelet::corrections_callback(
const quadrotor_msgs::Corrections::ConstPtr& msg)
{
corrections_[0] = msg->kf_correction;
corrections_[1] = msg->angle_corrections[0];
corrections_[2] = msg->angle_corrections[1];
}
void
SO3ControlNodelet::imu_callback(const sensor_msgs::Imu& imu)
{
cur_acc_init_ = true;
const Eigen::Vector3d acc(imu.linear_acceleration.x,
imu.linear_acceleration.y,
imu.linear_acceleration.z);
cur_acc_ = acc;
controller_.setAcc(acc);
}
void
SO3ControlNodelet::onInit(void)
{
ros::NodeHandle n(getPrivateNodeHandle());
std::string quadrotor_name;
n.param("quadrotor_name", quadrotor_name, std::string("quadrotor"));
frame_id_ = "/" + quadrotor_name;
double mass;
n.param("mass", mass, 0.5);
controller_.setMass(mass);
n.param("record_log", record_log_, false);
n.param("PID_logger_file_name", logger_file_name, std::string("/home/lu/"));
n.param("use_external_yaw", use_external_yaw_, true);
n.param("gains/rot/x", kR_[0], 1.5);
n.param("gains/rot/y", kR_[1], 1.5);
n.param("gains/rot/z", kR_[2], 1.0);
n.param("gains/ang/x", kOm_[0], 0.13);
n.param("gains/ang/y", kOm_[1], 0.13);
n.param("gains/ang/z", kOm_[2], 0.1);
n.param("gains/kx/x", kx_[0], 5.7);
n.param("gains/kx/y", kx_[1], 5.7);
n.param("gains/kx/z", kx_[2], 6.2);
n.param("gains/kv/x", kv_[0], 3.4);
n.param("gains/kv/y", kv_[1], 3.4);
n.param("gains/kv/z", kv_[2], 4.0);
n.param("corrections/z", corrections_[0], 0.0);
n.param("corrections/r", corrections_[1], 0.0);
n.param("corrections/p", corrections_[2], 0.0);
n.param("so3_control/init_state_x", init_x_, 0.0);
n.param("so3_control/init_state_y", init_y_, 0.0);
n.param("so3_control/init_state_z", init_z_, -10000.0);
so3_command_pub_ = n.advertise<quadrotor_msgs::SO3Command>("so3_cmd", 10);
odom_sub_ = n.subscribe("odom", 10, &SO3ControlNodelet::odom_callback, this,
ros::TransportHints().tcpNoDelay());
position_cmd_sub_ =
n.subscribe("position_cmd", 10, &SO3ControlNodelet::position_cmd_callback,
this, ros::TransportHints().tcpNoDelay());
enable_motors_sub_ =
n.subscribe("motors", 2, &SO3ControlNodelet::enable_motors_callback, this,
ros::TransportHints().tcpNoDelay());
corrections_sub_ =
n.subscribe("corrections", 10, &SO3ControlNodelet::corrections_callback,
this, ros::TransportHints().tcpNoDelay());
imu_sub_ = n.subscribe("imu", 10, &SO3ControlNodelet::imu_callback, this,
ros::TransportHints().tcpNoDelay());
if (record_log_)
initLogRecorder();
}
void SO3ControlNodelet::initLogRecorder()
{
std::cout << "logger_file_name: " << logger_file_name << std::endl;
std::string temp_file_name = logger_file_name + "PID_logger_";
time_t timep;
timep = time(0);
char tmp[64];
strftime(tmp, sizeof(tmp), "%Y_%m_%d_%H_%M_%S", localtime(&timep));
temp_file_name += tmp;
temp_file_name += ".csv";
if (logger.is_open())
{
logger.close();
}
logger.open(temp_file_name.c_str(), std::ios::out);
std::cout << "PID logger: " << temp_file_name << std::endl;
if (!logger.is_open())
{
std::cout << "cannot open the logger." << std::endl;
}
else
{
logger << "timestamp" << ',';
logger << "cur_x" << ',';
logger << "cur_y" << ',';
logger << "cur_z" << ',';
logger << "cur_vx" << ',';
logger << "cur_vy" << ',';
logger << "cur_vz" << ',';
logger << "cur_ax" << ',';
logger << "cur_ay" << ',';
logger << "cur_az" << ',';
logger << "not_use" << ',';
logger << "not_use" << ',';
logger << "cur_yaw" << ',';
logger << "des_yaw" << ',';
logger << "des_pos_x" << ',';
logger << "des_pos_y" << ',';
logger << "des_pos_z" << ',';
logger << "des_vel_x" << ',';
logger << "des_vel_y" << ',';
logger << "des_vel_z" << ',';
logger << "des_acc_x" << ',';
logger << "des_acc_y" << ',';
logger << "des_acc_z" << std::endl;
}
}
void SO3ControlNodelet::recordLog(){
if (logger.is_open())
{
logger << ros::Time::now().toNSec() << ',';
logger << cur_pos_(0) << ',';
logger << cur_pos_(1) << ',';
logger << cur_pos_(2) << ',';
logger << cur_vel_(0) << ',';
logger << cur_vel_(1) << ',';
logger << cur_vel_(2) << ',';
logger << cur_acc_(0) << ',';
logger << cur_acc_(1) << ',';
logger << cur_acc_(2) << ',';
logger << 0.0 << ',';
logger << 0.0 << ',';
logger << current_yaw_ << ',';
logger << des_yaw_ << ',';
logger << des_pos_(0) << ',';
logger << des_pos_(1) << ',';
logger << des_pos_(2) << ',';
logger << des_vel_(0) << ',';
logger << des_vel_(1) << ',';
logger << des_vel_(2) << ',';
logger << des_acc_(0) << ',';
logger << des_acc_(1) << ',';
logger << des_acc_(2) << std::endl;
}
}
#include <pluginlib/class_list_macros.h>
//PLUGINLIB_DECLARE_CLASS(so3_control, SO3ControlNodelet, SO3ControlNodelet,
// nodelet::Nodelet);
PLUGINLIB_EXPORT_CLASS(SO3ControlNodelet, nodelet::Nodelet);

53
Controller/src/so3_quadrotor_simulator/CMakeLists.txt Executable file
View File

@@ -0,0 +1,53 @@
cmake_minimum_required(VERSION 2.8.3)
project(so3_quadrotor_simulator)
add_compile_options(-std=c++11)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
find_package(catkin REQUIRED COMPONENTS
roscpp
quadrotor_msgs
uav_utils
cmake_utils
tf2_ros
roslib
)
###########
## Build ##
###########
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include/ode)
##
find_package(Armadillo REQUIRED)
include_directories(${ARMADILLO_INCLUDE_DIRS})
catkin_package(
INCLUDE_DIRS include
# LIBRARIES irobot_msgs
CATKIN_DEPENDS roscpp quadrotor_msgs uav_utils
DEPENDS Eigen3 system_lib
)
include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_library(quadrotor_dynamics src/dynamics/Quadrotor.cpp)
## Declare a cpp executable
#add_executable(odom_visualization src/odom_visualization.cpp)
add_executable(quadrotor_simulator_so3
src/quadrotor_simulator_so3.cpp)
target_link_libraries(quadrotor_simulator_so3
${catkin_LIBRARIES}
quadrotor_dynamics
)

212
Controller/src/so3_quadrotor_simulator/config/rviz.rviz Normal file
View File

@@ -0,0 +1,212 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /UAV1
Splitter Ratio: 0.5
Tree Height: 703
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
- /3D Nav Goal1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 138; 138; 138
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /odom_visualization/robot
Name: UAV
Namespaces:
mesh: true
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /odom_visualization_1/path
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.100000001
Class: rviz/Pose
Color: 0; 255; 0
Enabled: true
Head Length: 0.300000012
Head Radius: 0.100000001
Name: Pose
Shaft Length: 1
Shaft Radius: 0.0500000007
Shape: Arrow
Topic: /odom_visualization_1/pose
Unreliable: false
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /odom_visualization_1/velocity
Name: Velocity
Namespaces:
{}
Queue Size: 100
Value: true
- Alpha: 1
Buffer Length: 1
Class: rviz/Path
Color: 255; 85; 255
Enabled: true
Head Diameter: 0.300000012
Head Length: 0.200000003
Length: 0.300000012
Line Style: Lines
Line Width: 0.0299999993
Name: Path
Offset:
X: 0
Y: 0
Z: 0
Pose Color: 255; 85; 255
Pose Style: None
Radius: 0.0299999993
Shaft Diameter: 0.100000001
Shaft Length: 0.100000001
Topic: /CircleNode_1/path
Unreliable: false
Value: true
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.100000001
Reference Frame: <Fixed Frame>
Value: true
Enabled: true
Global Options:
Background Color: 89; 89; 89
Default Light: true
Fixed Frame: world
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
- Class: rviz_plugins/GameLikeInput
RangeX_max: 4
RangeX_min: -4
RangeY_max: 2.5
RangeY_min: -2.5
RangeZ_max: 1.70000005
RangeZ_min: 0.800000012
TopicPoint: point_list
TopicSelect: select_list
TopicSwarm: swarm
- Class: rviz_plugins/Goal3DTool
Topic: goal
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 14.550209
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0.054024078
Y: -0.301004827
Z: 2.69711113
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.759796858
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 4.71485233
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 984
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016a0000034efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000280000034e000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000037afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007300000000280000037a000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005400000003efc0100000002fb0000000800540069006d00650100000000000005400000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003d00000034e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1344
X: 565
Y: 24

10597
Controller/src/so3_quadrotor_simulator/config/uav.dae Executable file
View File

File diff suppressed because one or more lines are too long

9
Controller/src/so3_quadrotor_simulator/include/ode/CHANGELOG Executable file
View File

@@ -0,0 +1,9 @@
odeint 2.1
* versioning system
* generation functions
* bugfixing
odeint 2.2 (still running)
* removing same_size and resize from state_wrapper into separate functions

44
Controller/src/so3_quadrotor_simulator/include/ode/Jamroot Executable file
View File

@@ -0,0 +1,44 @@
# Copyright 2009 Karsten Ahnert and Mario Mulansky.
# Distributed under the Boost Software License, Version 1.0. (See
# accompanying file LICENSE_1_0.txt or copy at
# http://www.boost.org/LICENSE_1_0.txt)
import os ;
import modules ;
import path ;
path-constant BOOST_ROOT : [ os.environ BOOST_ROOT ] ;
project
: requirements
<include>$(BOOST_ROOT)
<toolset>clang:<cxxflags>-Wno-unused-variable
;
# tests, regression tests and examples
build-project libs/numeric/odeint/test ;
build-project libs/numeric/odeint/examples ;
# additional tests with external libraries :
# build-project libs/numeric/odeint/test_external/gmp ;
# build-project libs/numeric/odeint/test_external/mkl ;
# build-project libs/numeric/odeint/test_external/gsl ;
# docs:
# build-project libs/numeric/odeint/doc ;
###### The following is copied from another sandbox project #####
###### to get the quickbook and boostbook working ... #####
# local boost-root = [ modules.peek : BOOST_ROOT ] ;
# local explore-header-include = $(top)/../.. ;
# use-project /boost/regex : $(boost-root)/libs/regex/build ;
##################################################################

1
Controller/src/so3_quadrotor_simulator/include/ode/README Executable file
View File

@@ -0,0 +1 @@
odeint is a highly flexible library for solving ordinary differential equations.

75
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint.hpp Executable file
View File

@@ -0,0 +1,75 @@
/*
[auto_generated]
boost/numeric/odeint.hpp
[begin_description]
Forward include for odeint. Includes nearly everything.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_HPP_INCLUDED
#include <boost/numeric/odeint/version.hpp>
#include <boost/numeric/odeint/config.hpp>
// start with ublas wrapper because we need its specializations before including state_wrapper.hpp
#include <boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <boost/numeric/odeint/stepper/euler.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/bulirsch_stoer.hpp>
#ifndef __CUDACC__
/* Bulirsch Stoer with Dense Output does not compile with nvcc
* because of the binomial library used there which relies on unsupported SSE functions
*/
#include <boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp>
#endif
#include <boost/numeric/odeint/stepper/symplectic_euler.hpp>
#include <boost/numeric/odeint/stepper/symplectic_rkn_sb3a_mclachlan.hpp>
#include <boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp>
#include <boost/numeric/odeint/stepper/implicit_euler.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp>
/*
* Including this algebra slows down the compilation time
*/
// #include <boost/numeric/odeint/algebra/fusion_algebra.hpp>
#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>
#include <boost/numeric/odeint/integrate/integrate.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/integrate/integrate_n_steps.hpp>
#include <boost/numeric/odeint/integrate/integrate_times.hpp>
#include <boost/numeric/odeint/integrate/observer_collection.hpp>
#include <boost/numeric/odeint/stepper/generation.hpp>
#endif // BOOST_NUMERIC_ODEINT_HPP_INCLUDED

265
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/array_algebra.hpp Executable file
View File

@@ -0,0 +1,265 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/array_algebra.hpp
[begin_description]
Algebra for boost::array. Highly specialized for odeint. Const arguments are introduce to work with odeint.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED
#include <boost/array.hpp>
namespace boost {
namespace numeric {
namespace odeint {
struct array_algebra
{
template< typename T , size_t dim , class Op >
static void for_each1( boost::array< T , dim > &s1 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] );
}
template< typename T1 , typename T2 , size_t dim , class Op >
static void for_each2( boost::array< T1 , dim > &s1 ,
const boost::array< T2 , dim > &s2 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] );
}
template< typename T , size_t dim , class Op >
static void for_each3( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] );
}
/* different const signature - required for the scale_sum_swap2 operation */
template< typename T , size_t dim , class Op >
static void for_each3( boost::array< T , dim > &s1 ,
boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] );
}
template< typename T , size_t dim , class Op >
static void for_each4( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] );
}
template< typename T , size_t dim , class Op >
static void for_each5( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] );
}
template< typename T , size_t dim , class Op >
static void for_each6( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] );
}
template< typename T , size_t dim , class Op >
static void for_each7( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] );
}
template< typename T , size_t dim , class Op >
static void for_each8( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] );
}
template< typename T , size_t dim , class Op >
static void for_each9( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] );
}
template< typename T , size_t dim , class Op >
static void for_each10( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] );
}
template< typename T , size_t dim , class Op >
static void for_each11( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 ,
const boost::array< T , dim > &s11 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] , s11[i] );
}
template< typename T , size_t dim , class Op >
static void for_each12( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 ,
const boost::array< T , dim > &s11 ,
const boost::array< T , dim > &s12 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] , s11[i] , s12[i] );
}
template< typename T , size_t dim , class Op >
static void for_each13( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 ,
const boost::array< T , dim > &s11 ,
const boost::array< T , dim > &s12 ,
const boost::array< T , dim > &s13 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] , s11[i] , s12[i] , s13[i] );
}
template< typename T , size_t dim , class Op >
static void for_each14( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 ,
const boost::array< T , dim > &s11 ,
const boost::array< T , dim > &s12 ,
const boost::array< T , dim > &s13 ,
const boost::array< T , dim > &s14 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] , s11[i] , s12[i] , s13[i] , s14[i] );
}
template< typename T , size_t dim , class Op >
static void for_each15( boost::array< T , dim > &s1 ,
const boost::array< T , dim > &s2 ,
const boost::array< T , dim > &s3 ,
const boost::array< T , dim > &s4 ,
const boost::array< T , dim > &s5 ,
const boost::array< T , dim > &s6 ,
const boost::array< T , dim > &s7 ,
const boost::array< T , dim > &s8 ,
const boost::array< T , dim > &s9 ,
const boost::array< T , dim > &s10 ,
const boost::array< T , dim > &s11 ,
const boost::array< T , dim > &s12 ,
const boost::array< T , dim > &s13 ,
const boost::array< T , dim > &s14 ,
const boost::array< T , dim > &s15 , Op op )
{
for( size_t i=0 ; i<dim ; ++i )
op( s1[i] , s2[i] , s3[i] , s4[i] , s5[i] , s6[i] , s7[i] , s8[i] , s9[i] , s10[i] , s11[i] , s12[i] , s13[i] , s14[i] , s15[i] );
}
template< class Value , class T , size_t dim , class Red >
static Value reduce( const boost::array< T , dim > &s , Red red , Value init)
{
for( size_t i=0 ; i<dim ; ++i )
init = red( init , s[i] );
return init;
}
};
}
}
}
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_ARRAY_ALGEBRA_HPP_INCLUDED

602
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/default_operations.hpp Executable file
View File

@@ -0,0 +1,602 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/default_operations.hpp
[begin_description]
Default operations. They work with the default numerical types, like float, double, complex< double> ...
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED
#include <algorithm>
#include <boost/config.hpp>
#include <boost/array.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* Notes:
*
* * the results structs are needed in order to work with fusion_algebra
*/
struct default_operations
{
template< class Fac1 = double >
struct scale
{
const Fac1 m_alpha1;
scale( Fac1 alpha1 ) : m_alpha1( alpha1 ) { }
template< class T1 >
void operator()( T1 &t1 ) const
{
t1 *= m_alpha1;
}
typedef void result_type;
};
template< class Fac1 = double >
struct scale_sum1
{
const Fac1 m_alpha1;
scale_sum1( Fac1 alpha1 ) : m_alpha1( alpha1 ) { }
template< class T1 , class T2 >
void operator()( T1 &t1 , const T2 &t2 ) const
{
t1 = m_alpha1 * t2;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 >
struct scale_sum2
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }
template< class T1 , class T2 , class T3 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 >
struct scale_sum3
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { }
template< class T1 , class T2 , class T3 , class T4 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 >
struct scale_sum4
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 >
struct scale_sum5
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 >
struct scale_sum6
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
scale_sum6( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 , Fac6 alpha6 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ){ }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ,const T7 &t7) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 >
struct scale_sum7
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
scale_sum7( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 >
struct scale_sum8
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
scale_sum8( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 >
struct scale_sum9
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
scale_sum9( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 >
struct scale_sum10
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
const Fac10 m_alpha10;
scale_sum10( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 , Fac10 alpha10 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 >
struct scale_sum11
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
const Fac10 m_alpha10;
const Fac11 m_alpha11;
scale_sum11( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ,
Fac10 alpha10 , Fac11 alpha11 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 >
struct scale_sum12
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
const Fac10 m_alpha10;
const Fac11 m_alpha11;
const Fac12 m_alpha12;
scale_sum12( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ,
Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 >
struct scale_sum13
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
const Fac10 m_alpha10;
const Fac11 m_alpha11;
const Fac12 m_alpha12;
const Fac13 m_alpha13;
scale_sum13( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ,
Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 , class Fac8 = Fac7 , class Fac9 = Fac8 , class Fac10 = Fac9 , class Fac11 = Fac10 , class Fac12 = Fac11 , class Fac13 = Fac12 , class Fac14 = Fac13 >
struct scale_sum14
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
const Fac8 m_alpha8;
const Fac9 m_alpha9;
const Fac10 m_alpha10;
const Fac11 m_alpha11;
const Fac12 m_alpha12;
const Fac13 m_alpha13;
const Fac14 m_alpha14;
scale_sum14( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 ,
Fac5 alpha5 , Fac6 alpha6 , Fac7 alpha7 , Fac8 alpha8 , Fac9 alpha9 ,
Fac10 alpha10 , Fac11 alpha11 , Fac12 alpha12 , Fac13 alpha13 , Fac14 alpha14 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) , m_alpha8( alpha8 ) , m_alpha9( alpha9 ) , m_alpha10( alpha10 ) , m_alpha11( alpha11 ) , m_alpha12( alpha12 ) , m_alpha13( alpha13 ) , m_alpha14( alpha14 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 , class T7 , class T8 , class T9 , class T10 , class T11 , class T12 , class T13 , class T14 , class T15 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 , const T7 &t7 , const T8 &t8 , const T9 &t9 , const T10 &t10 , const T11 &t11 , const T12 &t12 , const T13 &t13 , const T14 &t14 , const T15 &t15 ) const
{
t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6 + m_alpha6 * t7 + m_alpha7 * t8 + m_alpha8 * t9 + m_alpha9 * t10 + m_alpha10 * t11 + m_alpha11 * t12 + m_alpha12 * t13 + m_alpha13 * t14 + m_alpha14 * t15;
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 >
struct scale_sum_swap2
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum_swap2( Fac1 alpha1 , Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }
template< class T1 , class T2 , class T3 >
void operator()( T1 &t1 , T2 &t2 , const T3 &t3) const
{
const T1 tmp( t1 );
t1 = m_alpha1 * t2 + m_alpha2 * t3;
t2 = tmp;
}
typedef void result_type;
};
/*
* for usage in for_each2
*
* Works with boost::units by eliminating the unit
*/
template< class Fac1 = double >
struct rel_error
{
const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt;
rel_error( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { }
template< class T1 , class T2 , class T3 >
void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const
{
using std::abs;
set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( t1 ) ) + m_a_dxdt * abs( get_unit_value( t2 ) ) ) ) );
}
typedef void result_type;
};
/*
* for usage in for_each3
*
* used in the controller for the rosenbrock4 method
*
* Works with boost::units by eliminating the unit
*/
template< class Fac1 = double >
struct default_rel_error
{
const Fac1 m_eps_abs , m_eps_rel ;
default_rel_error( Fac1 eps_abs , Fac1 eps_rel )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) { }
/*
* xerr = xerr / ( eps_abs + eps_rel * max( x , x_old ) )
*/
template< class T1 , class T2 , class T3 >
void operator()( T3 &t3 , const T1 &t1 , const T2 &t2 ) const
{
BOOST_USING_STD_MAX();
using std::abs;
Fac1 x1 = abs( get_unit_value( t1 ) ) , x2 = abs( get_unit_value( t2 ) );
set_unit_value( t3 , abs( get_unit_value( t3 ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( x1 , x2 ) ) );
}
typedef void result_type;
};
/*
* for usage in reduce
*/
template< class Value >
struct maximum
{
template< class Fac1 , class Fac2 >
Value operator()( Fac1 t1 , const Fac2 t2 ) const
{
using std::abs;
Value a1 = abs( get_unit_value( t1 ) ) , a2 = abs( get_unit_value( t2 ) );
return ( a1 < a2 ) ? a2 : a1 ;
}
typedef Value result_type;
};
template< class Fac1 = double >
struct rel_error_max
{
const Fac1 m_eps_abs , m_eps_rel;
rel_error_max( Fac1 eps_abs , Fac1 eps_rel )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel )
{ }
template< class Res , class T1 , class T2 , class T3 >
Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err )
{
BOOST_USING_STD_MAX();
using std::abs;
Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) );
return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp );
}
};
template< class Fac1 = double >
struct rel_error_max2
{
const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt;
rel_error_max2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt )
{ }
template< class Res , class T1 , class T2 , class T3 , class T4 >
Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err )
{
BOOST_USING_STD_MAX();
using std::abs;
Res tmp = abs( get_unit_value( x_err ) ) /
( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) );
return max BOOST_PREVENT_MACRO_SUBSTITUTION ( r , tmp );
}
};
template< class Fac1 = double >
struct rel_error_l2
{
const Fac1 m_eps_abs , m_eps_rel;
rel_error_l2( Fac1 eps_abs , Fac1 eps_rel )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel )
{ }
template< class Res , class T1 , class T2 , class T3 >
Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &x_err )
{
BOOST_USING_STD_MAX();
using std::abs;
Res tmp = abs( get_unit_value( x_err ) ) / ( m_eps_abs + m_eps_rel * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( x_old ) , abs( x ) ) );
return r + tmp * tmp;
}
};
template< class Fac1 = double >
struct rel_error_l2_2
{
const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt;
rel_error_l2_2( Fac1 eps_abs , Fac1 eps_rel , Fac1 a_x , Fac1 a_dxdt )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt )
{ }
template< class Res , class T1 , class T2 , class T3 , class T4 >
Res operator()( Res r , const T1 &x_old , const T2 &x , const T3 &dxdt_old , const T4 &x_err )
{
using std::abs;
Res tmp = abs( get_unit_value( x_err ) ) /
( m_eps_abs + m_eps_rel * ( m_a_x * abs( get_unit_value( x_old ) ) + m_a_dxdt * abs( get_unit_value( dxdt_old ) ) ) );
return r + tmp * tmp;
}
};
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DEFAULT_OPERATIONS_HPP_INCLUDED

165
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/for_each.hpp Executable file
View File

@@ -0,0 +1,165 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/detail/for_each.hpp
[begin_description]
Default for_each implementations.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class Iterator1 , class Operation >
inline void for_each1( Iterator1 first1 , Iterator1 last1 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ );
}
template< class Iterator1 , class Iterator2 , class Operation >
inline void for_each2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Operation >
inline void for_each3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Operation >
inline void for_each4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3, Iterator4 first4, Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Operation >
inline void for_each5( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Operation >
inline void for_each6( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Operation >
inline void for_each7( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Operation >
inline void for_each8( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Operation >
inline void for_each9( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Operation >
inline void for_each10( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Operation >
inline void for_each11( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Operation >
inline void for_each12( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Operation >
inline void for_each13( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Operation >
inline void for_each14( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 ,
Iterator14 first14 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ );
}
template< class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Iterator5 , class Iterator6 , class Iterator7 , class Iterator8 , class Iterator9 , class Iterator10 , class Iterator11 , class Iterator12 , class Iterator13 , class Iterator14 , class Iterator15 , class Operation >
inline void for_each15( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3,
Iterator4 first4, Iterator5 first5, Iterator6 first6 , Iterator7 first7 , Iterator8 first8 ,
Iterator9 first9 , Iterator10 first10 , Iterator11 first11 , Iterator12 first12 , Iterator13 first13 ,
Iterator14 first14 , Iterator15 first15 , Operation op )
{
for( ; first1 != last1 ; )
op( *first1++ , *first2++ , *first3++ , *first4++ , *first5++ , *first6++ , *first7++ , *first8++ , *first9++ , *first10++ , *first11++ , *first12++ , *first13++ , *first14++ , *first15++ );
}
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_FOR_EACH_HPP_INCLUDED

43
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/macros.hpp Executable file
View File

@@ -0,0 +1,43 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/detail/macros.hpp
[begin_description]
Some macros for type checking.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED
//type traits aren't working with nvcc
#ifndef __CUDACC__
#include <boost/type_traits.hpp>
#include <boost/static_assert.hpp>
#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 ) \
BOOST_STATIC_ASSERT(( boost::is_same< typename boost::remove_const< Type1 >::type , Type2 >::value ))
#else
//empty macro for nvcc
#define BOOST_ODEINT_CHECK_CONTAINER_TYPE( Type1 , Type2 )
#endif // __CUDACC__
/*
#define BOOST_ODEINT_CHECK_OPERATION_ARITY( Operation , Arity ) \
BOOST_STATIC_ASSERT(( boost::function_traits< Operation >::arity == Arity ))
*/
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_MACROS_HPP_INCLUDED

67
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/detail/reduce.hpp Executable file
View File

@@ -0,0 +1,67 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/detail/reduce.hpp
[begin_description]
Default reduce implementation.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class ValueType , class Iterator1 , class Reduction >
inline ValueType reduce( Iterator1 first1 , Iterator1 last1 , Reduction red, ValueType init)
{
for( ; first1 != last1 ; )
init = red( init , *first1++ );
return init;
}
template< class ValueType , class Iterator1 , class Iterator2 , class Reduction >
inline ValueType reduce2( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Reduction red, ValueType init)
{
for( ; first1 != last1 ; )
init = red( init , *first1++ , *first2++ );
return init;
}
template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Reduction >
inline ValueType reduce3( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Reduction red, ValueType init)
{
for( ; first1 != last1 ; )
init = red( init , *first1++ , *first2++ , *first3++ );
return init;
}
template< class ValueType , class Iterator1 , class Iterator2 , class Iterator3 , class Iterator4 , class Reduction >
inline ValueType reduce4( Iterator1 first1 , Iterator1 last1 , Iterator2 first2 , Iterator3 first3 , Iterator4 first4 , Reduction red, ValueType init)
{
for( ; first1 != last1 ; )
init = red( init , *first1++ , *first2++ , *first3++ , *first4++ );
return init;
}
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_DETAIL_REDUCE_HPP_INCLUDED

186
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/fusion_algebra.hpp Executable file
View File

@@ -0,0 +1,186 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/fusion_algebra.hpp
[begin_description]
Algebra for boost::fusion sequences.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED
#include <boost/fusion/container/vector.hpp>
#include <boost/fusion/algorithm/iteration/for_each.hpp>
#include <boost/fusion/view/zip_view.hpp>
#include <boost/fusion/functional/generation/make_fused.hpp>
#include <boost/fusion/algorithm/iteration/accumulate.hpp>
namespace boost {
namespace numeric {
namespace odeint {
struct fusion_algebra
{
template< class S1 , class Op >
static void for_each1( S1 &s1 , Op op )
{
boost::fusion::for_each( s1 , op );
};
template< class S1 , class S2 , class Op >
static void for_each2( S1 &s1 , S2 &s2 , Op op )
{
typedef boost::fusion::vector< S1& , S2& > Sequences;
Sequences sequences( s1 , s2 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class Op >
static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op )
{
typedef boost::fusion::vector< S1& , S2& , S3& > Sequences;
Sequences sequences( s1 , s2 , s3 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class Op >
static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op )
{
typedef boost::fusion::vector< S1& , S2& , S3& , S4& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op >
static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op )
{
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op >
static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op )
{
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class Op >
static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op )
{
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class Op >
static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 8 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class Op >
static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 9 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class Op >
static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 10 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class Op >
static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 11 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 11 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op >
static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 12 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 12 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op >
static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 13 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 13 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op >
static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 14 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 14 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op >
static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op )
{
BOOST_STATIC_ASSERT_MSG( BOOST_FUSION_INVOKE_MAX_ARITY >= 15 , "Macro Parameter BOOST_FUSION_INVOKE_MAX_ARITY to small!" );
BOOST_STATIC_ASSERT_MSG( BOOST_RESULT_OF_NUM_ARGS >= 15 , "Macro Parameter BOOST_RESULT_OF_NUM_ARGS to small!" );
typedef boost::fusion::vector< S1& , S2& , S3& , S4& , S5& , S6& , S7& , S8& , S9& , S10& , S11& , S12& , S13& , S14& , S15& > Sequences;
Sequences sequences( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 );
boost::fusion::for_each( boost::fusion::zip_view< Sequences >( sequences ) , boost::fusion::make_fused( op ) );
}
template< class Value , class S , class Reduction >
static Value reduce( const S &s , Reduction red , Value init)
{
return boost::fusion::accumulate( s , init , red );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_FUSION_ALGEBRA_HPP_INCLUDED

159
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/range_algebra.hpp Executable file
View File

@@ -0,0 +1,159 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/range_algebra.hpp
[begin_description]
Default algebra, which works with the most state types, like vector< double >, boost::array< double >, boost::range.
Internally is uses boost::range to obtain the begin and end iterator of the according sequence.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED
#include <boost/range.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/numeric/odeint/algebra/detail/macros.hpp>
#include <boost/numeric/odeint/algebra/detail/for_each.hpp>
#include <boost/numeric/odeint/algebra/detail/reduce.hpp>
namespace boost {
namespace numeric {
namespace odeint {
struct range_algebra
{
template< class S1 , class Op >
static void for_each1( S1 &s1 , Op op )
{
detail::for_each1( boost::begin( s1 ) , boost::end( s1 ) ,
op );
}
template< class S1 , class S2 , class Op >
static void for_each2( S1 &s1 , S2 &s2 , Op op )
{
detail::for_each2( boost::begin( s1 ) , boost::end( s1 ) ,
boost::begin( s2 ) , op );
}
template< class S1 , class S2 , class S3 , class Op >
static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op )
{
detail::for_each3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class Op >
static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op )
{
detail::for_each4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op >
static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op )
{
detail::for_each5( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op >
static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op )
{
detail::for_each6( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op >
static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op )
{
detail::for_each7( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op >
static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op )
{
detail::for_each8( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op >
static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op )
{
detail::for_each9( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op >
static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op )
{
detail::for_each10( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op >
static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op )
{
detail::for_each11( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op >
static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op )
{
detail::for_each12( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op >
static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op )
{
detail::for_each13( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op >
static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op )
{
detail::for_each14( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , op );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op >
static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op )
{
detail::for_each15( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , boost::begin( s5 ) , boost::begin( s6 ) , boost::begin( s7 ) , boost::begin( s8 ) , boost::begin( s9 ) , boost::begin( s10 ) , boost::begin( s11 ) , boost::begin( s12 ) , boost::begin( s13 ) , boost::begin( s14 ) , boost::begin( s15 ) , op );
}
template< class Value , class S , class Red >
static Value reduce( const S &s , Red red , Value init)
{
return detail::reduce( boost::begin( s ) , boost::end( s ) , red , init );
}
template< class Value , class S1 , class S2 , class Red >
static Value reduce2( const S1 &s1 , const S2 &s2 , Red red , Value init )
{
return detail::reduce2( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , red , init );
}
template< class Value , class S1 , class S2 , class S3 , class Red >
static Value reduce3( const S1 &s1 , const S2 &s2 , const S3 &s3 , Red red , Value init )
{
return detail::reduce3( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , red , init );
}
template< class Value , class S1 , class S2 , class S3 , class S4 , class Red >
static Value reduce4( const S1 &s1 , const S2 &s2 , const S3 &s3 , const S4 &s4 , Red red , Value init )
{
return detail::reduce4( boost::begin( s1 ) , boost::end( s1 ) , boost::begin( s2 ) , boost::begin( s3 ) , boost::begin( s4 ) , red , init );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_RANGE_ALGEBRA_HPP_INCLUDED

156
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/algebra/vector_space_algebra.hpp Executable file
View File

@@ -0,0 +1,156 @@
/*
[auto_generated]
boost/numeric/odeint/algebra/vector_space_algebra.hpp
[begin_description]
An algebra for types which have vector space semantics, hence types on which the operators +,-,* are well defined.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED
#include <boost/type_traits/remove_reference.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* This class template has to be overload in order to call vector_space_algebra::reduce
*/
template< class State > struct vector_space_reduce;
/*
* Example: instantiation for sole doubles
*/
template<>
struct vector_space_reduce< double >
{
template< class Op >
double operator()( double x , Op op , double init ) const
{
init = op( init , x );
return init;
}
};
struct vector_space_algebra
{
template< class S1 , class Op >
static void for_each1( S1 &s1 , Op op )
{
// ToDo : build checks, that the +-*/ operators are well defined
op( s1 );
}
template< class S1 , class S2 , class Op >
static void for_each2( S1 &s1 , S2 &s2 , Op op )
{
op( s1 , s2 );
}
template< class S1 , class S2 , class S3 , class Op >
static void for_each3( S1 &s1 , S2 &s2 , S3 &s3 , Op op )
{
op( s1 , s2 , s3 );
}
template< class S1 , class S2 , class S3 , class S4 , class Op >
static void for_each4( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , Op op )
{
op( s1 , s2 , s3 , s4 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class Op >
static void for_each5( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 , class Op >
static void for_each6( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class Op >
static void for_each7( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class Op >
static void for_each8( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class Op >
static void for_each9( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class Op >
static void for_each10( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class Op >
static void for_each11( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class Op >
static void for_each12( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class Op >
static void for_each13( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class Op >
static void for_each14( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 );
}
template< class S1 , class S2 , class S3 , class S4 , class S5 , class S6 ,class S7 , class S8 , class S9 , class S10 , class S11 , class S12 , class S13 , class S14 , class S15 , class Op >
static void for_each15( S1 &s1 , S2 &s2 , S3 &s3 , S4 &s4 , S5 &s5 , S6 &s6 , S7 &s7 , S8 &s8 , S9 &s9 , S10 &s10 , S11 &s11 , S12 &s12 , S13 &s13 , S14 &s14 , S15 &s15 , Op op )
{
op( s1 , s2 , s3 , s4 , s5 , s6 , s7 , s8 , s9 , s10 , s11 , s12 , s13 , s14 , s15 );
}
template< class Value , class S , class Red >
static Value reduce( const S &s , Red red , Value init )
{
boost::numeric::odeint::vector_space_reduce< S > r;
return r( s , red , init );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_ALGEBRA_VECTOR_SPACE_ALGEBRA_HPP_INCLUDED

47
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/config.hpp Executable file
View File

@@ -0,0 +1,47 @@
/*
[auto_generated]
boost/numeric/odeint/config.hpp
[begin_description]
Sets configurations for odeint and used libraries. Should be included before any other odeint library
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED
//increase macro variable to allow rk78 scheme
#ifndef FUSION_MAX_VECTOR_SIZE
#define FUSION_MAX_VECTOR_SIZE 15
#endif
/*
* the following definitions are only required if fusion vectors are used as state types
* in the rk78 scheme
* they should be defined by the user if required, see e.g. libs/numeric/examples/harmonic_oscillator_units.cpp
*/
#ifndef BOOST_FUSION_INVOKE_MAX_ARITY
#define BOOST_FUSION_INVOKE_MAX_ARITY 15
#endif
#ifndef BOOST_RESULT_OF_NUM_ARGS
#define BOOST_RESULT_OF_NUM_ARGS 15
#endif
/*
*/
#include <boost/config.hpp>
#if __cplusplus >= 201103L
#define BOOST_NUMERIC_ODEINT_CXX11 1
#endif
#endif // BOOST_NUMERIC_ODEINT_CONFIG_HPP_INCLUDED

229
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/gsl/gsl_wrapper.hpp vendored Executable file
View File

@@ -0,0 +1,229 @@
/*
[auto_generated]
boost/numeric/odeint/external/gsl/gsl_wrapper.hpp
[begin_description]
Wrapper for gsl_vector.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED
#include <new>
#include <gsl/gsl_vector.h>
#include <boost/type_traits/integral_constant.hpp>
#include <boost/range.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
class const_gsl_vector_iterator;
/*
* defines an iterator for gsl_vector
*/
class gsl_vector_iterator : public boost::iterator_facade< gsl_vector_iterator , double , boost::random_access_traversal_tag >
{
public :
gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { }
explicit gsl_vector_iterator( gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { }
friend gsl_vector_iterator end_iterator( gsl_vector * );
private :
friend class boost::iterator_core_access;
friend class const_gsl_vector_iterator;
void increment( void ) { m_p += m_stride; }
void decrement( void ) { m_p -= m_stride; }
void advance( ptrdiff_t n ) { m_p += n*m_stride; }
bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; }
bool equal( const const_gsl_vector_iterator &other ) const;
double& dereference( void ) const { return *m_p; }
double *m_p;
size_t m_stride;
};
/*
* defines an const iterator for gsl_vector
*/
class const_gsl_vector_iterator : public boost::iterator_facade< const_gsl_vector_iterator , const double , boost::random_access_traversal_tag >
{
public :
const_gsl_vector_iterator( void ): m_p(0) , m_stride( 0 ) { }
explicit const_gsl_vector_iterator( const gsl_vector *p ) : m_p( p->data ) , m_stride( p->stride ) { }
const_gsl_vector_iterator( const gsl_vector_iterator &p ) : m_p( p.m_p ) , m_stride( p.m_stride ) { }
private :
friend class boost::iterator_core_access;
friend class gsl_vector_iterator;
friend const_gsl_vector_iterator end_iterator( const gsl_vector * );
void increment( void ) { m_p += m_stride; }
void decrement( void ) { m_p -= m_stride; }
void advance( ptrdiff_t n ) { m_p += n*m_stride; }
bool equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; }
bool equal( const gsl_vector_iterator &other ) const { return this->m_p == other.m_p; }
const double& dereference( void ) const { return *m_p; }
const double *m_p;
size_t m_stride;
};
bool gsl_vector_iterator::equal( const const_gsl_vector_iterator &other ) const { return this->m_p == other.m_p; }
gsl_vector_iterator end_iterator( gsl_vector *x )
{
gsl_vector_iterator iter( x );
iter.m_p += iter.m_stride * x->size;
return iter;
}
const_gsl_vector_iterator end_iterator( const gsl_vector *x )
{
const_gsl_vector_iterator iter( x );
iter.m_p += iter.m_stride * x->size;
return iter;
}
namespace boost
{
template<>
struct range_mutable_iterator< gsl_vector* >
{
typedef gsl_vector_iterator type;
};
template<>
struct range_const_iterator< gsl_vector* >
{
typedef const_gsl_vector_iterator type;
};
} // namespace boost
// template<>
inline gsl_vector_iterator range_begin( gsl_vector *x )
{
return gsl_vector_iterator( x );
}
// template<>
inline const_gsl_vector_iterator range_begin( const gsl_vector *x )
{
return const_gsl_vector_iterator( x );
}
// template<>
inline gsl_vector_iterator range_end( gsl_vector *x )
{
return end_iterator( x );
}
// template<>
inline const_gsl_vector_iterator range_end( const gsl_vector *x )
{
return end_iterator( x );
}
namespace boost {
namespace numeric {
namespace odeint {
template<>
struct is_resizeable< gsl_vector* >
{
//struct type : public boost::true_type { };
typedef boost::true_type type;
const static bool value = type::value;
};
template <>
struct same_size_impl< gsl_vector* , gsl_vector* >
{
static bool same_size( const gsl_vector* x , const gsl_vector* y )
{
return x->size == y->size;
}
};
template <>
struct resize_impl< gsl_vector* , gsl_vector* >
{
static void resize( gsl_vector* x , const gsl_vector* y )
{
gsl_vector_free( x );
x = gsl_vector_alloc( y->size );
}
};
template<>
struct state_wrapper< gsl_vector* >
{
typedef double value_type;
typedef gsl_vector* state_type;
typedef state_wrapper< gsl_vector* > state_wrapper_type;
state_type m_v;
state_wrapper( )
{
m_v = gsl_vector_alloc( 1 );
}
state_wrapper( const state_wrapper_type &x )
{
resize( m_v , x.m_v );
gsl_vector_memcpy( m_v , x.m_v );
}
~state_wrapper()
{
gsl_vector_free( m_v );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_GSL_GSL_WRAPPER_HPP_INCLUDED

181
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mkl/mkl_operations.hpp vendored Executable file
View File

@@ -0,0 +1,181 @@
/*
[auto_generated]
boost/numeric/odeint/external/mkl/mkl_operations.hpp
[begin_description]
Wrapper classes for intel math kernel library types.
Get a free, non-commercial download of MKL at
http://software.intel.com/en-us/articles/non-commercial-software-download/
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED
#include <iostream>
#include <mkl_cblas.h>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
/* exemplary example for writing bindings to the Intel MKL library
* see test/mkl for how to use mkl with odeint
* this is a quick and dirty implementation showing the general possibility.
* It works only with containers based on double and sequential memory allocation.
*/
namespace boost {
namespace numeric {
namespace odeint {
/* only defined for doubles */
struct mkl_operations
{
//template< class Fac1 , class Fac2 > struct scale_sum2;
template< class F1 = double , class F2 = F1 >
struct scale_sum2
{
typedef double Fac1;
typedef double Fac2;
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 ) : m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }
template< class T1 , class T2 , class T3 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3) const
{ // t1 = m_alpha1 * t2 + m_alpha2 * t3;
// we get Containers that have size() and [i]-access
const int n = t1.size();
//boost::numeric::odeint::copy( t1 , t3 );
if( &(t2[0]) != &(t1[0]) )
{
cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
}
cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
//daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
}
};
template< class F1 = double , class F2 = F1 , class F3 = F2 >
struct scale_sum3
{
typedef double Fac1;
typedef double Fac2;
typedef double Fac3;
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { }
template< class T1 , class T2 , class T3 , class T4 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 ) const
{ // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4;
// we get Containers that have size() and [i]-access
const int n = t1.size();
//boost::numeric::odeint::copy( t1 , t3 );
if( &(t2[0]) != &(t1[0]) )
{
cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
}
cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
//daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
}
};
template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 >
struct scale_sum4
{
typedef double Fac1;
typedef double Fac2;
typedef double Fac3;
typedef double Fac4;
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 ) const
{ // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5;
// we get Containers that have size() and [i]-access
const int n = t1.size();
//boost::numeric::odeint::copy( t1 , t3 );
if( &(t2[0]) != &(t1[0]) )
{
cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
}
cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
//daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 );
}
};
template< class F1 = double , class F2 = F1 , class F3 = F2 , class F4 = F3 , class F5 = F4 >
struct scale_sum5
{
typedef double Fac1;
typedef double Fac2;
typedef double Fac3;
typedef double Fac4;
typedef double Fac5;
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 , const Fac5 alpha5 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 >
void operator()( T1 &t1 , const T2 &t2 , const T3 &t3 , const T4 &t4 , const T5 &t5 , const T6 &t6 ) const
{ // t1 = m_alpha1 * t2 + m_alpha2 * t3 + m_alpha3 * t4 + m_alpha4 * t5 + m_alpha5 * t6;
// we get Containers that have size() and [i]-access
const int n = t1.size();
//boost::numeric::odeint::copy( t1 , t3 );
if( &(t2[0]) != &(t1[0]) )
{
cblas_dcopy( n , &(t2[0]) , 1 , &(t1[0]) , 1 );
}
cblas_dscal( n , m_alpha1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha2 , &(t3[0]) , 1 , &(t1[0]) , 1 );
//daxpby( &n , &m_alpha2 , &(t3[0]) , &one , &m_alpha1 , &(t1[0]) , &one );
cblas_daxpy( n , m_alpha3 , &(t4[0]) , 1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha4 , &(t5[0]) , 1 , &(t1[0]) , 1 );
cblas_daxpy( n , m_alpha5 , &(t6[0]) , 1 , &(t1[0]) , 1 );
}
};
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MKL_MKL_OPERATIONS_HPP_INCLUDED

162
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/implicit_euler_mtl4.hpp vendored Executable file
View File

@@ -0,0 +1,162 @@
/*
[begin_description]
Modification of the implicit Euler method, works with the MTL4 matrix library only.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Copyright 2012 Andreas Angelopoulos
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED
#include <utility>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/external/mtl4/mtl4_resize.hpp>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/itl/itl.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class ValueType , class Resizer = initially_resizer >
class implicit_euler_mtl4
{
public:
typedef ValueType value_type;
typedef value_type time_type;
typedef mtl::dense_vector<value_type> state_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_type deriv_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef mtl::compressed2D< value_type > matrix_type;
typedef state_wrapper< matrix_type > wrapped_matrix_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
typedef implicit_euler_mtl4< ValueType , Resizer > stepper_type;
implicit_euler_mtl4( const value_type epsilon = 1E-6 )
: m_epsilon( epsilon ) , m_resizer() ,
m_dxdt() , m_x() ,
m_identity() , m_jacobi()
{ }
template< class System >
void do_step( System system , state_type &x , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
m_resizer.adjust_size( x , detail::bind(
&stepper_type::template resize_impl< state_type > , detail::ref( *this ) , detail::_1 ) );
m_identity.m_v = 1;
t += dt;
m_x.m_v = x;
deriv_func( x , m_dxdt.m_v , t );
jacobi_func( x , m_jacobi.m_v , t );
m_dxdt.m_v *= -dt;
m_jacobi.m_v *= dt;
m_jacobi.m_v -= m_identity.m_v ;
// using ilu_0 preconditioning -incomplete LU factorisation
// itl::pc::diagonal<matrix_type,double> L(m_jacobi.m_v);
itl::pc::ilu_0<matrix_type> L( m_jacobi.m_v );
solve( m_jacobi.m_v , m_x.m_v , m_dxdt.m_v , L );
x+= m_x.m_v;
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
}
private:
/*
Applying approximate iterative linear solvers
default solver is Biconjugate gradient stabilized method
itl::bicgstab(A, x, b, L, iter);
*/
template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB, class Preconditioner>
void solve(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
const Preconditioner& L, int max_iteractions =500)
{
// Termination criterion: r < 1e-6 * b or N iterations
itl::basic_iteration< double > iter( b , max_iteractions , 1e-6 );
itl::bicgstab( A , x , b , L , iter );
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_identity , x , typename is_resizeable<matrix_type>::type() );
resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
return resized;
}
private:
value_type m_epsilon;
resizer_type m_resizer;
wrapped_deriv_type m_dxdt;
wrapped_state_type m_x;
wrapped_matrix_type m_identity;
wrapped_matrix_type m_jacobi;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_IMPLICIT_EULER_MTL4_HPP_INCLUDED

133
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/mtl4/mtl4_resize.hpp vendored Executable file
View File

@@ -0,0 +1,133 @@
/*
[begin_description]
Modification of the implicit Euler method, works with the MTL4 matrix library only.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Copyright 2012 Andreas Angelopoulos
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resize.hpp>
#include <boost/numeric/odeint/util/same_size.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/matrix/dense2D.hpp>
#include <boost/numeric/mtl/matrix/compressed2D.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class Value , class Parameters >
struct is_resizeable< mtl::dense_vector< Value , Parameters > >
{
typedef boost::true_type type;
const static bool value = type::value;
};
template< class Value , class Parameters >
struct is_resizeable< mtl::dense2D< Value , Parameters > >
{
typedef boost::true_type type;
const static bool value = type::value;
};
template< class Value , class Parameters >
struct is_resizeable< mtl::compressed2D< Value , Parameters > >
{
typedef boost::true_type type;
const static bool value = type::value;
};
template< class Value , class Parameters >
struct same_size_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > >
{
static bool same_size( const mtl::dense_vector< Value , Parameters > &v1 ,
const mtl::dense_vector< Value , Parameters > &v2 )
{
return mtl::size( v1 ) == mtl::size( v2 );
}
};
template< class Value , class Parameters >
struct resize_impl< mtl::dense_vector< Value , Parameters > , mtl::dense_vector< Value , Parameters > >
{
static void resize( mtl::dense_vector< Value , Parameters > &v1 ,
const mtl::dense_vector< Value , Parameters > &v2 )
{
v1.change_dim( mtl::size( v2 ) );
}
};
template< class Value , class MatrixParameters , class VectorParameters >
struct same_size_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
static bool same_size( const mtl::dense2D< Value , MatrixParameters > &m ,
const mtl::dense_vector< Value , VectorParameters > &v )
{
return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) );
}
};
template< class Value , class MatrixParameters , class VectorParameters >
struct resize_impl< mtl::dense2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
static void resize( mtl::dense2D< Value , MatrixParameters > &m ,
const mtl::dense_vector< Value , VectorParameters > &v )
{
m.change_dim( mtl::size( v ) , mtl::size( v ) , false );
}
};
template< class Value , class MatrixParameters , class VectorParameters >
struct same_size_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
static bool same_size( const mtl::compressed2D< Value , MatrixParameters > &m ,
const mtl::dense_vector< Value , VectorParameters > &v )
{
return ( ( mtl::size( v ) == m.num_cols() ) && ( mtl::size( v ) == m.num_rows() ) );
}
};
template< class Value , class MatrixParameters , class VectorParameters >
struct resize_impl< mtl::compressed2D< Value , MatrixParameters > , mtl::dense_vector< Value , VectorParameters > >
{
static void resize( mtl::compressed2D< Value , MatrixParameters > &m ,
const mtl::dense_vector< Value , VectorParameters > &v )
{
m.change_dim( mtl::size( v ) , mtl::size( v ) );
}
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_MTL4_RESIZE_HPP_INCLUDED

198
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_algebra.hpp vendored Executable file
View File

@@ -0,0 +1,198 @@
/*
[auto_generated]
boost/numeric/odeint/external/thrust/thrust_algebra.hpp
[begin_description]
An algebra for thrusts device_vectors.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED
#include <thrust/device_vector.h>
#include <thrust/for_each.h>
#include <thrust/iterator/zip_iterator.h>
#include <boost/range.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/** ToDO extend until for_each14 for rk78 */
/*
* The const versions are needed for boost.range to work, i.e.
* it allows you to do
* for_each1( make_pair( vec1.begin() , vec1.begin() + 10 ) , op );
*/
struct thrust_algebra
{
template< class StateType , class Operation >
static void for_each1( StateType &s , Operation op )
{
thrust::for_each( boost::begin(s) , boost::begin(s) , op );
}
template< class StateType1 , class StateType2 , class Operation >
static void for_each2( StateType1 &s1 , StateType2 &s2 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 , class Operation >
static void for_each3( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 , class StateType4 ,
class Operation >
static void for_each4( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 ,
Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ,
boost::begin(s4) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ,
boost::end(s4) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 ,
class StateType4 , class StateType5 ,class Operation >
static void for_each5( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 ,
StateType5 &s5 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ,
boost::begin(s4) ,
boost::begin(s5) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ,
boost::end(s4) ,
boost::end(s5) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 ,
class StateType4 , class StateType5 , class StateType6 , class Operation >
static void for_each6( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 ,
StateType5 &s5 , StateType6 &s6 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ,
boost::begin(s4) ,
boost::begin(s5) ,
boost::begin(s6) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ,
boost::end(s4) ,
boost::end(s5) ,
boost::end(s6) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 , class StateType4 ,
class StateType5 , class StateType6 , class StateType7 , class Operation >
static void for_each7( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 ,
StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ,
boost::begin(s4) ,
boost::begin(s5) ,
boost::begin(s6) ,
boost::begin(s7) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ,
boost::end(s4) ,
boost::end(s5) ,
boost::end(s6) ,
boost::end(s7) ) ) ,
op);
}
template< class StateType1 , class StateType2 , class StateType3 , class StateType4 ,
class StateType5 , class StateType6 , class StateType7 , class StateType8 , class Operation >
static void for_each8( StateType1 &s1 , StateType2 &s2 , StateType3 &s3 , StateType4 &s4 ,
StateType5 &s5 , StateType6 &s6 , StateType7 &s7 , StateType8 &s8 , Operation op )
{
thrust::for_each(
thrust::make_zip_iterator( thrust::make_tuple( boost::begin(s1) ,
boost::begin(s2) ,
boost::begin(s3) ,
boost::begin(s4) ,
boost::begin(s5) ,
boost::begin(s6) ,
boost::begin(s7) ,
boost::begin(s8) ) ) ,
thrust::make_zip_iterator( thrust::make_tuple( boost::end(s1) ,
boost::end(s2) ,
boost::end(s3) ,
boost::end(s4) ,
boost::end(s5) ,
boost::end(s6) ,
boost::end(s7) ,
boost::end(s8) ) ) ,
op);
}
template< class Value , class S , class Red >
Value reduce( const S &s , Red red , Value init)
{
return thrust::reduce( boost::begin( s ) , boost::end( s ) , init , red );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_ALGEBRA_HPP_INCLUDED

252
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_operations.hpp vendored Executable file
View File

@@ -0,0 +1,252 @@
/*
[auto_generated]
boost/numeric/odeint/external/thrust/thrust_operations.hpp
[begin_description]
Operations of thrust zipped iterators. Is the counterpart of the thrust_algebra.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
#include <thrust/tuple.h>
#include <thrust/iterator/zip_iterator.h>
/**ToDo extend to scale_sum13 for rk78 */
struct thrust_operations
{
template< class Fac1 = double , class Fac2 = Fac1 >
struct scale_sum2
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum2( const Fac1 alpha1 , const Fac2 alpha2 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t);
}
};
template< class Fac1 = double , class Fac2 = Fac1 >
struct scale_sum_swap2
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum_swap2( const Fac1 alpha1 , const Fac2 alpha2 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
typename thrust::tuple_element<0,Tuple>::type tmp = thrust::get<0>(t);
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) + m_alpha2 * thrust::get<2>(t);
thrust::get<1>(t) = tmp;
}
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 >
struct scale_sum3
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
scale_sum3( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) +
m_alpha2 * thrust::get<2>(t) +
m_alpha3 * thrust::get<3>(t);
}
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 >
struct scale_sum4
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
scale_sum4( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 , const Fac4 alpha4 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ){ }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) +
m_alpha2 * thrust::get<2>(t) +
m_alpha3 * thrust::get<3>(t) +
m_alpha4 * thrust::get<4>(t);
}
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 ,
class Fac4 = Fac3 , class Fac5 = Fac4 >
struct scale_sum5
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
scale_sum5( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ,
const Fac4 alpha4 , const Fac5 alpha5 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) ,
m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) +
m_alpha2 * thrust::get<2>(t) +
m_alpha3 * thrust::get<3>(t) +
m_alpha4 * thrust::get<4>(t) +
m_alpha5 * thrust::get<5>(t);
}
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 ,
class Fac4 = Fac3 , class Fac5 = Fac4 , class Fac6 = Fac5 >
struct scale_sum6
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
scale_sum6( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ,
const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) ,
m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) +
m_alpha2 * thrust::get<2>(t) +
m_alpha3 * thrust::get<3>(t) +
m_alpha4 * thrust::get<4>(t) +
m_alpha5 * thrust::get<5>(t) +
m_alpha6 * thrust::get<6>(t);
}
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 ,
class Fac5 = Fac4 , class Fac6 = Fac5 , class Fac7 = Fac6 >
struct scale_sum7
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
const Fac6 m_alpha6;
const Fac7 m_alpha7;
scale_sum7( const Fac1 alpha1 , const Fac2 alpha2 , const Fac3 alpha3 ,
const Fac4 alpha4 , const Fac5 alpha5 , const Fac6 alpha6 , const Fac7 alpha7 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) ,
m_alpha4( alpha4 ) , m_alpha5( alpha5 ) , m_alpha6( alpha6 ) , m_alpha7( alpha7 ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
thrust::get<0>(t) = m_alpha1 * thrust::get<1>(t) +
m_alpha2 * thrust::get<2>(t) +
m_alpha3 * thrust::get<3>(t) +
m_alpha4 * thrust::get<4>(t) +
m_alpha5 * thrust::get<5>(t) +
m_alpha6 * thrust::get<6>(t) +
m_alpha7 * thrust::get<7>(t) ;
}
};
template< class Fac1 = double >
struct rel_error
{
const Fac1 m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt;
rel_error( const Fac1 eps_abs , const Fac1 eps_rel , const Fac1 a_x , const Fac1 a_dxdt )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt ) { }
template< class Tuple >
__host__ __device__
void operator()( Tuple t ) const
{
using std::abs;
thrust::get< 0 >( t ) = abs( thrust::get< 0 >( t ) ) /
( m_eps_abs + m_eps_rel * ( m_a_x * abs( thrust::get< 1 >( t ) + m_a_dxdt * abs( thrust::get< 2 >( t ) ) ) ) );
}
typedef void result_type;
};
/*
* for usage in reduce
*/
template< class Value >
struct maximum
{
template< class Fac1 , class Fac2 >
__host__ __device__
Value operator()( const Fac1 t1 , const Fac2 t2 ) const
{
using std::max;
return ( abs( t1 ) < abs( t2 ) ) ? t2 : t1 ;
}
typedef Value result_type;
};
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_OPERATIONS_HPP_INCLUDED

119
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/thrust/thrust_resize.hpp vendored Executable file
View File

@@ -0,0 +1,119 @@
/*
[auto_generated]
boost/numeric/odeint/external/thrust/thrust_resize.hpp
[begin_description]
Enable resizing for thrusts device and host_vector.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED
#include <thrust/device_vector.h>
#include <thrust/host_vector.h>
#include <boost/numeric/odeint/util/copy.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class T >
struct is_resizeable< thrust::device_vector< T > >
{
struct type : public boost::true_type { };
const static bool value = type::value;
};
template< class T >
struct same_size_impl< thrust::device_vector< T > , thrust::device_vector< T > >
{
static bool same_size( const thrust::device_vector< T > &x , const thrust::device_vector< T > &y )
{
return x.size() == y.size();
}
};
template< class T >
struct resize_impl< thrust::device_vector< T > , thrust::device_vector< T > >
{
static void resize( thrust::device_vector< T > &x , const thrust::device_vector< T > &y )
{
x.resize( y.size() );
}
};
template< class T >
struct is_resizeable< thrust::host_vector< T > >
{
struct type : public boost::true_type { };
const static bool value = type::value;
};
template< class T >
struct same_size_impl< thrust::host_vector< T > , thrust::host_vector< T > >
{
static bool same_size( const thrust::host_vector< T > &x , const thrust::host_vector< T > &y )
{
return x.size() == y.size();
}
};
template< class T >
struct resize_impl< thrust::host_vector< T > , thrust::host_vector< T > >
{
static void resize( thrust::host_vector< T > &x , const thrust::host_vector< T > &y )
{
x.resize( y.size() );
}
};
template< class Container1, class Value >
struct copy_impl< Container1 , thrust::device_vector< Value > >
{
static void copy( const Container1 &from , thrust::device_vector< Value > &to )
{
thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) );
}
};
template< class Value , class Container2 >
struct copy_impl< thrust::device_vector< Value > , Container2 >
{
static void copy( const thrust::device_vector< Value > &from , Container2 &to )
{
thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) );
}
};
template< class Value >
struct copy_impl< thrust::device_vector< Value > , thrust::device_vector< Value > >
{
static void copy( const thrust::device_vector< Value > &from , thrust::device_vector< Value > &to )
{
thrust::copy( boost::begin( from ) , boost::end( from ) , boost::begin( to ) );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_THRUST_THRUST_RESIZE_HPP_INCLUDED

92
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/vexcl/vexcl_resize.hpp vendored Executable file
View File

@@ -0,0 +1,92 @@
/*
[auto_generated]
boost/numeric/odeint/external/vexcl/vexcl_resize.hpp
[begin_description]
Enable resizing for vexcl vector and multivector.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED
#include <vexcl/vector.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resize.hpp>
#include <boost/numeric/odeint/util/same_size.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* specializations for vex::vector< T >
*/
template< typename T >
struct is_resizeable< vex::vector< T > > : boost::true_type { };
template< typename T >
struct resize_impl< vex::vector< T > , vex::vector< T > >
{
static void resize( vex::vector< T > &x1 , const vex::vector< T > &x2 )
{
x1.resize( x2.queue_list() , x2.size() );
}
};
template< typename T >
struct same_size_impl< vex::vector< T > , vex::vector< T > >
{
static bool same_size( const vex::vector< T > &x1 , const vex::vector< T > &x2 )
{
return x1.size() == x2.size();
}
};
/*
* specializations for vex::multivector< T >
*/
template< typename T , size_t N, bool own >
struct is_resizeable< vex::multivector< T , N , own > > : boost::true_type { };
template< typename T , size_t N, bool own >
struct resize_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > >
{
static void resize( vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 )
{
x1.resize( x2.queue_list() , x2.size() );
}
};
template< typename T , size_t N, bool own >
struct same_size_impl< vex::multivector< T , N , own > , vex::multivector< T , N , own > >
{
static bool same_size( const vex::multivector< T , N , own > &x1 , const vex::multivector< T , N , own > &x2 )
{
return x1.size() == x2.size();
}
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_RESIZE_HPP_INCLUDED

245
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_operations.hpp vendored Executable file
View File

@@ -0,0 +1,245 @@
/*
[auto_generated]
boost/numeric/odeint/external/viennacl_operations.hpp
[begin_description]
ViennaCL operations.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED
#include <viennacl/vector.hpp>
#include <viennacl/generator/custom_operation.hpp>
namespace boost {
namespace numeric {
namespace odeint {
struct viennacl_operations
{
template< class Fac1 = double , class Fac2 = Fac1 >
struct scale_sum2
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
scale_sum2( Fac1 alpha1 , Fac2 alpha2 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 )
{ }
template< class T1 , class T2 , class T3 >
void operator()( viennacl::vector<T1> &v1 ,
const viennacl::vector<T2> &v2 ,
const viennacl::vector<T3> &v3
) const
{
using namespace viennacl;
static generator::symbolic_vector <0, T1> sym_v1;
static generator::symbolic_vector <1, T2> sym_v2;
static generator::symbolic_vector <2, T3> sym_v3;
static generator::cpu_symbolic_scalar<3, Fac1> sym_a1;
static generator::cpu_symbolic_scalar<4, Fac2> sym_a2;
static generator::custom_operation op(
sym_v1 = sym_a1 * sym_v2
+ sym_a2 * sym_v3
);
ocl::enqueue( op(v1,
const_cast< viennacl::vector<T2>& >(v2),
const_cast< viennacl::vector<T3>& >(v3),
const_cast< Fac1& >(m_alpha1),
const_cast< Fac2& >(m_alpha2)
) );
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 >
struct scale_sum3
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
scale_sum3( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 )
{ }
template< class T1 , class T2 , class T3 , class T4 >
void operator()( viennacl::vector<T1> &v1 ,
const viennacl::vector<T2> &v2 ,
const viennacl::vector<T3> &v3 ,
const viennacl::vector<T4> &v4
) const
{
using namespace viennacl;
static generator::symbolic_vector <0, T1> sym_v1;
static generator::symbolic_vector <1, T2> sym_v2;
static generator::symbolic_vector <2, T3> sym_v3;
static generator::symbolic_vector <3, T4> sym_v4;
static generator::cpu_symbolic_scalar<4, Fac1> sym_a1;
static generator::cpu_symbolic_scalar<5, Fac2> sym_a2;
static generator::cpu_symbolic_scalar<6, Fac3> sym_a3;
static generator::custom_operation op(
sym_v1 = sym_a1 * sym_v2
+ sym_a2 * sym_v3
+ sym_a3 * sym_v4
);
ocl::enqueue( op(v1,
const_cast< viennacl::vector<T2>& >(v2),
const_cast< viennacl::vector<T3>& >(v3),
const_cast< viennacl::vector<T4>& >(v4),
const_cast< Fac1& >(m_alpha1),
const_cast< Fac2& >(m_alpha2),
const_cast< Fac3& >(m_alpha3)
) );
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 >
struct scale_sum4
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
scale_sum4( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 >
void operator()( viennacl::vector<T1> &v1 ,
const viennacl::vector<T2> &v2 ,
const viennacl::vector<T3> &v3 ,
const viennacl::vector<T4> &v4 ,
const viennacl::vector<T5> &v5
) const
{
using namespace viennacl;
static generator::symbolic_vector <0, T1> sym_v1;
static generator::symbolic_vector <1, T2> sym_v2;
static generator::symbolic_vector <2, T3> sym_v3;
static generator::symbolic_vector <3, T4> sym_v4;
static generator::symbolic_vector <4, T5> sym_v5;
static generator::cpu_symbolic_scalar<5, Fac1> sym_a1;
static generator::cpu_symbolic_scalar<6, Fac2> sym_a2;
static generator::cpu_symbolic_scalar<7, Fac3> sym_a3;
static generator::cpu_symbolic_scalar<8, Fac4> sym_a4;
static generator::custom_operation op(
sym_v1 = sym_a1 * sym_v2
+ sym_a2 * sym_v3
+ sym_a3 * sym_v4
+ sym_a4 * sym_v5
);
ocl::enqueue( op(v1,
const_cast< viennacl::vector<T2>& >(v2),
const_cast< viennacl::vector<T3>& >(v3),
const_cast< viennacl::vector<T4>& >(v4),
const_cast< viennacl::vector<T5>& >(v5),
const_cast< Fac1& >(m_alpha1),
const_cast< Fac2& >(m_alpha2),
const_cast< Fac3& >(m_alpha3),
const_cast< Fac4& >(m_alpha4)
) );
}
typedef void result_type;
};
template< class Fac1 = double , class Fac2 = Fac1 , class Fac3 = Fac2 , class Fac4 = Fac3 , class Fac5 = Fac4 >
struct scale_sum5
{
const Fac1 m_alpha1;
const Fac2 m_alpha2;
const Fac3 m_alpha3;
const Fac4 m_alpha4;
const Fac5 m_alpha5;
scale_sum5( Fac1 alpha1 , Fac2 alpha2 , Fac3 alpha3 , Fac4 alpha4 , Fac5 alpha5 )
: m_alpha1( alpha1 ) , m_alpha2( alpha2 ) , m_alpha3( alpha3 ) , m_alpha4( alpha4 ) , m_alpha5( alpha5 ) { }
template< class T1 , class T2 , class T3 , class T4 , class T5 , class T6 >
void operator()( viennacl::vector<T1> &v1 ,
const viennacl::vector<T2> &v2 ,
const viennacl::vector<T3> &v3 ,
const viennacl::vector<T4> &v4 ,
const viennacl::vector<T5> &v5 ,
const viennacl::vector<T6> &v6
) const
{
using namespace viennacl;
static generator::symbolic_vector < 0, T1> sym_v1;
static generator::symbolic_vector < 1, T2> sym_v2;
static generator::symbolic_vector < 2, T3> sym_v3;
static generator::symbolic_vector < 3, T4> sym_v4;
static generator::symbolic_vector < 4, T5> sym_v5;
static generator::symbolic_vector < 5, T6> sym_v6;
static generator::cpu_symbolic_scalar< 6, Fac1> sym_a1;
static generator::cpu_symbolic_scalar< 7, Fac2> sym_a2;
static generator::cpu_symbolic_scalar< 8, Fac3> sym_a3;
static generator::cpu_symbolic_scalar< 9, Fac4> sym_a4;
static generator::cpu_symbolic_scalar<10, Fac5> sym_a5;
static generator::custom_operation op(
sym_v1 = sym_a1 * sym_v2
+ sym_a2 * sym_v3
+ sym_a3 * sym_v4
+ sym_a4 * sym_v5
+ sym_a5 * sym_v6
);
ocl::enqueue( op(v1,
const_cast< viennacl::vector<T2>& >(v2),
const_cast< viennacl::vector<T3>& >(v3),
const_cast< viennacl::vector<T4>& >(v4),
const_cast< viennacl::vector<T5>& >(v5),
const_cast< viennacl::vector<T6>& >(v6),
const_cast< Fac1& >(m_alpha1),
const_cast< Fac2& >(m_alpha2),
const_cast< Fac3& >(m_alpha3),
const_cast< Fac4& >(m_alpha4),
const_cast< Fac5& >(m_alpha5)
) );
}
typedef void result_type;
};
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_OPERATIONS_HPP_INCLUDED

65
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/external/viennacl/viennacl_resize.hpp vendored Executable file
View File

@@ -0,0 +1,65 @@
/*
[auto_generated]
boost/numeric/odeint/external/viennacl/viennacl_resize.hpp
[begin_description]
Enable resizing for viennacl vector.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED
#include <viennacl/vector.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resize.hpp>
#include <boost/numeric/odeint/util/same_size.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* specializations for viennacl::vector< T >
*/
template< typename T >
struct is_resizeable< viennacl::vector< T > > : boost::true_type { };
template< typename T >
struct resize_impl< viennacl::vector< T > , viennacl::vector< T > >
{
static void resize( viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 )
{
x1.resize( x2.size() , false );
}
};
template< typename T >
struct same_size_impl< viennacl::vector< T > , viennacl::vector< T > >
{
static bool same_size( const viennacl::vector< T > &x1 , const viennacl::vector< T > &x2 )
{
return x1.size() == x2.size();
}
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VIENNACL_VIENNACL_RESIZE_HPP_INCLUDED

154
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp Executable file
View File

@@ -0,0 +1,154 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp
[begin_description]
Default Integrate adaptive implementation.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED
#include <stdexcept>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_const.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
#include <iostream>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
// forward declaration
template< class Stepper , class System , class State , class Time , class Observer>
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , stepper_tag );
/*
* integrate_adaptive for simple stepper is basically an integrate_const + some last step
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , stepper_tag
)
{
size_t steps = detail::integrate_const( stepper , system , start_state , start_time ,
end_time , dt , observer , stepper_tag() );
Time end = start_time + dt*steps;
if( less_with_sign( end , end_time , dt ) )
{ //make a last step to end exactly at end_time
stepper.do_step( system , start_state , end , end_time - end );
steps++;
typename odeint::unwrap_reference< Observer >::type &obs = observer;
obs( start_state , end_time );
}
return steps;
}
/*
* classical integrate adaptive
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time &start_time , Time end_time , Time &dt ,
Observer observer , controlled_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
const size_t max_attempts = 1000;
const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found.";
size_t count = 0;
while( less_with_sign( start_time , end_time , dt ) )
{
obs( start_state , start_time );
if( less_with_sign( end_time , start_time + dt , dt ) )
{
dt = end_time - start_time;
}
size_t trials = 0;
controlled_step_result res = success;
do
{
res = stepper.try_step( system , start_state , start_time , dt );
++trials;
}
while( ( res == fail ) && ( trials < max_attempts ) );
if( trials == max_attempts ) throw std::overflow_error( error_string );
++count;
}
obs( start_state , start_time );
return count;
}
/*
* integrate adaptive for dense output steppers
*
* step size control is used if the stepper supports it
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , dense_output_stepper_tag )
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
size_t count = 0;
stepper.initialize( start_state , start_time , dt );
while( less_with_sign( stepper.current_time() , end_time , stepper.current_time_step() ) )
{
while( less_eq_with_sign( stepper.current_time() + stepper.current_time_step() ,
end_time ,
stepper.current_time_step() ) )
{ //make sure we don't go beyond the end_time
obs( stepper.current_state() , stepper.current_time() );
stepper.do_step( system );
++count;
}
stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() );
}
obs( stepper.current_state() , stepper.current_time() );
// overwrite start_state with the final point
boost::numeric::odeint::copy( stepper.current_state() , start_state );
return count;
}
} // namespace detail
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED

159
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_const.hpp Executable file
View File

@@ -0,0 +1,159 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/detail/integrate_const.hpp
[begin_description]
integrate const implementation
[end_description]
Copyright 2009-2012 Karsten Ahnert
Copyright 2009-2012 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_CONST_HPP_INCLUDED
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
// forward declaration
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time &start_time , Time end_time , Time &dt ,
Observer observer , controlled_stepper_tag
);
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
int step = 0;
while( less_eq_with_sign( time+dt , end_time , dt ) )
{
obs( start_state , time );
stepper.do_step( system , start_state , time , dt );
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
++step;
time = start_time + static_cast< typename unit_value_type<Time>::type >(step) * dt;
}
obs( start_state , time );
return step;
}
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , controlled_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
const Time time_step = dt;
int step = 0;
while( less_eq_with_sign( time+time_step , end_time , dt ) )
{
obs( start_state , time );
detail::integrate_adaptive( stepper , system , start_state , time , time+time_step , dt ,
null_observer() , controlled_stepper_tag() );
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
++step;
time = start_time + static_cast< typename unit_value_type<Time>::type >(step) * time_step;
}
obs( start_state , time );
return step;
}
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer , dense_output_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
stepper.initialize( start_state , time , dt );
obs( start_state , time );
time += dt;
int obs_step( 1 );
int real_step( 0 );
while( less_with_sign( time+dt , end_time , dt ) )
{
while( less_eq_with_sign( time , stepper.current_time() , dt ) )
{
stepper.calc_state( time , start_state );
obs( start_state , time );
++obs_step;
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
time = start_time + static_cast< typename unit_value_type<Time>::type >(obs_step) * dt;
}
// we have not reached the end, do another real step
if( less_with_sign( stepper.current_time()+stepper.current_time_step() ,
end_time ,
stepper.current_time_step() ) )
{
while( less_eq_with_sign( stepper.current_time() , time , dt ) )
{
stepper.do_step( system );
++real_step;
}
}
else if( less_with_sign( stepper.current_time() , end_time , stepper.current_time_step() ) )
{ // do the last step ending exactly on the end point
stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() );
stepper.do_step( system );
++real_step;
}
}
// last observation, if we are still in observation interval
if( less_eq_with_sign( time , end_time , dt ) )
{
stepper.calc_state( time , start_state );
obs( start_state , time );
}
return real_step;
}
} } } }
#endif

156
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_n_steps.hpp Executable file
View File

@@ -0,0 +1,156 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/detail/integrate_n_steps.hpp
[begin_description]
integrate steps implementation
[end_description]
Copyright 2009-2012 Karsten Ahnert
Copyright 2009-2012 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_N_STEPS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_N_STEPS_HPP_INCLUDED
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
// forward declaration
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time &start_time , Time end_time , Time &dt ,
Observer observer , controlled_stepper_tag
);
/* basic version */
template< class Stepper , class System , class State , class Time , class Observer>
Time integrate_n_steps(
Stepper stepper , System system , State &start_state ,
Time start_time , Time dt , size_t num_of_steps ,
Observer observer , stepper_tag )
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
for( size_t step = 0; step < num_of_steps ; ++step )
{
obs( start_state , time );
stepper.do_step( system , start_state , time , dt );
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
time = start_time + static_cast< typename unit_value_type<Time>::type >( step+1 ) * dt;
}
obs( start_state , time );
return time;
}
/* controlled version */
template< class Stepper , class System , class State , class Time , class Observer>
Time integrate_n_steps(
Stepper stepper , System system , State &start_state ,
Time start_time , Time dt , size_t num_of_steps ,
Observer observer , controlled_stepper_tag )
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
Time time_step = dt;
for( size_t step = 0; step < num_of_steps ; ++step )
{
obs( start_state , time );
detail::integrate_adaptive( stepper , system , start_state , time , time+time_step , dt ,
null_observer() , controlled_stepper_tag() );
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
time = start_time + static_cast< typename unit_value_type<Time>::type >(step+1) * time_step;
}
obs( start_state , time );
return time;
}
/* dense output version */
template< class Stepper , class System , class State , class Time , class Observer>
Time integrate_n_steps(
Stepper stepper , System system , State &start_state ,
Time start_time , Time dt , size_t num_of_steps ,
Observer observer , dense_output_stepper_tag )
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
Time time = start_time;
const Time end_time = start_time + static_cast< typename unit_value_type<Time>::type >(num_of_steps) * dt;
stepper.initialize( start_state , time , dt );
size_t step = 0;
while( step < num_of_steps )
{
while( less_with_sign( time , stepper.current_time() , stepper.current_time_step() ) )
{
stepper.calc_state( time , start_state );
obs( start_state , time );
++step;
// direct computation of the time avoids error propagation happening when using time += dt
// we need clumsy type analysis to get boost units working here
time = start_time + static_cast< typename unit_value_type<Time>::type >(step) * dt;
}
// we have not reached the end, do another real step
if( less_with_sign( stepper.current_time()+stepper.current_time_step() ,
end_time ,
stepper.current_time_step() ) )
{
stepper.do_step( system );
}
else if( less_with_sign( stepper.current_time() , end_time , stepper.current_time_step() ) )
{ // do the last step ending exactly on the end point
stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() );
stepper.do_step( system );
}
}
while( stepper.current_time() < end_time )
{
if( less_with_sign( end_time ,
stepper.current_time()+stepper.current_time_step() ,
stepper.current_time_step() ) )
stepper.initialize( stepper.current_state() , stepper.current_time() , end_time - stepper.current_time() );
stepper.do_step( system );
}
// observation at end point, only if we ended exactly on the end-point (or above due to finite precision)
obs( stepper.current_state() , end_time );
return time;
}
}
}
}
}
#endif /* BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_N_STEPS_HPP_INCLUDED */

166
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/detail/integrate_times.hpp Executable file
View File

@@ -0,0 +1,166 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/detail/integrate_times.hpp
[begin_description]
Default integrate times implementation.
[end_description]
Copyright 2009-2012 Karsten Ahnert
Copyright 2009-2012 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_TIMES_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_TIMES_HPP_INCLUDED
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
/*
* integrate_times for simple stepper
*/
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator start_time , TimeIterator end_time , Time dt ,
Observer observer , stepper_tag
)
{
BOOST_USING_STD_MIN();
typename odeint::unwrap_reference< Observer >::type &obs = observer;
size_t steps = 0;
Time current_dt = dt;
while( true )
{
Time current_time = *start_time++;
obs( start_state , current_time );
if( start_time == end_time )
break;
while( less_with_sign( current_time , *start_time , current_dt ) )
{
current_dt = min BOOST_PREVENT_MACRO_SUBSTITUTION ( dt , *start_time - current_time );
stepper.do_step( system , start_state , current_time , current_dt );
current_time += current_dt;
steps++;
}
}
return steps;
}
/*
* integrate_times for controlled stepper
*/
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator start_time , TimeIterator end_time , Time dt ,
Observer observer , controlled_stepper_tag
)
{
BOOST_USING_STD_MIN();
typename odeint::unwrap_reference< Observer >::type &obs = observer;
const size_t max_attempts = 1000;
const char *error_string = "Integrate adaptive : Maximal number of iterations reached. A step size could not be found.";
size_t steps = 0;
while( true )
{
size_t fail_steps = 0;
Time current_time = *start_time++;
obs( start_state , current_time );
if( start_time == end_time )
break;
while( less_with_sign( current_time , *start_time , dt ) )
{
dt = min BOOST_PREVENT_MACRO_SUBSTITUTION ( dt , *start_time - current_time );
if( stepper.try_step( system , start_state , current_time , dt ) == success )
{
++steps;
}
else
{
++fail_steps;
}
if( fail_steps == max_attempts ) throw std::overflow_error( error_string );
}
}
return steps;
}
/*
* integrate_times for dense output stepper
*/
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator start_time , TimeIterator end_time , Time dt ,
Observer observer , dense_output_stepper_tag
)
{
typename odeint::unwrap_reference< Observer >::type &obs = observer;
if( start_time == end_time )
return 0;
Time last_time_point = *(end_time-1);
stepper.initialize( start_state , *start_time , dt );
obs( start_state , *start_time++ );
size_t count = 0;
while( start_time != end_time )
{
while( ( start_time != end_time ) && less_eq_with_sign( *start_time , stepper.current_time() , stepper.current_time_step() ) )
{
stepper.calc_state( *start_time , start_state );
obs( start_state , *start_time );
start_time++;
}
// we have not reached the end, do another real step
if( less_eq_with_sign( stepper.current_time() + stepper.current_time_step() ,
last_time_point ,
stepper.current_time_step() ) )
{
stepper.do_step( system );
++count;
}
else if( start_time != end_time )
{ // do the last step ending exactly on the end point
stepper.initialize( stepper.current_state() , stepper.current_time() , last_time_point - stepper.current_time() );
stepper.do_step( system );
++count;
}
}
return count;
}
} // namespace detail
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_DETAIL_INTEGRATE_ADAPTIVE_HPP_INCLUDED

105
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/integrate.hpp Executable file
View File

@@ -0,0 +1,105 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/integrate.hpp
[begin_description]
Convenience methods which choose the stepper for the current ODE.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/integrate_adaptive.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* ToDo :
*
* determine type of dxdt for units
*
*/
template< class System , class State , class Time , class Observer >
size_t integrate( System system , State &start_state , Time start_time , Time end_time , Time dt , Observer observer )
{
return integrate_adaptive( controlled_runge_kutta< runge_kutta_dopri5< State > >() , system , start_state , start_time , end_time , dt , observer );
}
/*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class State , class Time >
size_t integrate( System system , State &start_state , Time start_time , Time end_time , Time dt )
{
return integrate( system , start_state , start_time , end_time , dt , null_observer() );
}
/**
* \fn integrate( System system , State &start_state , Time start_time , Time end_time , Time dt , Observer observer )
* \brief Integrates the ODE.
*
* Integrates the ODE given by system from start_time to end_time starting
* with start_state as initial condition and dt as initial time step.
* This function uses a dense output dopri5 stepper and performs an adaptive
* integration with step size control, thus dt changes during the integration.
* This method uses standard error bounds of 1E-6.
* After each step, the observer is called.
*
* \param system The system function to solve, hence the r.h.s. of the
* ordinary differential equation.
* \param start_state The initial state.
* \param start_time Start time of the integration.
* \param end_time End time of the integration.
* \param dt Initial step size, will be adjusted during the integration.
* \param observer Observer that will be called after each time step.
* \return The number of steps performed.
*/
/**
* \fn integrate( System system , State &start_state , Time start_time , Time end_time , Time dt )
* \brief Integrates the ODE without observer calls.
*
* Integrates the ODE given by system from start_time to end_time starting
* with start_state as initial condition and dt as initial time step.
* This function uses a dense output dopri5 stepper and performs an adaptive
* integration with step size control, thus dt changes during the integration.
* This method uses standard error bounds of 1E-6.
* No observer is called.
*
* \param system The system function to solve, hence the r.h.s. of the
* ordinary differential equation.
* \param start_state The initial state.
* \param start_time Start time of the integration.
* \param end_time End time of the integration.
* \param dt Initial step size, will be adjusted during the integration.
* \return The number of steps performed.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_HPP_INCLUDED

127
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/integrate_adaptive.hpp Executable file
View File

@@ -0,0 +1,127 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/integrate_adaptive.hpp
[begin_description]
Adaptive integration of ODEs.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_ADAPTIVE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_ADAPTIVE_HPP_INCLUDED
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer )
{
return detail::integrate_adaptive(
stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
/*
* Suggestion for a new extendable version:
*
* integrator_adaptive< Stepper , System, State , Time , Observer , typename Stepper::stepper_category > integrator;
* return integrator.run( stepper , system , start_state , start_time , end_time , dt , observer );
*/
}
/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_adaptive(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer )
{
return detail::integrate_adaptive(
stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
}
/**
* \brief integrate_adaptive without an observer.
*/
template< class Stepper , class System , class State , class Time >
size_t integrate_adaptive(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt )
{
return integrate_adaptive( stepper , system , start_state , start_time , end_time , dt , null_observer() );
}
/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time >
size_t integrate_adaptive(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time end_time , Time dt )
{
return integrate_adaptive( stepper , system , start_state , start_time , end_time , dt , null_observer() );
}
/************* DOXYGEN ************/
/**
* \fn integrate_adaptive( Stepper stepper , System system , State &start_state , Time start_time , Time end_time , Time dt , Observer observer )
* \brief Integrates the ODE with adaptive step size.
*
* This function integrates the ODE given by system with the given stepper.
* The observer is called after each step. If the stepper has no error
* control, the step size remains constant and the observer is called at
* equidistant time points t0+n*dt. If the stepper is a ControlledStepper,
* the step size is adjusted and the observer is called in non-equidistant
* intervals.
*
* \param stepper The stepper to be used for numerical integration.
* \param system Function/Functor defining the rhs of the ODE.
* \param start_state The initial condition x0.
* \param start_time The initial time t0.
* \param end_time The final integration time tend.
* \param dt The time step between observer calls, _not_ necessarily the
* time step of the integration.
* \param observer Function/Functor called at equidistant time intervals.
* \return The number of steps performed.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_ADAPTIVE_HPP_INCLUDED

156
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/integrate_const.hpp Executable file
View File

@@ -0,0 +1,156 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/integrate_const.hpp
[begin_description]
Constant integration of ODEs, meaning that the state of the ODE is observed on constant time intervals.
The routines makes full use of adaptive and dense-output methods.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_const.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_adaptive.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* Integrates with constant time step dt.
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer
)
{
// we want to get as fast as possible to the end
if( boost::is_same< null_observer , Observer >::value )
{
return detail::integrate_adaptive(
stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
}
else
{
return detail::integrate_const( stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
}
}
/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time , class Observer >
size_t integrate_const(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time end_time , Time dt ,
Observer observer
)
{
// we want to get as fast as possible to the end
if( boost::is_same< null_observer , Observer >::value )
{
return detail::integrate_adaptive(
stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
}
else
{
return detail::integrate_const( stepper , system , start_state ,
start_time , end_time , dt ,
observer , typename Stepper::stepper_category() );
}
}
/**
* \brief integrate_const without observer calls
*/
template< class Stepper , class System , class State , class Time >
size_t integrate_const(
Stepper stepper , System system , State &start_state ,
Time start_time , Time end_time , Time dt
)
{
return integrate_const( stepper , system , start_state , start_time , end_time , dt , null_observer() );
}
/**
* \brief Second version to solve the forwarding problem,
* can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time >
size_t integrate_const(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time end_time , Time dt
)
{
return integrate_const( stepper , system , start_state , start_time , end_time , dt , null_observer() );
}
/********* DOXYGEN *********/
/**
* \fn integrate_const( Stepper stepper , System system , State &start_state , Time start_time , Time end_time , Time dt , Observer observer )
* \brief Integrates the ODE with constant step size.
*
* Integrates the ODE defined by system using the given stepper.
* This method ensures that the observer is called at constant intervals dt.
* If the Stepper is a normal stepper without step size control, dt is also
* used for the numerical scheme. If a ControlledStepper is provided, the
* algorithm might reduce the step size to meet the error bounds, but it is
* ensured that the observer is always called at equidistant time points
* t0 + n*dt. If a DenseOutputStepper is used, the step size also may vary
* and the dense output is used to call the observer at equidistant time
* points.
*
* \param stepper The stepper to be used for numerical integration.
* \param system Function/Functor defining the rhs of the ODE.
* \param start_state The initial condition x0.
* \param start_time The initial time t0.
* \param end_time The final integration time tend.
* \param dt The time step between observer calls, _not_ necessarily the
* time step of the integration.
* \param observer Function/Functor called at equidistant time intervals.
* \return The number of steps performed.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_CONST_HPP_INCLUDED

124
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/integrate_n_steps.hpp Executable file
View File

@@ -0,0 +1,124 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/integrate_n_steps.hpp
[begin_description]
Integration of n steps with constant time size. Adaptive and dense-output methods are fully supported.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_N_STEPS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_N_STEPS_HPP_INCLUDED
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_n_steps.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* Integrates n steps
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class Stepper , class System , class State , class Time , class Observer>
Time integrate_n_steps(
Stepper stepper , System system , State &start_state ,
Time start_time , Time dt , size_t num_of_steps ,
Observer observer )
{
return detail::integrate_n_steps(
stepper , system , start_state ,
start_time , dt , num_of_steps ,
observer , typename Stepper::stepper_category() );
}
/**
* \brief Solves the forwarding problem, can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time , class Observer >
Time integrate_n_steps(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time dt , size_t num_of_steps ,
Observer observer )
{
return detail::integrate_n_steps(
stepper , system , start_state ,
start_time , dt , num_of_steps ,
observer , typename Stepper::stepper_category() );
}
/**
* \brief The same function as above, but without observer calls.
*/
template< class Stepper , class System , class State , class Time >
Time integrate_n_steps(
Stepper stepper , System system , State &start_state ,
Time start_time , Time dt , size_t num_of_steps )
{
return integrate_n_steps( stepper , system , start_state , start_time , dt , num_of_steps , null_observer() );
}
/**
* \brief Solves the forwarding problem, can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class Time >
Time integrate_n_steps(
Stepper stepper , System system , const State &start_state ,
Time start_time , Time dt , size_t num_of_steps )
{
return integrate_n_steps( stepper , system , start_state , start_time , dt , num_of_steps , null_observer() );
}
/************* DOXYGEN *************/
/**
* \fn Time integrate_n_steps( Stepper stepper , System system , State &start_state , Time start_time , Time dt , size_t num_of_steps , Observer observer )
* \brief Integrates the ODE with constant step size.
*
* This function is similar to integrate_const. The observer is called at
* equidistant time intervals t0 + n*dt.
* If the Stepper is a normal stepper without step size control, dt is also
* used for the numerical scheme. If a ControlledStepper is provided, the
* algorithm might reduce the step size to meet the error bounds, but it is
* ensured that the observer is always called at equidistant time points
* t0 + n*dt. If a DenseOutputStepper is used, the step size also may vary
* and the dense output is used to call the observer at equidistant time
* points. The final integration time is always t0 + num_of_steps*dt.
*
* \param stepper The stepper to be used for numerical integration.
* \param system Function/Functor defining the rhs of the ODE.
* \param start_state The initial condition x0.
* \param start_time The initial time t0.
* \param dt The time step between observer calls, _not_ necessarily the
* time step of the integration.
* \param num_of_steps Number of steps to be performed
* \param observer Function/Functor called at equidistant time intervals.
* \return The number of steps performed.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_N_STEPS_HPP_INCLUDED

131
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/integrate_times.hpp Executable file
View File

@@ -0,0 +1,131 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/integrate_times.hpp
[begin_description]
Integration of ODEs with observation at user defined points
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_TIMES_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_TIMES_HPP_INCLUDED
#include <boost/type_traits/is_same.hpp>
#include <boost/range.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/integrate/null_observer.hpp>
#include <boost/numeric/odeint/integrate/detail/integrate_times.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
TimeIterator times_start , TimeIterator times_end , Time dt ,
Observer observer )
{
return detail::integrate_times(
stepper , system , start_state ,
times_start , times_end , dt ,
observer , typename Stepper::stepper_category() );
}
/**
* \brief Solves the forwarding problem, can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class TimeIterator , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , const State &start_state ,
TimeIterator times_start , TimeIterator times_end , Time dt ,
Observer observer )
{
return detail::integrate_times(
stepper , system , start_state ,
times_start , times_end , dt ,
observer , typename Stepper::stepper_category() );
}
/**
* \brief The same function as above, but without observer calls.
*/
template< class Stepper , class System , class State , class TimeRange , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , State &start_state ,
const TimeRange &times , Time dt ,
Observer observer )
{
return integrate_times(
stepper , system , start_state ,
boost::begin( times ) , boost::end( times ) , dt , observer );
}
/**
* \brief Solves the forwarding problem, can be called with Boost.Range as start_state.
*/
template< class Stepper , class System , class State , class TimeRange , class Time , class Observer >
size_t integrate_times(
Stepper stepper , System system , const State &start_state ,
const TimeRange &times , Time dt ,
Observer observer )
{
return integrate_times(
stepper , system , start_state ,
boost::begin( times ) , boost::end( times ) , dt , observer );
}
/********* DOXYGEN ***********/
/**
* \fn size_t integrate_times( Stepper stepper , System system , State &start_state , TimeIterator times_start , TimeIterator times_end , Time dt , Observer observer )
* \brief Integrates the ODE with observer calls at given time points.
*
* Integrates the ODE given by system using the given stepper. This function
* does observer calls at the subsequent time points given by the range
* times_start, times_end. If the stepper has not step size control, the
* step size might be reduced occasionally to ensure observer calls exactly
* at the time points from the given sequence. If the stepper is a
* ControlledStepper, the step size is adjusted to meet the error bounds,
* but also might be reduced occasionally to ensure correct observer calls.
* If a DenseOutputStepper is provided, the dense output functionality is
* used to call the observer at the given times. The end time of the
* integration is always *(end_time-1).
*
* \param stepper The stepper to be used for numerical integration.
* \param system Function/Functor defining the rhs of the ODE.
* \param start_state The initial condition x0.
* \param times_start Iterator to the start time
* \param times_end Iterator to the end time
* \param dt The time step between observer calls, _not_ necessarily the
* time step of the integration.
* \param observer Function/Functor called at equidistant time intervals.
* \return The number of steps performed.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_INTEGRATE_TIMES_HPP_INCLUDED

38
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/null_observer.hpp Executable file
View File

@@ -0,0 +1,38 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/null_observer.hpp
[begin_description]
null_observer
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_NULL_OBSERVER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_NULL_OBSERVER_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
struct null_observer
{
template< class State , class Time >
void operator()( const State& /* x */ , Time /* t */ ) const
{
}
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_NULL_OBSERVER_HPP_INCLUDED

56
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/integrate/observer_collection.hpp Executable file
View File

@@ -0,0 +1,56 @@
/*
[auto_generated]
boost/numeric/odeint/integrate/observer_collection.hpp
[begin_description]
Collection of observers, which are all called during the evolution of the ODE.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_INTEGRATE_OBSERVER_COLLECTION_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_INTEGRATE_OBSERVER_COLLECTION_HPP_INCLUDED
#include <vector>
#include <boost/function.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class State , class Time >
class observer_collection
{
public:
typedef boost::function< void( const State& , const Time& ) > observer_type;
typedef std::vector< observer_type > collection_type;
void operator()( const State& x , Time t )
{
for( size_t i=0 ; i<m_observers.size() ; ++i )
m_observers[i]( x , t );
}
collection_type& observers( void ) { return m_observers; }
const collection_type& observers( void ) const { return m_observers; }
private:
collection_type m_observers;
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_INTEGRATE_OBSERVER_COLLECTION_HPP_INCLUDED

416
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/adams_bashforth.hpp Executable file
View File

@@ -0,0 +1,416 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/adams_bashforth.hpp
[begin_description]
Implementaton of the Adam-Bashforth method a multistep method used for the predictor step in the
Adams-Bashforth-Moulton method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED
#include <boost/static_assert.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_bashforth_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
size_t Steps ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer ,
class InitializingStepper = runge_kutta4< State , Value , Deriv , Time , Algebra , Operations, Resizer >
>
class adams_bashforth : public algebra_stepper_base< Algebra , Operations >
{
#ifndef DOXYGEN_SKIP
BOOST_STATIC_ASSERT(( Steps > 0 ));
BOOST_STATIC_ASSERT(( Steps < 9 ));
#endif
public :
typedef State state_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef Time time_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
typedef InitializingStepper initializing_stepper_type;
typedef typename algebra_stepper_base< Algebra , Operations >::algebra_type algebra_type;
typedef typename algebra_stepper_base< Algebra , Operations >::operations_type operations_type;
#ifndef DOXYGEN_SKIP
typedef adams_bashforth< Steps , State , Value , Deriv , Time , Algebra , Operations , Resizer , InitializingStepper > stepper_type;
#endif
static const size_t steps = Steps;
typedef unsigned short order_type;
static const order_type order_value = steps;
typedef detail::rotating_buffer< wrapped_deriv_type , steps > step_storage_type;
order_type order( void ) const { return order_value; }
adams_bashforth( const algebra_type &algebra = algebra_type() )
: m_step_storage() , m_resizer() , m_coefficients() ,
m_steps_initialized( 0 ) , m_initializing_stepper() ,
m_algebra( algebra )
{ }
adams_bashforth( const adams_bashforth &stepper )
: m_step_storage( stepper.m_step_storage ) , m_resizer( stepper.m_resizer ) , m_coefficients() ,
m_steps_initialized( stepper.m_steps_initialized ) , m_initializing_stepper( stepper.m_initializing_stepper ) ,
m_algebra( stepper.m_algebra )
{ }
adams_bashforth& operator=( const adams_bashforth &stepper )
{
m_resizer = stepper.m_resizer;
m_step_storage = stepper.m_step_storage;
m_algebra = stepper.m_algebra;
return *this;
}
/*
* Version 1 : do_step( system , x , t , dt );
*
* solves the forwarding problem
*/
template< class System , class StateInOut >
void do_step( System system , StateInOut &x , time_type t , time_type dt )
{
do_step( system , x , t , x , dt );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &x , time_type t , time_type dt )
{
do_step( system , x , t , x , dt );
}
/*
* Version 2 : do_step( system , in , t , out , dt );
*
* solves the forwarding problem
*/
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
do_step_impl( system , in , t , out , dt );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateOut.
*/
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , const StateOut &out , time_type dt )
{
do_step_impl( system , in , t , out , dt );
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
}
const step_storage_type& step_storage( void ) const
{
return m_step_storage;
}
step_storage_type& step_storage( void )
{
return m_step_storage;
}
template< class ExplicitStepper , class System , class StateIn >
void initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
{
typename odeint::unwrap_reference< ExplicitStepper >::type &stepper = explicit_stepper;
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
for( size_t i=0 ; i<steps-1 ; ++i )
{
if( i != 0 ) m_step_storage.rotate();
sys( x , m_step_storage[0].m_v , t );
stepper.do_step( system , x , m_step_storage[0].m_v , t , dt );
t += dt;
}
m_steps_initialized = steps;
}
template< class System , class StateIn >
void initialize( System system , StateIn &x , time_type &t , time_type dt )
{
initialize( detail::ref( m_initializing_stepper ) , system , x , t , dt );
}
void reset( void )
{
m_steps_initialized = 0;
}
bool is_initialized( void ) const
{
return m_steps_initialized >= steps;
}
const initializing_stepper_type& initializing_stepper( void ) const { return m_initializing_stepper; }
initializing_stepper_type& initializing_stepper( void ) { return m_initializing_stepper; }
private:
template< class System , class StateIn , class StateOut >
void do_step_impl( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
if( m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) ) )
{
m_steps_initialized = 0;
}
if( m_steps_initialized < steps - 1 )
{
if( m_steps_initialized != 0 ) m_step_storage.rotate();
sys( in , m_step_storage[0].m_v , t );
m_initializing_stepper.do_step( system , in , m_step_storage[0].m_v , t , out , dt );
m_steps_initialized++;
}
else
{
m_step_storage.rotate();
sys( in , m_step_storage[0].m_v , t );
detail::adams_bashforth_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_step_storage , m_coefficients , dt );
}
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
for( size_t i=0 ; i<steps ; ++i )
{
resized |= adjust_size_by_resizeability( m_step_storage[i] , x , typename is_resizeable<deriv_type>::type() );
}
return resized;
}
step_storage_type m_step_storage;
resizer_type m_resizer;
const detail::adams_bashforth_coefficients< value_type , steps > m_coefficients;
size_t m_steps_initialized;
initializing_stepper_type m_initializing_stepper;
protected:
algebra_type m_algebra;
};
/***** DOXYGEN *****/
/**
* \class adams_bashforth
* \brief The Adams-Bashforth multistep algorithm.
*
* The Adams-Bashforth method is a multi-step algorithm with configurable step
* number. The step number is specified as template parameter Steps and it
* then uses the result from the previous Steps steps. See also
* <a href="http://en.wikipedia.org/wiki/Linear_multistep_method">en.wikipedia.org/wiki/Linear_multistep_method</a>.
* Currently, a maximum of Steps=8 is supported.
* The method is explicit and fulfills the Stepper concept. Step size control
* or continuous output are not provided.
*
* This class derives from algebra_base and inherits its interface via
* CRTP (current recurring template pattern). For more details see
* algebra_stepper_base.
*
* \tparam Steps The number of steps (maximal 8).
* \tparam State The state type.
* \tparam Value The value type.
* \tparam Deriv The type representing the time derivative of the state.
* \tparam Time The time representing the independent variable - the time.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
* \tparam InitializingStepper The stepper for the first two steps.
*/
/**
* \fn adams_bashforth::adams_bashforth( const algebra_type &algebra )
* \brief Constructs the adams_bashforth class. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored.
*/
/**
* \fn order_type adams_bashforth::order( void ) const
* \brief Returns the order of the algorithm, which is equal to the number of steps.
* \return order of the method.
*/
/**
* \fn void adams_bashforth::do_step( System system , StateInOut &x , time_type t , time_type dt )
* \brief This method performs one step. It transforms the result in-place.
*
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn void adams_bashforth::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn void adams_bashforth::adjust_size( const StateType &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
/**
* \fn const step_storage_type& adams_bashforth::step_storage( void ) const
* \brief Returns the storage of intermediate results.
* \return The storage of intermediate results.
*/
/**
* \fn step_storage_type& adams_bashforth::step_storage( void )
* \brief Returns the storage of intermediate results.
* \return The storage of intermediate results.
*/
/**
* \fn void adams_bashforth::initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
* \brief Initialized the stepper. Does Steps-1 steps with the explicit_stepper to fill the buffer.
* \param explicit_stepper the stepper used to fill the buffer of previous step results
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn void adams_bashforth::initialize( System system , StateIn &x , time_type &t , time_type dt )
* \brief Initialized the stepper. Does Steps-1 steps with an internal instance of InitializingStepper to fill the buffer.
* \note The state x and time t are updated to the values after Steps-1 initial steps.
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The initial state of the ODE which should be solved, updated in this method.
* \param t The initial value of the time, updated in this method.
* \param dt The step size.
*/
/**
* \fn void adams_bashforth::reset( void )
* \brief Resets the internal buffer of the stepper.
*/
/**
* \fn bool adams_bashforth::is_initialized( void ) const
* \brief Returns true if the stepper has been initialized.
* \return bool true if stepper is initialized, false otherwise
*/
/**
* \fn const initializing_stepper_type& adams_bashforth::initializing_stepper( void ) const
* \brief Returns the internal initializing stepper instance.
* \return initializing_stepper
*/
/**
* \fn const initializing_stepper_type& adams_bashforth::initializing_stepper( void ) const
* \brief Returns the internal initializing stepper instance.
* \return initializing_stepper
*/
/**
* \fn initializing_stepper_type& adams_bashforth::initializing_stepper( void )
* \brief Returns the internal initializing stepper instance.
* \return initializing_stepper
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_HPP_INCLUDED

258
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp Executable file
View File

@@ -0,0 +1,258 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp
[begin_description]
Implementation of the Adams-Bashforth-Moulton method, a predictor-corrector multistep method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_MOULTON_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_MOULTON_HPP_INCLUDED
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/adams_bashforth.hpp>
#include <boost/numeric/odeint/stepper/adams_moulton.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
size_t Steps ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class adams_bashforth_moulton
{
#ifndef DOXYGEN_SKIP
BOOST_STATIC_ASSERT(( Steps > 0 ));
BOOST_STATIC_ASSERT(( Steps < 9 ));
#endif
public :
typedef State state_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef Time time_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
static const size_t steps = Steps;
#ifndef DOXYGEN_SKIP
typedef adams_bashforth< steps , state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > adams_bashforth_type;
typedef adams_moulton< steps , state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > adams_moulton_type;
#endif //DOXYGEN_SKIP
typedef unsigned short order_type;
static const order_type order_value = steps + 1;
/** \brief Constructs the adams_bashforth class. */
adams_bashforth_moulton( void )
: m_adams_bashforth() , m_adams_moulton( m_adams_bashforth.algebra() )
{ }
adams_bashforth_moulton( const algebra_type &algebra )
: m_adams_bashforth( algebra ) , m_adams_moulton( m_adams_bashforth.algebra() )
{ }
order_type order( void ) const { return order_value; }
template< class System , class StateInOut >
void do_step( System system , StateInOut &x , time_type t , time_type dt )
{
m_adams_bashforth.do_step( system , x , t , dt );
m_adams_moulton.do_step( system , x , t , dt , m_adams_bashforth.step_storage() );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &x , time_type t , time_type dt )
{
m_adams_bashforth.do_step( system , x , t , dt );
m_adams_moulton.do_step( system , x , t , dt , m_adams_bashforth.step_storage() );
}
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , const StateOut &out , time_type dt )
{
m_adams_bashforth.do_step( system , in , t , out , dt );
m_adams_moulton.do_step( system , out , t , dt , m_adams_bashforth.step_storage() );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateOut.
*/
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
m_adams_bashforth.do_step( system , in , t , out , dt );
m_adams_moulton.do_step( system , out , t , dt , m_adams_bashforth.step_storage() );
}
template< class StateType >
void adjust_size( const StateType &x )
{
m_adams_bashforth.adjust_size( x );
m_adams_moulton.adjust_size( x );
}
template< class ExplicitStepper , class System , class StateIn >
void initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
{
m_adams_bashforth.initialize( explicit_stepper , system , x , t , dt );
}
template< class System , class StateIn >
void initialize( System system , StateIn &x , time_type &t , time_type dt )
{
m_adams_bashforth.initialize( system , x , t , dt );
}
private:
adams_bashforth_type m_adams_bashforth;
adams_moulton_type m_adams_moulton;
};
/********* DOXYGEN ********/
/**
* \class adams_bashforth_moulton
* \brief The Adams-Bashforth-Moulton multistep algorithm.
*
* The Adams-Bashforth method is a multi-step predictor-corrector algorithm
* with configurable step number. The step number is specified as template
* parameter Steps and it then uses the result from the previous Steps steps.
* See also
* <a href="http://en.wikipedia.org/wiki/Linear_multistep_method">en.wikipedia.org/wiki/Linear_multistep_method</a>.
* Currently, a maximum of Steps=8 is supported.
* The method is explicit and fulfills the Stepper concept. Step size control
* or continuous output are not provided.
*
* This class derives from algebra_base and inherits its interface via
* CRTP (current recurring template pattern). For more details see
* algebra_stepper_base.
*
* \tparam Steps The number of steps (maximal 8).
* \tparam State The state type.
* \tparam Value The value type.
* \tparam Deriv The type representing the time derivative of the state.
* \tparam Time The time representing the independent variable - the time.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
* \tparam InitializingStepper The stepper for the first two steps.
*/
/**
* \fn adams_bashforth_moulton::adams_bashforth_moulton( const algebra_type &algebra )
* \brief Constructs the adams_bashforth class. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored.
*/
/**
* \fn adams_bashforth_moulton::order( void ) const
* \brief Returns the order of the algorithm, which is equal to the number of steps+1.
* \return order of the method.
*/
/**
* \fn adams_bashforth_moulton::do_step( System system , StateInOut &x , time_type t , time_type dt )
* \brief This method performs one step. It transforms the result in-place.
*
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn adams_bashforth_moulton::do_step( System system , const StateIn &in , time_type t , const StateOut &out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn adams_bashforth_moulton::adjust_size( const StateType &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
/**
* \fn adams_bashforth_moulton::initialize( ExplicitStepper explicit_stepper , System system , StateIn &x , time_type &t , time_type dt )
* \brief Initialized the stepper. Does Steps-1 steps with the explicit_stepper to fill the buffer.
* \note The state x and time t are updated to the values after Steps-1 initial steps.
* \param explicit_stepper the stepper used to fill the buffer of previous step results
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The initial state of the ODE which should be solved, updated after in this method.
* \param t The initial time, updated in this method.
* \param dt The step size.
*/
/**
* \fn adams_bashforth_moulton::initialize( System system , StateIn &x , time_type &t , time_type dt )
* \brief Initialized the stepper. Does Steps-1 steps using the standard initializing stepper
* of the underlying adams_bashforth stepper.
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_BASHFORTH_MOULTON_HPP_INCLUDED

195
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/adams_moulton.hpp Executable file
View File

@@ -0,0 +1,195 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/adams_moulton.hpp
[begin_description]
Implementation of the Adams-Moulton method. This is method is not a real stepper, it is more a helper class
which computes the corrector step in the Adams-Bashforth-Moulton method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4_classic.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_moulton_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp>
#include <boost/numeric/odeint/stepper/detail/rotating_buffer.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* Static implicit Adams-Moulton multistep-solver without step size control and without dense output.
*/
template<
size_t Steps ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class adams_moulton
{
private:
public :
typedef State state_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef Time time_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
typedef adams_moulton< Steps , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;
static const size_t steps = Steps;
typedef unsigned short order_type;
static const order_type order_value = steps + 1;
typedef detail::rotating_buffer< wrapped_deriv_type , steps > step_storage_type;
adams_moulton( )
: m_coefficients() , m_dxdt() , m_resizer() ,
m_algebra_instance() , m_algebra( m_algebra_instance )
{ }
adams_moulton( algebra_type &algebra )
: m_coefficients() , m_dxdt() , m_resizer() ,
m_algebra_instance() , m_algebra( algebra )
{ }
adams_moulton& operator=( const adams_moulton &stepper )
{
m_dxdt = stepper.m_dxdt;
m_resizer = stepper.m_resizer;
m_algebra = stepper.m_algebra;
return *this;
}
order_type order( void ) const { return order_value; }
/*
* Version 1 : do_step( system , x , t , dt , buf );
*
* solves the forwarding problem
*/
template< class System , class StateInOut , class ABBuf >
void do_step( System system , StateInOut &in , time_type t , time_type dt , const ABBuf &buf )
{
do_step( system , in , t , in , dt , buf );
}
template< class System , class StateInOut , class ABBuf >
void do_step( System system , const StateInOut &in , time_type t , time_type dt , const ABBuf &buf )
{
do_step( system , in , t , in , dt , buf );
}
/*
* Version 2 : do_step( system , in , t , out , dt , buf );
*
* solves the forwarding problem
*/
template< class System , class StateIn , class StateOut , class ABBuf >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , const ABBuf &buf )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v , t );
detail::adams_moulton_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_dxdt.m_v , buf , m_coefficients , dt );
}
template< class System , class StateIn , class StateOut , class ABBuf >
void do_step( System system , const StateIn &in , time_type t , const StateOut &out , time_type dt , const ABBuf &buf )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v , t );
detail::adams_moulton_call_algebra< steps , algebra_type , operations_type >()( m_algebra , in , out , m_dxdt.m_v , buf , m_coefficients , dt );
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
}
algebra_type& algebra()
{ return m_algebra; }
const algebra_type& algebra() const
{ return m_algebra; }
private:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
const detail::adams_moulton_coefficients< value_type , steps > m_coefficients;
wrapped_deriv_type m_dxdt;
resizer_type m_resizer;
protected:
algebra_type m_algebra_instance;
algebra_type &m_algebra;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ADAMS_MOULTON_HPP_INCLUDED

91
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp Executable file
View File

@@ -0,0 +1,91 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp
[begin_description]
Base class for all steppers with an algebra and operations.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BASE_ALGEBRA_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_ALGEBRA_STEPPER_BASE_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
template< class Algebra , class Operations >
class algebra_stepper_base
{
public:
typedef Algebra algebra_type;
typedef Operations operations_type;
algebra_stepper_base( const algebra_type &algebra = algebra_type() )
: m_algebra( algebra ) { }
algebra_type& algebra()
{
return m_algebra;
}
const algebra_type& algebra() const
{
return m_algebra;
}
protected:
algebra_type m_algebra;
};
/******* DOXYGEN *******/
/**
* \class algebra_stepper_base
* \brief Base class for all steppers with algebra and operations.
*
* This class serves a base class for all steppers with algebra and operations. It holds the
* algebra and provides access to the algebra. The operations are not instantiated, since they are
* static classes inside the operations class.
*
* \tparam Algebra The type of the algebra. Must fulfill the Algebra Concept, at least partially to work
* with the stepper.
* \tparam Operations The type of the operations. Must fulfill the Operations Concept, at least partially
* to work with the stepper.
*/
/**
* \fn algebra_stepper_base::algebra_stepper_base( const algebra_type &algebra = algebra_type() )
* \brief Constructs a algebra_stepper_base and creates the algebra. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra The algebra_stepper_base stores and uses a copy of algebra.
*/
/**
* \fn algebra_type& algebra_stepper_base::algebra()
* \return A reference to the algebra which is held by this class.
*/
/**
* \fn const algebra_type& algebra_stepper_base::algebra() const
* \return A const reference to the algebra which is held by this class.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_ALGEBRA_STEPPER_BASE_HPP_INCLUDED

560
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/base/explicit_error_stepper_base.hpp Executable file
View File

@@ -0,0 +1,560 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/base/explicit_error_stepper_base.hpp
[begin_description]
Base class for all explicit Runge Kutta stepper which are also error steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_BASE_HPP_INCLUDED
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* base class for explicit stepper and error steppers
* models the stepper AND the error stepper concept
*
* this class provides the following do_step variants:
* do_step( sys , x , t , dt )
* do_step( sys , x , dxdt , t , dt )
* do_step( sys , in , t , out , dt )
* do_step( sys , in , dxdt , t , out , dt )
* do_step( sys , x , t , dt , xerr )
* do_step( sys , x , dxdt , t , dt , xerr )
* do_step( sys , in , t , out , dt , xerr )
* do_step( sys , in , dxdt , t , out , dt , xerr )
*/
template<
class Stepper ,
unsigned short Order ,
unsigned short StepperOrder ,
unsigned short ErrorOrder ,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class explicit_error_stepper_base : public algebra_stepper_base< Algebra , Operations >
{
public:
typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
typedef typename algebra_stepper_base_type::algebra_type algebra_type;
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Resizer resizer_type;
typedef Stepper stepper_type;
typedef explicit_error_stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef explicit_error_stepper_base< Stepper , Order , StepperOrder , ErrorOrder ,
State , Value , Deriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
#endif
typedef unsigned short order_type;
static const order_type order_value = Order;
static const order_type stepper_order_value = StepperOrder;
static const order_type error_order_value = ErrorOrder;
explicit_error_stepper_base( const algebra_type &algebra = algebra_type() )
: algebra_stepper_base_type( algebra )
{ }
order_type order( void ) const
{
return order_value;
}
order_type stepper_order( void ) const
{
return stepper_order_value;
}
order_type error_order( void ) const
{
return error_order_value;
}
/*
* Version 1 : do_step( sys , x , t , dt )
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class StateInOut >
void do_step( System system , StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/*
* Version 2 : do_step( sys , x , dxdt , t , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateInOut , class DerivIn >
typename boost::disable_if< boost::is_same< DerivIn , time_type > , void >::type
do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )
{
this->stepper().do_step_impl( system , x , dxdt , t , x , dt );
}
/*
* Version 3 : do_step( sys , in , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateIn , class StateOut >
typename boost::disable_if< boost::is_same< StateIn , time_type > , void >::type
do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , dt );
}
/*
* Version 4 :do_step( sys , in , dxdt , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateIn , class DerivIn , class StateOut >
typename boost::disable_if< boost::is_same< DerivIn , time_type > , void >::type
do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
{
this->stepper().do_step_impl( system , in , dxdt , t , out , dt );
}
/*
* Version 5 :do_step( sys , x , t , dt , xerr )
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class StateInOut , class Err >
void do_step( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
{
do_step_v5( system , x , t , dt , xerr );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut , class Err >
void do_step( System system , const StateInOut &x , time_type t , time_type dt , Err &xerr )
{
do_step_v5( system , x , t , dt , xerr );
}
/*
* Version 6 :do_step( sys , x , dxdt , t , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateInOut , class DerivIn , class Err >
typename boost::disable_if< boost::is_same< DerivIn , time_type > , void >::type
do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt , Err &xerr )
{
this->stepper().do_step_impl( system , x , dxdt , t , x , dt , xerr );
}
/*
* Version 7 : do_step( sys , in , t , out , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class StateOut , class Err >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , Err &xerr )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , dt , xerr );
}
/*
* Version 8 : do_step( sys , in , dxdt , t , out , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class DerivIn , class StateOut , class Err >
void do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
{
this->stepper().do_step_impl( system , in , dxdt , t , out , dt , xerr );
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
}
private:
template< class System , class StateInOut >
void do_step_v1( System system , StateInOut &x , time_type t , time_type dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl<StateInOut> , detail::ref( *this ) , detail::_1 ) );
sys( x , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , dt );
}
template< class System , class StateInOut , class Err >
void do_step_v5( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl<StateInOut> , detail::ref( *this ) , detail::_1 ) );
sys( x , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , dt , xerr );
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
stepper_type& stepper( void )
{
return *static_cast< stepper_type* >( this );
}
const stepper_type& stepper( void ) const
{
return *static_cast< const stepper_type* >( this );
}
resizer_type m_resizer;
protected:
wrapped_deriv_type m_dxdt;
};
/******** DOXYGEN *******/
/**
* \class explicit_error_stepper_base
* \brief Base class for explicit steppers with error estimation. This class can used with
* controlled steppers for step size control.
*
* This class serves as the base class for all explicit steppers with algebra and operations. In contrast to
* explicit_stepper_base it also estimates the error and can be used in a controlled stepper to provide
* step size control.
*
* \note This stepper provides `do_step` methods with and without error estimation. It has therefore three orders,
* one for the order of a step if the error is not estimated. The other two orders are the orders of the step and
* the error step if the error estimation is performed.
*
* explicit_error_stepper_base is used as the interface in a CRTP (currently recurring template
* pattern). In order to work correctly the parent class needs to have a method
* `do_step_impl( system , in , dxdt_in , t , out , dt , xerr )`.
* explicit_error_stepper_base derives from algebra_stepper_base.
*
* explicit_error_stepper_base provides several overloaded `do_step` methods, see the list below. Only two of them
* are needed to fulfill the Error Stepper concept. The other ones are for convenience and for performance. Some
* of them simply update the state out-of-place, while other expect that the first derivative at `t` is passed to the
* stepper.
*
* - `do_step( sys , x , t , dt )` - The classical `do_step` method needed to fulfill the Error Stepper concept. The
* state is updated in-place. A type modelling a Boost.Range can be used for x.
* - `do_step( sys , x , dxdt , t , dt )` - This method updates the state in-place, but the derivative at the point `t`
* must be explicitly passed in `dxdt`.
* - `do_step( sys , in , t , out , dt )` - This method updates the state out-of-place, hence the result of the step
* is stored in `out`.
* - `do_step( sys , in , dxdt , t , out , dt )` - This method update the state out-of-place and expects that the
* derivative at the point `t` is explicitly passed in `dxdt`. It is a combination of the two `do_step` methods
* above.
* - `do_step( sys , x , t , dt , xerr )` - This `do_step` method is needed to fulfill the Error Stepper concept. The
* state is updated in-place and an error estimate is calculated. A type modelling a Boost.Range can be used for x.
* - `do_step( sys , x , dxdt , t , dt , xerr )` - This method updates the state in-place, but the derivative at the
* point `t` must be passed in `dxdt`. An error estimate is calculated.
* - `do_step( sys , in , t , out , dt , xerr )` - This method updates the state out-of-place and estimates the error
* during the step.
* - `do_step( sys , in , dxdt , t , out , dt , xerr )` - This methods updates the state out-of-place and estimates
* the error during the step. Furthermore, the derivative at `t` must be passed in `dxdt`.
*
* \note The system is always passed as value, which might result in poor performance if it contains data. In this
* case it can be used with `boost::ref` or `std::ref`, for example `stepper.do_step( boost::ref( sys ) , x , t , dt );`
*
* \note The time `t` is not advanced by the stepper. This has to done manually, or by the appropriate `integrate`
* routines or `iterator`s.
*
* \tparam Stepper The stepper on which this class should work. It is used via CRTP, hence explicit_stepper_base
* provides the interface for the Stepper.
* \tparam Order The order of a stepper if the stepper is used without error estimation.
* \tparam StepperOrder The order of a step if the stepper is used with error estimation. Usually Order and StepperOrder have
* the same value.
* \tparam ErrorOrder The order of the error step if the stepper is used with error estimation.
* \tparam State The state type for the stepper.
* \tparam Value The value type for the stepper. This should be a floating point type, like float,
* double, or a multiprecision type. It must not necessary be the value_type of the State. For example
* the State can be a `vector< complex< double > >` in this case the Value must be double.
* The default value is double.
* \tparam Deriv The type representing time derivatives of the state type. It is usually the same type as the
* state type, only if used with Boost.Units both types differ.
* \tparam Time The type representing the time. Usually the same type as the value type. When Boost.Units is
* used, this type has usually a unit.
* \tparam Algebra The algebra type which must fulfill the Algebra Concept.
* \tparam Operations The type for the operations which must fulfill the Operations Concept.
* \tparam Resizer The resizer policy class.
*/
/**
* \fn explicit_error_stepper_base::explicit_error_stepper_base( const algebra_type &algebra = algebra_type() )
*
* \brief Constructs a explicit_error_stepper_base class. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn explicit_error_stepper_base::order( void ) const
* \return Returns the order of the stepper if it used without error estimation.
*/
/**
* \fn explicit_error_stepper_base::stepper_order( void ) const
* \return Returns the order of a step if the stepper is used without error estimation.
*/
/**
* \fn explicit_error_stepper_base::error_order( void ) const
* \return Returns the order of an error step if the stepper is used without error estimation.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , StateInOut &x , time_type t , time_type dt )
* \brief This method performs one step. It transforms the result in-place.
*
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. Additionally to the other method
* the derivative of x is also passed to this method. It is supposed to be used in the following way:
*
* \code
* sys( x , dxdt , t );
* stepper.do_step( sys , x , dxdt , t , dt );
* \endcode
*
* The result is updated in place in x. This method is disabled if Time and Deriv are of the same type. In this
* case the method could not be distinguished from other `do_step` versions.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* This method is disabled if StateIn and Time are the same type. In this case the method can not be distinguished from
* other `do_step` variants.
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the derivative of x at t is passed to the stepper. It is supposed to be used in the following way:
*
* \code
* sys( in , dxdt , t );
* stepper.do_step( sys , in , dxdt , t , out , dt );
* \endcode
*
* This method is disabled if DerivIn and Time are of same type.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper and estimates the error. The state of the ODE
* is updated in-place.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. x is updated by this method.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
* \param xerr The estimation of the error is stored in xerr.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. Additionally to the other method
* the derivative of x is also passed to this method. It is supposed to be used in the following way:
*
* \code
* sys( x , dxdt , t );
* stepper.do_step( sys , x , dxdt , t , dt , xerr );
* \endcode
*
* The result is updated in place in x. This method is disabled if Time and DerivIn are of the same type. In this
* case the method could not be distinguished from other `do_step` versions.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
* \param xerr The error estimate is stored in xerr.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the error is estimated.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
* \param xerr The error estimate.
*/
/**
* \fn explicit_error_stepper_base::do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the derivative of x at t is passed to the stepper and the error is estimated. It is supposed to be used in the following way:
*
* \code
* sys( in , dxdt , t );
* stepper.do_step( sys , in , dxdt , t , out , dt );
* \endcode
*
* This method is disabled if DerivIn and Time are of same type.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
* \param xerr The error estimate.
*/
/**
* \fn explicit_error_stepper_base::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_BASE_HPP_INCLUDED

658
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/base/explicit_error_stepper_fsal_base.hpp Executable file
View File

@@ -0,0 +1,658 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/base/explicit_error_stepper_fsal_base.hpp
[begin_description]
Base class for all explicit first-same-as-last Runge Kutta steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_FSAL_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_FSAL_BASE_HPP_INCLUDED
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* base class for explicit stepper and error steppers with the fsal property
* models the stepper AND the error stepper fsal concept
*
* this class provides the following do_step overloads
* do_step( sys , x , t , dt )
* do_step( sys , x , dxdt , t , dt )
* do_step( sys , in , t , out , dt )
* do_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
* do_step( sys , x , t , dt , xerr )
* do_step( sys , x , dxdt , t , dt , xerr )
* do_step( sys , in , t , out , dt , xerr )
* do_step( sys , in , dxdt_in , t , out , dxdt_out , dt , xerr )
*/
template<
class Stepper ,
unsigned short Order ,
unsigned short StepperOrder ,
unsigned short ErrorOrder ,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class explicit_error_stepper_fsal_base : public algebra_stepper_base< Algebra , Operations >
{
public:
typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
typedef typename algebra_stepper_base_type::algebra_type algebra_type;
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Resizer resizer_type;
typedef Stepper stepper_type;
typedef explicit_error_stepper_fsal_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef explicit_error_stepper_fsal_base< Stepper , Order , StepperOrder , ErrorOrder ,
State , Value , Deriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
#endif
typedef unsigned short order_type;
static const order_type order_value = Order;
static const order_type stepper_order_value = StepperOrder;
static const order_type error_order_value = ErrorOrder;
explicit_error_stepper_fsal_base( const algebra_type &algebra = algebra_type() )
: algebra_stepper_base_type( algebra ) , m_first_call( true )
{ }
order_type order( void ) const
{
return order_value;
}
order_type stepper_order( void ) const
{
return stepper_order_value;
}
order_type error_order( void ) const
{
return error_order_value;
}
/*
* version 1 : do_step( sys , x , t , dt )
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class StateInOut >
void do_step( System system , StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/*
* version 2 : do_step( sys , x , dxdt , t , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateInOut , class DerivInOut >
typename boost::disable_if< boost::is_same< StateInOut , time_type > , void >::type
do_step( System system , StateInOut &x , DerivInOut &dxdt , time_type t , time_type dt )
{
m_first_call = true;
this->stepper().do_step_impl( system , x , dxdt , t , x , dxdt , dt );
}
/*
* version 3 : do_step( sys , in , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateIn , class StateOut >
typename boost::disable_if< boost::is_same< StateIn , time_type > , void >::type
do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
if( m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , in , t );
}
this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , m_dxdt.m_v , dt );
}
/*
* version 4 : do_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
void do_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
StateOut &out , DerivOut &dxdt_out , time_type dt )
{
m_first_call = true;
this->stepper().do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt );
}
/*
* version 5 : do_step( sys , x , t , dt , xerr )
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class StateInOut , class Err >
void do_step( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
{
do_step_v5( system , x , t , dt , xerr );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut , class Err >
void do_step( System system , const StateInOut &x , time_type t , time_type dt , Err &xerr )
{
do_step_v5( system , x , t , dt , xerr );
}
/*
* version 6 : do_step( sys , x , dxdt , t , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateInOut , class DerivInOut , class Err >
typename boost::disable_if< boost::is_same< StateInOut , time_type > , void >::type
do_step( System system , StateInOut &x , DerivInOut &dxdt , time_type t , time_type dt , Err &xerr )
{
m_first_call = true;
this->stepper().do_step_impl( system , x , dxdt , t , x , dxdt , dt , xerr );
}
/*
* version 7 : do_step( sys , in , t , out , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class StateOut , class Err >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , Err &xerr )
{
if( m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , in , t );
}
this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , m_dxdt.m_v , dt , xerr );
}
/*
* version 8 : do_step( sys , in , dxdt_in , t , out , dxdt_out , dt , xerr )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut , class Err >
void do_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t ,
StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
{
m_first_call = true;
this->stepper().do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt , xerr );
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
}
void reset( void )
{
m_first_call = true;
}
template< class DerivIn >
void initialize( const DerivIn &deriv )
{
boost::numeric::odeint::copy( deriv , m_dxdt.m_v );
m_first_call = false;
}
template< class System , class StateIn >
void initialize( System system , const StateIn &x , time_type t )
{
typename odeint::unwrap_reference< System >::type &sys = system;
sys( x , m_dxdt.m_v , t );
m_first_call = false;
}
bool is_initialized( void ) const
{
return ! m_first_call;
}
private:
template< class System , class StateInOut >
void do_step_v1( System system , StateInOut &x , time_type t , time_type dt )
{
if( m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , x , t );
}
this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , m_dxdt.m_v , dt );
}
template< class System , class StateInOut , class Err >
void do_step_v5( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
{
if( m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , x , t );
}
this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , m_dxdt.m_v , dt , xerr );
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
stepper_type& stepper( void )
{
return *static_cast< stepper_type* >( this );
}
const stepper_type& stepper( void ) const
{
return *static_cast< const stepper_type* >( this );
}
resizer_type m_resizer;
bool m_first_call;
protected:
wrapped_deriv_type m_dxdt;
};
/******* DOXYGEN *******/
/**
* \class explicit_error_stepper_fsal_base
* \brief Base class for explicit steppers with error estimation and stepper fulfilling the FSAL (first-same-as-last)
* property. This class can be used with controlled steppers for step size control.
*
* This class serves as the base class for all explicit steppers with algebra and operations and which fulfill the FSAL
* property. In contrast to explicit_stepper_base it also estimates the error and can be used in a controlled stepper
* to provide step size control.
*
* The FSAL property means that the derivative of the system at t+dt is already used in the current step going from
* t to t +dt. Therefore, some more do_steps method can be introduced and the controlled steppers can explicitly make use
* of this property.
*
* \note This stepper provides `do_step` methods with and without error estimation. It has therefore three orders,
* one for the order of a step if the error is not estimated. The other two orders are the orders of the step and
* the error step if the error estimation is performed.
*
* explicit_error_stepper_fsal_base is used as the interface in a CRTP (currently recurring template
* pattern). In order to work correctly the parent class needs to have a method
* `do_step_impl( system , in , dxdt_in , t , out , dxdt_out , dt , xerr )`.
* explicit_error_stepper_fsal_base derives from algebra_stepper_base.
*
* This class can have an intrinsic state depending on the explicit usage of the `do_step` method. This means that some
* `do_step` methods are expected to be called in order. For example the `do_step( sys , x , t , dt , xerr )` will keep track
* of the derivative of `x` which is the internal state. The first call of this method is recognized such that one
* does not explicitly initialize the internal state, so it is safe to use this method like
*
* \code
* stepper_type stepper;
* stepper.do_step( sys , x , t , dt , xerr );
* stepper.do_step( sys , x , t , dt , xerr );
* stepper.do_step( sys , x , t , dt , xerr );
* \endcode
*
* But it is unsafe to call this method with different system functions after each other. Do do so, one must initialize the
* internal state with the `initialize` method or reset the internal state with the `reset` method.
*
* explicit_error_stepper_fsal_base provides several overloaded `do_step` methods, see the list below. Only two of them are needed
* to fulfill the Error Stepper concept. The other ones are for convenience and for better performance. Some of them
* simply update the state out-of-place, while other expect that the first derivative at `t` is passed to the stepper.
*
* - `do_step( sys , x , t , dt )` - The classical `do_step` method needed to fulfill the Error Stepper concept. The
* state is updated in-place. A type modelling a Boost.Range can be used for x.
* - `do_step( sys , x , dxdt , t , dt )` - This method updates the state x and the derivative dxdt in-place. It is expected
* that dxdt has the value of the derivative of x at time t.
* - `do_step( sys , in , t , out , dt )` - This method updates the state out-of-place, hence the result of the step
* is stored in `out`.
* - `do_step( sys , in , dxdt_in , t , out , dxdt_out , dt )` - This method updates the state and the derivative
* out-of-place. It expects that the derivative at the point `t` is explicitly passed in `dxdt_in`.
* - `do_step( sys , x , t , dt , xerr )` - This `do_step` method is needed to fulfill the Error Stepper concept. The
* state is updated in-place and an error estimate is calculated. A type modelling a Boost.Range can be used for x.
* - `do_step( sys , x , dxdt , t , dt , xerr )` - This method updates the state and the derivative in-place. It is assumed
* that the dxdt has the value of the derivative of x at time t. An error estimate is calculated.
* - `do_step( sys , in , t , out , dt , xerr )` - This method updates the state out-of-place and estimates the error
* during the step.
* - `do_step( sys , in , dxdt_in , t , out , dxdt_out , dt , xerr )` - This methods updates the state and the derivative
* out-of-place and estimates the error during the step. It is assumed the dxdt_in is derivative of in at time t.
*
* \note The system is always passed as value, which might result in poor performance if it contains data. In this
* case it can be used with `boost::ref` or `std::ref`, for example `stepper.do_step( boost::ref( sys ) , x , t , dt );`
*
* \note The time `t` is not advanced by the stepper. This has to done manually, or by the appropriate `integrate`
* routines or `iterator`s.
*
* \tparam Stepper The stepper on which this class should work. It is used via CRTP, hence explicit_stepper_base
* provides the interface for the Stepper.
* \tparam Order The order of a stepper if the stepper is used without error estimation.
* \tparam StepperOrder The order of a step if the stepper is used with error estimation. Usually Order and StepperOrder have
* the same value.
* \tparam ErrorOrder The order of the error step if the stepper is used with error estimation.
* \tparam State The state type for the stepper.
* \tparam Value The value type for the stepper. This should be a floating point type, like float,
* double, or a multiprecision type. It must not necessary be the value_type of the State. For example
* the State can be a `vector< complex< double > >` in this case the Value must be double.
* The default value is double.
* \tparam Deriv The type representing time derivatives of the state type. It is usually the same type as the
* state type, only if used with Boost.Units both types differ.
* \tparam Time The type representing the time. Usually the same type as the value type. When Boost.Units is
* used, this type has usually a unit.
* \tparam Algebra The algebra type which must fulfill the Algebra Concept.
* \tparam Operations The type for the operations which must fulfill the Operations Concept.
* \tparam Resizer The resizer policy class.
*/
/**
* \fn explicit_error_stepper_fsal_base::explicit_error_stepper_fsal_base( const algebra_type &algebra )
* \brief Constructs a explicit_stepper_fsal_base class. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn explicit_error_stepper_fsal_base::order( void ) const
* \return Returns the order of the stepper if it used without error estimation.
*/
/**
* \fn explicit_error_stepper_fsal_base::stepper_order( void ) const
* \return Returns the order of a step if the stepper is used without error estimation.
*/
/**
* \fn explicit_error_stepper_fsal_base::error_order( void ) const
* \return Returns the order of an error step if the stepper is used without error estimation.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , StateInOut &x , time_type t , time_type dt )
* \brief This method performs one step. It transforms the result in-place.
*
* \note This method uses the internal state of the stepper.
*
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , StateInOut &x , DerivInOut &dxdt , time_type t , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. Additionally to the other methods
* the derivative of x is also passed to this method. Therefore, dxdt must be evaluated initially:
*
* \code
* ode( x , dxdt , t );
* for( ... )
* {
* stepper.do_step( ode , x , dxdt , t , dt );
* t += dt;
* }
* \endcode
*
* \note This method does NOT use the initial state, since the first derivative is explicitly passed to this method.
*
* The result is updated in place in x as well as the derivative dxdt. This method is disabled if
* Time and StateInOut are of the same type. In this case the method could not be distinguished from other `do_step`
* versions.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param dxdt The derivative of x at t. After calling `do_step` dxdt is updated to the new value.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* This method is disabled if StateIn and Time are the same type. In this case the method can not be distinguished from
* other `do_step` variants.
*
* \note This method uses the internal state of the stepper.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the derivative of x at t is passed to the stepper and updated by the stepper to its new value at
* t+dt.
*
* \note This method does not solve the forwarding problem.
*
* \note This method does NOT use the internal state of the stepper.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt_in The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dxdt_out The updated derivative of `out` at `t+dt`.
* \param dt The step size.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , StateInOut &x , time_type t , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper and estimates the error. The state of the ODE
* is updated in-place.
*
*
* \note This method uses the internal state of the stepper.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. x is updated by this method.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
* \param xerr The estimation of the error is stored in xerr.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , StateInOut &x , DerivInOut &dxdt , time_type t , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. Additionally to the other method
* the derivative of x is also passed to this method and updated by this method.
*
* \note This method does NOT use the internal state of the stepper.
*
* The result is updated in place in x. This method is disabled if Time and Deriv are of the same type. In this
* case the method could not be distinguished from other `do_step` versions. This method is disabled if StateInOut and
* Time are of the same type.
*
* \note This method does NOT use the internal state of the stepper.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param dxdt The derivative of x at t. After calling `do_step` this value is updated to the new value at `t+dt`.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
* \param xerr The error estimate is stored in xerr.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the error is estimated.
*
* \note This method uses the internal state of the stepper.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
* \param xerr The error estimate.
*/
/**
* \fn explicit_error_stepper_fsal_base::do_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type t , StateOut &out , DerivOut &dxdt_out , time_type dt , Err &xerr )
* \brief The method performs one step with the stepper passed by Stepper. The state of the ODE is updated out-of-place.
* Furthermore, the derivative of x at t is passed to the stepper and the error is estimated.
*
* \note This method does NOT use the internal state of the stepper.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt_in The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dxdt_out The new derivative at `t+dt` is written into this variable.
* \param dt The step size.
* \param xerr The error estimate.
*/
/**
* \fn explicit_error_stepper_fsal_base::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
/**
* \fn explicit_error_stepper_fsal_base::reset( void )
* \brief Resets the internal state of this stepper. After calling this method it is safe to use all
* `do_step` method without explicitly initializing the stepper.
*/
/**
* \fn explicit_error_stepper_fsal_base::initialize( const DerivIn &deriv )
* \brief Initializes the internal state of the stepper.
* \param deriv The derivative of x. The next call of `do_step` expects that the derivative of `x` passed to `do_step`
* has the value of `deriv`.
*/
/**
* \fn explicit_error_stepper_fsal_base::initialize( System system , const StateIn &x , time_type t )
* \brief Initializes the internal state of the stepper.
*
* This method is equivalent to
* \code
* Deriv dxdt;
* system( x , dxdt , t );
* stepper.initialize( dxdt );
* \endcode
*
* \param system The system function for the next calls of `do_step`.
* \param x The current state of the ODE.
* \param t The current time of the ODE.
*/
/**
* \fn explicit_error_stepper_fsal_base::is_initialized( void ) const
* \brief Returns if the stepper is already initialized. If the stepper is not initialized, the first
* call of `do_step` will initialize the state of the stepper. If the stepper is already initialized
* the system function can not be safely exchanged between consecutive `do_step` calls.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_ERROR_STEPPER_FSAL_BASE_HPP_INCLUDED

383
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp Executable file
View File

@@ -0,0 +1,383 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp
[begin_description]
Base class for all explicit Runge Kutta steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* base class for explicit steppers
* models the stepper concept
*
* this class provides the following overloads
* do_step( sys , x , t , dt )
* do_step( sys , in , t , out , dt )
* do_step( sys , x , dxdt_in , t , dt )
* do_step( sys , in , dxdt_in , t , out , dt )
*/
template<
class Stepper ,
unsigned short Order ,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class explicit_stepper_base : public algebra_stepper_base< Algebra , Operations >
{
public:
#ifndef DOXYGEN_SKIP
typedef explicit_stepper_base< Stepper , Order , State , Value , Deriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
#endif // DOXYGEN_SKIP
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Resizer resizer_type;
typedef Stepper stepper_type;
typedef stepper_tag stepper_category;
typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
typedef typename algebra_stepper_base_type::algebra_type algebra_type;
typedef typename algebra_stepper_base_type::operations_type operations_type;
typedef unsigned short order_type;
#ifndef DOXYGEN_SKIP
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
#endif // DOXYGEN_SKIP
static const order_type order_value = Order;
explicit_stepper_base( const algebra_type &algebra = algebra_type() )
: algebra_stepper_base_type( algebra )
{ }
/**
* \return Returns the order of the stepper.
*/
order_type order( void ) const
{
return order_value;
}
/*
* Version 1 : do_step( sys , x , t , dt )
*
* the two overloads are needed in order to solve the forwarding problem
*/
template< class System , class StateInOut >
void do_step( System system , StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/**
* \brief Second version to solve the forwarding problem, can be called with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &x , time_type t , time_type dt )
{
do_step_v1( system , x , t , dt );
}
/*
* Version 2 : do_step( sys , x , dxdt , t , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
template< class System , class StateInOut , class DerivIn >
typename boost::disable_if< boost::is_same< DerivIn , time_type > , void >::type
do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )
{
this->stepper().do_step_impl( system , x , dxdt , t , x , dt );
}
/*
* Version 3 : do_step( sys , in , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( in , detail::bind( &internal_stepper_base_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , in , m_dxdt.m_v , t , out , dt );
}
/*
* Version 4 : do_step( sys , in , dxdt , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
{
this->stepper().do_step_impl( system , in , dxdt , t , out , dt );
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
}
private:
stepper_type& stepper( void )
{
return *static_cast< stepper_type* >( this );
}
const stepper_type& stepper( void ) const
{
return *static_cast< const stepper_type* >( this );
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
template< class System , class StateInOut >
void do_step_v1( System system , StateInOut &x , time_type t , time_type dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_resizer.adjust_size( x , detail::bind( &internal_stepper_base_type::template resize_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
sys( x , m_dxdt.m_v ,t );
this->stepper().do_step_impl( system , x , m_dxdt.m_v , t , x , dt );
}
resizer_type m_resizer;
protected:
wrapped_deriv_type m_dxdt;
};
/******* DOXYGEN *********/
/**
* \class explicit_stepper_base
* \brief Base class for explicit steppers without step size control and without dense output.
*
* This class serves as the base class for all explicit steppers with algebra and operations.
* Step size control and error estimation as well as dense output are not provided. explicit_stepper_base
* is used as the interface in a CRTP (currently recurring template pattern). In order to work
* correctly the parent class needs to have a method `do_step_impl( system , in , dxdt_in , t , out , dt )`.
* This is method is used by explicit_stepper_base. explicit_stepper_base derives from
* algebra_stepper_base. An example how this class can be used is
*
* \code
* template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resizer >
* class custom_euler : public explicit_stepper_base< 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
* {
* public:
*
* typedef explicit_stepper_base< 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer > base_type;
*
* custom_euler( const Algebra &algebra = Algebra() ) { }
*
* template< class Sys , class StateIn , class DerivIn , class StateOut >
* void do_step_impl( Sys sys , const StateIn &in , const DerivIn &dxdt , Time t , StateOut &out , Time dt )
* {
* m_algebra.for_each3( out , in , dxdt , Operations::scale_sum2< Value , Time >( 1.0 , dt );
* }
*
* template< class State >
* void adjust_size( const State &x )
* {
* base_type::adjust_size( x );
* }
* };
* \endcode
*
* For the Stepper concept only the `do_step( sys , x , t , dt )` needs to be implemented. But this class
* provides additional `do_step` variants since the stepper is explicit. These methods can be used to increase
* the performance in some situation, for example if one needs to analyze `dxdt` during each step. In this case
* one can use
*
* \code
* sys( x , dxdt , t );
* stepper.do_step( sys , x , dxdt , t , dt ); // the value of dxdt is used here
* t += dt;
* \endcode
*
* In detail explicit_stepper_base provides the following `do_step` variants
* - `do_step( sys , x , t , dt )` - The classical `do_step` method needed to fulfill the Stepper concept. The state is updated in-place.
* A type modelling a Boost.Range can be used for x.
* - `do_step( sys , in , t , out , dt )` - This method updates the state out-of-place, hence the result of the step is stored in `out`.
* - `do_step( sys , x , dxdt , t , dt )` - This method updates the state in-place, but the derivative at the point `t` must be
* explicitly passed in `dxdt`. For an example see the code snippet above.
* - `do_step( sys , in , dxdt , t , out , dt )` - This method update the state out-of-place and expects that the derivative at the point
* `t` is explicitly passed in `dxdt`. It is a combination of the two `do_step` methods above.
*
* \note The system is always passed as value, which might result in poor performance if it contains data. In this case it can be used with `boost::ref`
* or `std::ref`, for example `stepper.do_step( boost::ref( sys ) , x , t , dt );`
*
* \note The time `t` is not advanced by the stepper. This has to done manually, or by the appropriate `integrate` routines or `iterator`s.
*
* \tparam Stepper The stepper on which this class should work. It is used via CRTP, hence explicit_stepper_base
* provides the interface for the Stepper.
* \tparam Order The order of the stepper.
* \tparam State The state type for the stepper.
* \tparam Value The value type for the stepper. This should be a floating point type, like float,
* double, or a multiprecision type. It must not necessary be the value_type of the State. For example
* the State can be a `vector< complex< double > >` in this case the Value must be double.
* The default value is double.
* \tparam Deriv The type representing time derivatives of the state type. It is usually the same type as the
* state type, only if used with Boost.Units both types differ.
* \tparam Time The type representing the time. Usually the same type as the value type. When Boost.Units is
* used, this type has usually a unit.
* \tparam Algebra The algebra type which must fulfill the Algebra Concept.
* \tparam Operations The type for the operations which must fulfill the Operations Concept.
* \tparam Resizer The resizer policy class.
*/
/**
* \fn explicit_stepper_base::explicit_stepper_base( const algebra_type &algebra )
* \brief Constructs a explicit_stepper_base class. This constructor can be used as a default
* constructor if the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn explicit_stepper_base::order_type order( void ) const
* \return Returns the order of the stepper.
*/
/**
* \fn explicit_stepper_base::do_step( System system , StateInOut &x , time_type t , time_type dt )
* \brief This method performs one step. It transforms the result in-place.
*
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn explicit_stepper_base::do_step( System system , StateInOut &x , const DerivIn &dxdt , time_type t , time_type dt )
* \brief The method performs one step. Additionally to the other method
* the derivative of x is also passed to this method. It is supposed to be used in the following way:
*
* \code
* sys( x , dxdt , t );
* stepper.do_step( sys , x , dxdt , t , dt );
* \endcode
*
* The result is updated in place in x. This method is disabled if Time and Deriv are of the same type. In this
* case the method could not be distinguished from other `do_step` versions.
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. After calling do_step the result is updated in x.
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param dt The step size.
*/
/**
* \fn void explicit_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step. The state of the ODE is updated out-of-place.
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn void explicit_stepper_base::do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
* \brief The method performs one step. The state of the ODE is updated out-of-place.
* Furthermore, the derivative of x at t is passed to the stepper.
* It is supposed to be used in the following way:
*
* \code
* sys( in , dxdt , t );
* stepper.do_step( sys , in , dxdt , t , out , dt );
* \endcode
*
* \note This method does not solve the forwarding problem.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn void explicit_stepper_base::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_EXPLICIT_STEPPER_BASE_HPP_INCLUDED

433
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/base/symplectic_rkn_stepper_base.hpp Executable file
View File

@@ -0,0 +1,433 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/base/symplectic_rkn_stepper_base.hpp
[begin_description]
Base class for symplectic Runge-Kutta-Nystrom steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED
#include <boost/array.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/is_pair.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
size_t NumOfStages ,
unsigned short Order ,
class Coor ,
class Momentum ,
class Value ,
class CoorDeriv ,
class MomentumDeriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class symplectic_nystroem_stepper_base : public algebra_stepper_base< Algebra , Operations >
{
public:
typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
typedef typename algebra_stepper_base_type::algebra_type algebra_type;
typedef typename algebra_stepper_base_type::operations_type operations_type;
const static size_t num_of_stages = NumOfStages;
typedef Coor coor_type;
typedef Momentum momentum_type;
typedef std::pair< coor_type , momentum_type > state_type;
typedef CoorDeriv coor_deriv_type;
typedef state_wrapper< coor_deriv_type> wrapped_coor_deriv_type;
typedef MomentumDeriv momentum_deriv_type;
typedef state_wrapper< momentum_deriv_type > wrapped_momentum_deriv_type;
typedef std::pair< coor_deriv_type , momentum_deriv_type > deriv_type;
typedef Value value_type;
typedef Time time_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef symplectic_nystroem_stepper_base< NumOfStages , Order , Coor , Momentum , Value ,
CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
#endif
typedef unsigned short order_type;
static const order_type order_value = Order;
typedef boost::array< value_type , num_of_stages > coef_type;
symplectic_nystroem_stepper_base( const coef_type &coef_a , const coef_type &coef_b , const algebra_type &algebra = algebra_type() )
: algebra_stepper_base_type( algebra ) , m_coef_a( coef_a ) , m_coef_b( coef_b ) ,
m_dqdt_resizer() , m_dpdt_resizer() , m_dqdt() , m_dpdt()
{ }
order_type order( void ) const
{
return order_value;
}
/*
* Version 1 : do_step( system , x , t , dt )
*
* This version does not solve the forwarding problem, boost.range can not be used.
*/
template< class System , class StateInOut >
void do_step( System system , const StateInOut &state , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
do_step_impl( system , state , t , state , dt , typename is_pair< system_type >::type() );
}
/**
* \brief Same function as above. It differs only in a different const specifier in order
* to solve the forwarding problem, can be used with Boost.Range.
*/
template< class System , class StateInOut >
void do_step( System system , StateInOut &state , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
do_step_impl( system , state , t , state , dt , typename is_pair< system_type >::type() );
}
/*
* Version 2 : do_step( system , q , p , t , dt );
*
* For Convenience
*
* The two overloads are needed in order to solve the forwarding problem.
*/
template< class System , class CoorInOut , class MomentumInOut >
void do_step( System system , CoorInOut &q , MomentumInOut &p , time_type t , time_type dt )
{
do_step( system , std::make_pair( detail::ref( q ) , detail::ref( p ) ) , t , dt );
}
/**
* \brief Same function as do_step( system , q , p , t , dt ). It differs only in a different const specifier in order
* to solve the forwarding problem, can be called with Boost.Range.
*/
template< class System , class CoorInOut , class MomentumInOut >
void do_step( System system , const CoorInOut &q , const MomentumInOut &p , time_type t , time_type dt )
{
do_step( system , std::make_pair( detail::ref( q ) , detail::ref( p ) ) , t , dt );
}
/*
* Version 3 : do_step( system , in , t , out , dt )
*
* The forwarding problem is not solved in this version
*/
template< class System , class StateIn , class StateOut >
void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
do_step_impl( system , in , t , out , dt , typename is_pair< system_type >::type() );
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_dqdt( x );
resize_dpdt( x );
}
/** \brief Returns the coefficients a. */
const coef_type& coef_a( void ) const { return m_coef_a; }
/** \brief Returns the coefficients b. */
const coef_type& coef_b( void ) const { return m_coef_b; }
private:
// stepper for systems with function for dq/dt = f(p) and dp/dt = -f(q)
template< class System , class StateIn , class StateOut >
void do_step_impl( System system , const StateIn &in , time_type t , StateOut &out , time_type dt , boost::mpl::true_ )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type coor_deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type momentum_deriv_func_type;
system_type &sys = system;
coor_deriv_func_type &coor_func = sys.first;
momentum_deriv_func_type &momentum_func = sys.second;
typedef typename odeint::unwrap_reference< StateIn >::type state_in_type;
typedef typename odeint::unwrap_reference< typename state_in_type::first_type >::type coor_in_type;
typedef typename odeint::unwrap_reference< typename state_in_type::second_type >::type momentum_in_type;
const state_in_type &state_in = in;
const coor_in_type &coor_in = state_in.first;
const momentum_in_type &momentum_in = state_in.second;
typedef typename odeint::unwrap_reference< StateOut >::type state_out_type;
typedef typename odeint::unwrap_reference< typename state_out_type::first_type >::type coor_out_type;
typedef typename odeint::unwrap_reference< typename state_out_type::second_type >::type momentum_out_type;
state_out_type &state_out = out;
coor_out_type &coor_out = state_out.first;
momentum_out_type &momentum_out = state_out.second;
m_dqdt_resizer.adjust_size( coor_in , detail::bind( &internal_stepper_base_type::template resize_dqdt< coor_in_type > , detail::ref( *this ) , detail::_1 ) );
m_dpdt_resizer.adjust_size( momentum_in , detail::bind( &internal_stepper_base_type::template resize_dpdt< momentum_in_type > , detail::ref( *this ) , detail::_1 ) );
// ToDo: check sizes?
for( size_t l=0 ; l<num_of_stages ; ++l )
{
if( l == 0 )
{
coor_func( momentum_in , m_dqdt.m_v );
this->m_algebra.for_each3( coor_out , coor_in , m_dqdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
momentum_func( coor_out , m_dpdt.m_v );
this->m_algebra.for_each3( momentum_out , momentum_in , m_dpdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
}
else
{
coor_func( momentum_out , m_dqdt.m_v );
this->m_algebra.for_each3( coor_out , coor_out , m_dqdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
momentum_func( coor_out , m_dpdt.m_v );
this->m_algebra.for_each3( momentum_out , momentum_out , m_dpdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
}
}
}
// stepper for systems with only function dp /dt = -f(q), dq/dt = p, time not required but still expected for compatibility reasons
template< class System , class StateIn , class StateOut >
void do_step_impl( System system , const StateIn &in , time_type /* t */ , StateOut &out , time_type dt , boost::mpl::false_ )
{
typedef typename odeint::unwrap_reference< System >::type momentum_deriv_func_type;
momentum_deriv_func_type &momentum_func = system;
typedef typename odeint::unwrap_reference< StateIn >::type state_in_type;
typedef typename odeint::unwrap_reference< typename state_in_type::first_type >::type coor_in_type;
typedef typename odeint::unwrap_reference< typename state_in_type::second_type >::type momentum_in_type;
const state_in_type &state_in = in;
const coor_in_type &coor_in = state_in.first;
const momentum_in_type &momentum_in = state_in.second;
typedef typename odeint::unwrap_reference< StateOut >::type state_out_type;
typedef typename odeint::unwrap_reference< typename state_out_type::first_type >::type coor_out_type;
typedef typename odeint::unwrap_reference< typename state_out_type::second_type >::type momentum_out_type;
state_out_type &state_out = out;
coor_out_type &coor_out = state_out.first;
momentum_out_type &momentum_out = state_out.second;
m_dqdt_resizer.adjust_size( coor_in , detail::bind( &internal_stepper_base_type::template resize_dqdt< coor_in_type > , detail::ref( *this ) , detail::_1 ) );
m_dpdt_resizer.adjust_size( momentum_in , detail::bind( &internal_stepper_base_type::template resize_dpdt< momentum_in_type > , detail::ref( *this ) , detail::_1 ) );
// ToDo: check sizes?
for( size_t l=0 ; l<num_of_stages ; ++l )
{
if( l == 0 )
{
this->m_algebra.for_each3( coor_out , coor_in , momentum_in ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
momentum_func( coor_out , m_dpdt.m_v );
this->m_algebra.for_each3( momentum_out , momentum_in , m_dpdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
}
else
{
this->m_algebra.for_each3( coor_out , coor_out , momentum_out ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
momentum_func( coor_out , m_dpdt.m_v );
this->m_algebra.for_each3( momentum_out , momentum_out , m_dpdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
}
}
}
template< class StateIn >
bool resize_dqdt( const StateIn &x )
{
return adjust_size_by_resizeability( m_dqdt , x , typename is_resizeable<coor_deriv_type>::type() );
}
template< class StateIn >
bool resize_dpdt( const StateIn &x )
{
return adjust_size_by_resizeability( m_dpdt , x , typename is_resizeable<momentum_deriv_type>::type() );
}
const coef_type m_coef_a;
const coef_type m_coef_b;
resizer_type m_dqdt_resizer;
resizer_type m_dpdt_resizer;
wrapped_coor_deriv_type m_dqdt;
wrapped_momentum_deriv_type m_dpdt;
};
/********* DOXYGEN *********/
/**
* \class symplectic_nystroem_stepper_base
* \brief Base class for all symplectic steppers of Nystroem type.
*
* This class is the base class for the symplectic Runge-Kutta-Nystroem steppers. Symplectic steppers are usually
* used to solve Hamiltonian systems and they conserve the phase space volume, see
* <a href="http://en.wikipedia.org/wiki/Symplectic_integrator">en.wikipedia.org/wiki/Symplectic_integrator</a>.
* Furthermore, the energy is conserved
* in average. In detail this class of steppers can be used to solve separable Hamiltonian systems which can be written
* in the form H(q,p) = H1(p) + H2(q). q is usually called the coordinate, while p is the momentum. The equations of motion
* are dq/dt = dH1/dp, dp/dt = -dH2/dq.
*
* ToDo : add formula for solver and explanation of the coefficients
*
* symplectic_nystroem_stepper_base uses odeints algebra and operation system. Step size and error estimation are not
* provided for this class of solvers. It derives from algebra_stepper_base. Several `do_step` variants are provided:
*
* - `do_step( sys , x , t , dt )` - The classical `do_step` method. The sys can be either a pair of function objects
* for the coordinate or the momentum part or one function object for the momentum part. `x` is a pair of coordinate
* and momentum. The state is updated in-place.
* - `do_step( sys , q , p , t , dt )` - This method is similar to the method above with the difference that the coordinate
* and the momentum are passed explicitly and not packed into a pair.
* - `do_step( sys , x_in , t , x_out , dt )` - This method transforms the state out-of-place. `x_in` and `x_out` are here pairs
* of coordinate and momentum.
*
* \tparam NumOfStages Number of stages.
* \tparam Order The order of the stepper.
* \tparam Coor The type representing the coordinates q.
* \tparam Momentum The type representing the coordinates p.
* \tparam Value The basic value type. Should be something like float, double or a high-precision type.
* \tparam CoorDeriv The type representing the time derivative of the coordinate dq/dt.
* \tparam MomemtnumDeriv The type representing the time derivative of the momentum dp/dt.
* \tparam Time The type representing the time t.
* \tparam Algebra The algebra.
* \tparam Operations The operations.
* \tparam Resizer The resizer policy.
*/
/**
* \fn symplectic_nystroem_stepper_base::symplectic_nystroem_stepper_base( const coef_type &coef_a , const coef_type &coef_b , const algebra_type &algebra )
* \brief Constructs a symplectic_nystroem_stepper_base class. The parameters of the specific Nystroem method and the
* algebra have to be passed.
* \param coef_a The coefficients a.
* \param coef_b The coefficients b.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn symplectic_nystroem_stepper_base::order( void ) const
* \return Returns the order of the stepper.
*/
/**
* \fn symplectic_nystroem_stepper_base::do_step( System system , const StateInOut &state , time_type t , time_type dt )
* \brief This method performs one step. The system can be either a pair of two function object
* describing the momentum part and the coordinate part or one function object describing only
* the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
* is updated in-place.
*
* \note boost::ref or std::ref can be used for the system as well as for the state. So, it is correct
* to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , make_pair( std::ref( q ) , std::ref( p ) ) , t , dt )`.
*
* \note This method solves the forwarding problem.
*
* \param system The system, can be represented as a pair of two function object or one function object. See above.
* \param state The state of the ODE. It is a pair of Coor and Momentum. The state is updated in-place, therefore, the
* new value of the state will be written into this variable.
* \param t The time of the ODE. It is not advanced by this method.
* \param dt The time step.
*/
/**
* \fn symplectic_nystroem_stepper_base::do_step( System system , CoorInOut &q , MomentumInOut &p , time_type t , time_type dt )
* \brief This method performs one step. The system can be either a pair of two function object
* describing the momentum part and the coordinate part or one function object describing only
* the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
* is updated in-place.
*
* \note boost::ref or std::ref can be used for the system. So, it is correct
* to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , q , p , t , dt )`.
*
* \note This method solves the forwarding problem.
*
* \param system The system, can be represented as a pair of two function object or one function object. See above.
* \param q The coordinate of the ODE. It is updated in-place. Therefore, the new value of the coordinate will be written
* into this variable.
* \param p The momentum of the ODE. It is updated in-place. Therefore, the new value of the momentum will be written info
* this variable.
* \param t The time of the ODE. It is not advanced by this method.
* \param dt The time step.
*/
/**
* \fn symplectic_nystroem_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
* \brief This method performs one step. The system can be either a pair of two function object
* describing the momentum part and the coordinate part or one function object describing only
* the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
* is updated out-of-place.
*
* \note boost::ref or std::ref can be used for the system. So, it is correct
* to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , x_in , t , x_out , dt )`.
*
* \note This method NOT solve the forwarding problem.
*
* \param system The system, can be represented as a pair of two function object or one function object. See above.
* \param in The state of the ODE, which is a pair of coordinate and momentum. The state is updated out-of-place, therefore the
* new value is written into out
* \param t The time of the ODE. It is not advanced by this method.
* \param out The new state of the ODE.
* \param dt The time step.
*/
/**
* \fn symplectic_nystroem_stepper_base::adjust_size( const StateType &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED

623
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/bulirsch_stoer.hpp Executable file
View File

@@ -0,0 +1,623 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/bulirsch_stoer.hpp
[begin_description]
Implementation of the Burlish-Stoer method. As described in
Ernst Hairer, Syvert Paul Norsett, Gerhard Wanner
Solving Ordinary Differential Equations I. Nonstiff Problems.
Springer Series in Comput. Mathematics, Vol. 8, Springer-Verlag 1987, Second revised edition 1993.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED
#include <iostream>
#include <algorithm>
#include <boost/config.hpp> // for min/max guidelines
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/util/detail/less_with_sign.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class bulirsch_stoer {
public:
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
typedef Resizer resizer_type;
#ifndef DOXYGEN_SKIP
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef controlled_stepper_tag stepper_category;
typedef bulirsch_stoer< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;
typedef typename inverse_time< time_type >::type inv_time_type;
typedef std::vector< value_type > value_vector;
typedef std::vector< time_type > time_vector;
typedef std::vector< inv_time_type > inv_time_vector; //should be 1/time_type for boost.units
typedef std::vector< value_vector > value_matrix;
typedef std::vector< size_t > int_vector;
typedef std::vector< wrapped_state_type > state_table_type;
#endif //DOXYGEN_SKIP
const static size_t m_k_max = 8;
bulirsch_stoer(
value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 )
: m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) , m_midpoint() ,
m_last_step_rejected( false ) , m_first( true ) ,
m_interval_sequence( m_k_max+1 ) ,
m_coeff( m_k_max+1 ) ,
m_cost( m_k_max+1 ) ,
m_table( m_k_max ) ,
STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
/* initialize sequence of stage numbers and work */
for( unsigned short i = 0; i < m_k_max+1; i++ )
{
m_interval_sequence[i] = 2 * (i+1);
if( i == 0 )
m_cost[i] = m_interval_sequence[i];
else
m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
m_coeff[i].resize(i);
for( size_t k = 0 ; k < i ; ++k )
{
const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
}
// crude estimate of optimal order
m_current_k_opt = 4;
/* no calculation because log10 might not exist for value_type!
const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >(1.0E-12) ) ) * 0.6 + 0.5 );
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( 1 ) , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<value_type>( m_k_max-1 ) , logfact ));
*/
}
}
/*
* Version 1 : try_step( sys , x , t , dt )
*
* The overloads are needed to solve the forwarding problem
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t, dt );
}
/**
* \brief Second version to solve the forwarding problem, can be used with Boost.Range as StateInOut.
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t, dt );
}
/*
* Version 2 : try_step( sys , x , dxdt , t , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateInOut , class DerivIn >
controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
{
m_xnew_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_xnew< StateInOut > , detail::ref( *this ) , detail::_1 ) );
controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
if( res == success )
{
boost::numeric::odeint::copy( m_xnew.m_v , x );
}
return res;
}
/*
* Version 3 : try_step( sys , in , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
template< class System , class StateIn , class StateOut >
typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateIn > , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v , t );
return try_step( system , in , m_dxdt.m_v , t , out , dt );
}
/*
* Full version : try_step( sys , in , dxdt_in , t , out , dt )
*
* contains the actual implementation
*/
template< class System , class StateIn , class DerivIn , class StateOut >
controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
static const value_type val1( 1.0 );
typename odeint::unwrap_reference< System >::type &sys = system;
if( m_resizer.adjust_size( in , detail::bind( &controlled_error_bs_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) )
{
reset(); // system resized -> reset
}
if( dt != m_dt_last )
{
reset(); // step size changed from outside -> reset
}
bool reject( true );
time_vector h_opt( m_k_max+1 );
inv_time_vector work( m_k_max+1 );
time_type new_h = dt;
/* m_current_k_opt is the estimated current optimal stage number */
for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
{
/* the stage counts are stored in m_interval_sequence */
m_midpoint.set_steps( m_interval_sequence[k] );
if( k == 0 )
{
m_midpoint.do_step( sys , in , dxdt , t , out , dt );
/* the first step, nothing more to do */
}
else
{
m_midpoint.do_step( sys , in , dxdt , t , m_table[k-1].m_v , dt );
extrapolate( k , m_table , m_coeff , out );
// get error estimate
m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
h_opt[k] = calc_h_opt( dt , error , k );
work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];
if( (k == m_current_k_opt-1) || m_first )
{ // convergence before k_opt ?
if( error < 1.0 )
{
//convergence
reject = false;
if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
{
// leave order as is (except we were in first round)
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
new_h = h_opt[k];
new_h *= static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
} else {
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
new_h = h_opt[k];
}
break;
}
else if( should_reject( error , k ) && !m_first )
{
reject = true;
new_h = h_opt[k];
break;
}
}
if( k == m_current_k_opt )
{ // convergence at k_opt ?
if( error < 1.0 )
{
//convergence
reject = false;
if( (work[k-1] < KFAC2*work[k]) )
{
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
new_h = h_opt[m_current_k_opt];
}
else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
{
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max-1) , static_cast<int>(m_current_k_opt)+1 );
new_h = h_opt[k];
new_h *= m_cost[m_current_k_opt]/m_cost[k];
} else
new_h = h_opt[m_current_k_opt];
break;
}
else if( should_reject( error , k ) )
{
reject = true;
new_h = h_opt[m_current_k_opt];
break;
}
}
if( k == m_current_k_opt+1 )
{ // convergence at k_opt+1 ?
if( error < 1.0 )
{ //convergence
reject = false;
if( work[k-2] < KFAC2*work[k-1] )
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
new_h = h_opt[m_current_k_opt];
} else
{
reject = true;
new_h = h_opt[m_current_k_opt];
}
break;
}
}
}
if( !reject )
{
t += dt;
}
if( !m_last_step_rejected || boost::numeric::odeint::detail::less_with_sign(new_h, dt, dt) )
{
m_dt_last = new_h;
dt = new_h;
}
m_last_step_rejected = reject;
m_first = false;
if( reject )
return fail;
else
return success;
}
/** \brief Resets the internal state of the stepper */
void reset()
{
m_first = true;
m_last_step_rejected = false;
}
/* Resizer methods */
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_m_dxdt( x );
resize_m_xnew( x );
resize_impl( x );
m_midpoint.adjust_size();
}
private:
template< class StateIn >
bool resize_m_dxdt( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
template< class StateIn >
bool resize_m_xnew( const StateIn &x )
{
return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
for( size_t i = 0 ; i < m_k_max ; ++i )
resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );
return resized;
}
template< class System , class StateInOut >
controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_error_bs_type::template resize_m_dxdt< StateInOut > , detail::ref( *this ) , detail::_1 ) );
sys( x , m_dxdt.m_v ,t );
return try_step( system , x , m_dxdt.m_v , t , dt );
}
template< class StateInOut >
void extrapolate( size_t k , state_table_type &table , const value_matrix &coeff , StateInOut &xest )
/* polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
uses the obtained intermediate results to extrapolate to dt->0
*/
{
static const value_type val1 = static_cast< value_type >( 1.0 );
for( int j=k-1 ; j>0 ; --j )
{
m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][j] , -coeff[k][j] ) );
}
m_algebra.for_each3( xest , table[0].m_v , xest ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k][0] , -coeff[k][0]) );
}
time_type calc_h_opt( time_type h , value_type error , size_t k ) const
/* calculates the optimal step size for a given error and stage number */
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
value_type expo( 1.0/(2*k+1) );
value_type facmin = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , expo );
value_type fac;
if (error == 0.0)
fac=1.0/facmin;
else
{
fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( facmin/STEPFAC4 , min BOOST_PREVENT_MACRO_SUBSTITUTION( 1.0/facmin , fac ) );
}
return h*fac;
}
controlled_step_result set_k_opt( size_t k , const inv_time_vector &work , const time_vector &h_opt , time_type &dt )
/* calculates the optimal stage number */
{
if( k == 1 )
{
m_current_k_opt = 2;
return success;
}
if( (work[k-1] < KFAC1*work[k]) || (k == m_k_max) )
{ // order decrease
m_current_k_opt = k-1;
dt = h_opt[ m_current_k_opt ];
return success;
}
else if( (work[k] < KFAC2*work[k-1]) || m_last_step_rejected || (k == m_k_max-1) )
{ // same order - also do this if last step got rejected
m_current_k_opt = k;
dt = h_opt[ m_current_k_opt ];
return success;
}
else
{ // order increase - only if last step was not rejected
m_current_k_opt = k+1;
dt = h_opt[ m_current_k_opt-1 ] * m_cost[ m_current_k_opt ] / m_cost[ m_current_k_opt-1 ] ;
return success;
}
}
bool in_convergence_window( size_t k ) const
{
if( (k == m_current_k_opt-1) && !m_last_step_rejected )
return true; // decrease stepsize only if last step was not rejected
return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
}
bool should_reject( value_type error , size_t k ) const
{
if( k == m_current_k_opt-1 )
{
const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
(m_interval_sequence[0]*m_interval_sequence[0]);
//step will fail, criterion 17.3.17 in NR
return ( error > d*d );
}
else if( k == m_current_k_opt )
{
const value_type d = m_interval_sequence[m_current_k_opt] / m_interval_sequence[0];
return ( error > d*d );
} else
return error > 1.0;
}
default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
modified_midpoint< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;
bool m_last_step_rejected;
bool m_first;
time_type m_dt_last;
time_type m_t_last;
size_t m_current_k_opt;
algebra_type m_algebra;
resizer_type m_dxdt_resizer;
resizer_type m_xnew_resizer;
resizer_type m_resizer;
wrapped_state_type m_xnew;
wrapped_state_type m_err;
wrapped_deriv_type m_dxdt;
int_vector m_interval_sequence; // stores the successive interval counts
value_matrix m_coeff;
int_vector m_cost; // costs for interval count
state_table_type m_table; // sequence of states for extrapolation
const value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};
/******** DOXYGEN ********/
/**
* \class bulirsch_stoer
* \brief The Bulirsch-Stoer algorithm.
*
* The Bulirsch-Stoer is a controlled stepper that adjusts both step size
* and order of the method. The algorithm uses the modified midpoint and
* a polynomial extrapolation compute the solution.
*
* \tparam State The state type.
* \tparam Value The value type.
* \tparam Deriv The type representing the time derivative of the state.
* \tparam Time The time representing the independent variable - the time.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
*/
/**
* \fn bulirsch_stoer::bulirsch_stoer( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt )
* \brief Constructs the bulirsch_stoer class, including initialization of
* the error bounds.
*
* \param eps_abs Absolute tolerance level.
* \param eps_rel Relative tolerance level.
* \param factor_x Factor for the weight of the state.
* \param factor_dxdt Factor for the weight of the derivative.
*/
/**
* \fn bulirsch_stoer::try_step( System system , StateInOut &x , time_type &t , time_type &dt )
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed. Also, the internal order of the stepper is adjusted if required.
*
* \param system The system function to solve, hence the r.h.s. of the ODE.
* It must fulfill the Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
/**
* \fn bulirsch_stoer::try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed. Also, the internal order of the stepper is adjusted if required.
*
* \param system The system function to solve, hence the r.h.s. of the ODE.
* It must fulfill the Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
/**
* \fn bulirsch_stoer::try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
* \brief Tries to perform one step.
*
* \note This method is disabled if state_type=time_type to avoid ambiguity.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed. Also, the internal order of the stepper is adjusted if required.
*
* \param system The system function to solve, hence the r.h.s. of the ODE.
* It must fulfill the Simple System concept.
* \param in The state of the ODE which should be solved.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
/**
* \fn bulirsch_stoer::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed. Also, the internal order of the stepper is adjusted if required.
*
* \param system The system function to solve, hence the r.h.s. of the ODE.
* It must fulfill the Simple System concept.
* \param in The state of the ODE which should be solved.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
/**
* \fn bulirsch_stoer::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED

812
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp Executable file
View File

@@ -0,0 +1,812 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/bulirsch_stoer_dense_out.hpp
[begin_description]
Implementaiton of the Burlish-Stoer method with dense output
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_DENSE_OUT_HPP_INCLUDED
#include <iostream>
#include <algorithm>
#include <boost/config.hpp> // for min/max guidelines
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/modified_midpoint.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/type_traits.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class bulirsch_stoer_dense_out {
public:
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
typedef Resizer resizer_type;
typedef dense_output_stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef bulirsch_stoer_dense_out< State , Value , Deriv , Time , Algebra , Operations , Resizer > controlled_error_bs_type;
typedef typename inverse_time< time_type >::type inv_time_type;
typedef std::vector< value_type > value_vector;
typedef std::vector< time_type > time_vector;
typedef std::vector< inv_time_type > inv_time_vector; //should be 1/time_type for boost.units
typedef std::vector< value_vector > value_matrix;
typedef std::vector< size_t > int_vector;
typedef std::vector< wrapped_state_type > state_vector_type;
typedef std::vector< wrapped_deriv_type > deriv_vector_type;
typedef std::vector< deriv_vector_type > deriv_table_type;
#endif //DOXYGEN_SKIP
const static size_t m_k_max = 8;
bulirsch_stoer_dense_out(
value_type eps_abs = 1E-6 , value_type eps_rel = 1E-6 ,
value_type factor_x = 1.0 , value_type factor_dxdt = 1.0 ,
bool control_interpolation = false )
: m_error_checker( eps_abs , eps_rel , factor_x, factor_dxdt ) ,
m_control_interpolation( control_interpolation) ,
m_last_step_rejected( false ) , m_first( true ) ,
m_current_state_x1( true ) ,
m_error( m_k_max ) ,
m_interval_sequence( m_k_max+1 ) ,
m_coeff( m_k_max+1 ) ,
m_cost( m_k_max+1 ) ,
m_table( m_k_max ) ,
m_mp_states( m_k_max+1 ) ,
m_derivs( m_k_max+1 ) ,
m_diffs( 2*m_k_max+1 ) ,
STEPFAC1( 0.65 ) , STEPFAC2( 0.94 ) , STEPFAC3( 0.02 ) , STEPFAC4( 4.0 ) , KFAC1( 0.8 ) , KFAC2( 0.9 )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
for( unsigned short i = 0; i < m_k_max+1; i++ )
{
/* only this specific sequence allows for dense output */
m_interval_sequence[i] = 2 + 4*i; // 2 6 10 14 ...
m_derivs[i].resize( m_interval_sequence[i] );
if( i == 0 )
m_cost[i] = m_interval_sequence[i];
else
m_cost[i] = m_cost[i-1] + m_interval_sequence[i];
m_coeff[i].resize(i);
for( size_t k = 0 ; k < i ; ++k )
{
const value_type r = static_cast< value_type >( m_interval_sequence[i] ) / static_cast< value_type >( m_interval_sequence[k] );
m_coeff[i][k] = 1.0 / ( r*r - static_cast< value_type >( 1.0 ) ); // coefficients for extrapolation
}
// crude estimate of optimal order
m_current_k_opt = 4;
/* no calculation because log10 might not exist for value_type!
const value_type logfact( -log10( max BOOST_PREVENT_MACRO_SUBSTITUTION( eps_rel , static_cast< value_type >( 1.0E-12 ) ) ) * 0.6 + 0.5 );
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 1 , min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>( m_k_max-1 ) , static_cast<int>( logfact ) ));
*/
}
int num = 1;
for( int i = 2*(m_k_max) ; i >=0 ; i-- )
{
m_diffs[i].resize( num );
num += (i+1)%2;
}
}
template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
static const value_type val1( 1.0 );
typename odeint::unwrap_reference< System >::type &sys = system;
bool reject( true );
time_vector h_opt( m_k_max+1 );
inv_time_vector work( m_k_max+1 );
m_k_final = 0;
time_type new_h = dt;
//std::cout << "t=" << t <<", dt=" << dt << ", k_opt=" << m_current_k_opt << ", first: " << m_first << std::endl;
for( size_t k = 0 ; k <= m_current_k_opt+1 ; k++ )
{
m_midpoint.set_steps( m_interval_sequence[k] );
if( k == 0 )
{
m_midpoint.do_step( sys , in , dxdt , t , out , dt , m_mp_states[k].m_v , m_derivs[k]);
}
else
{
m_midpoint.do_step( sys , in , dxdt , t , m_table[k-1].m_v , dt , m_mp_states[k].m_v , m_derivs[k] );
extrapolate( k , m_table , m_coeff , out );
// get error estimate
m_algebra.for_each3( m_err.m_v , out , m_table[0].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 , -val1 ) );
const value_type error = m_error_checker.error( m_algebra , in , dxdt , m_err.m_v , dt );
h_opt[k] = calc_h_opt( dt , error , k );
work[k] = static_cast<value_type>( m_cost[k] ) / h_opt[k];
m_k_final = k;
if( (k == m_current_k_opt-1) || m_first )
{ // convergence before k_opt ?
if( error < 1.0 )
{
//convergence
reject = false;
if( (work[k] < KFAC2*work[k-1]) || (m_current_k_opt <= 2) )
{
// leave order as is (except we were in first round)
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k)+1 ) );
new_h = h_opt[k] * static_cast<value_type>( m_cost[k+1] ) / static_cast<value_type>( m_cost[k] );
} else {
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(k) ) );
new_h = h_opt[k];
}
break;
}
else if( should_reject( error , k ) && !m_first )
{
reject = true;
new_h = h_opt[k];
break;
}
}
if( k == m_current_k_opt )
{ // convergence at k_opt ?
if( error < 1.0 )
{
//convergence
reject = false;
if( (work[k-1] < KFAC2*work[k]) )
{
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
new_h = h_opt[m_current_k_opt];
}
else if( (work[k] < KFAC2*work[k-1]) && !m_last_step_rejected )
{
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(m_current_k_opt)+1 );
new_h = h_opt[k]*static_cast<value_type>( m_cost[m_current_k_opt] ) / static_cast<value_type>( m_cost[k] );
} else
new_h = h_opt[m_current_k_opt];
break;
}
else if( should_reject( error , k ) )
{
reject = true;
new_h = h_opt[m_current_k_opt];
break;
}
}
if( k == m_current_k_opt+1 )
{ // convergence at k_opt+1 ?
if( error < 1.0 )
{ //convergence
reject = false;
if( work[k-2] < KFAC2*work[k-1] )
m_current_k_opt = max BOOST_PREVENT_MACRO_SUBSTITUTION( 2 , static_cast<int>(m_current_k_opt)-1 );
if( (work[k] < KFAC2*work[m_current_k_opt]) && !m_last_step_rejected )
m_current_k_opt = min BOOST_PREVENT_MACRO_SUBSTITUTION( static_cast<int>(m_k_max)-1 , static_cast<int>(k) );
new_h = h_opt[m_current_k_opt];
} else
{
reject = true;
new_h = h_opt[m_current_k_opt];
}
break;
}
}
}
if( !reject )
{
//calculate dxdt for next step and dense output
sys( out , dxdt_new , t+dt );
//prepare dense output
value_type error = prepare_dense_output( m_k_final , in , dxdt , out , dxdt_new , dt );
if( error > static_cast<value_type>(10) ) // we are not as accurate for interpolation as for the steps
{
reject = true;
new_h = dt * pow BOOST_PREVENT_MACRO_SUBSTITUTION( error , static_cast<value_type>(-1)/(2*m_k_final+2) );
} else {
t += dt;
}
}
//set next stepsize
if( !m_last_step_rejected || (new_h < dt) )
dt = new_h;
m_last_step_rejected = reject;
if( reject )
return fail;
else
return success;
}
template< class StateType >
void initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
{
m_resizer.adjust_size( x0 , detail::bind( &controlled_error_bs_type::template resize_impl< StateType > , detail::ref( *this ) , detail::_1 ) );
boost::numeric::odeint::copy( x0 , get_current_state() );
m_t = t0;
m_dt = dt0;
reset();
}
/* =======================================================
* the actual step method that should be called from outside (maybe make try_step private?)
*/
template< class System >
std::pair< time_type , time_type > do_step( System system )
{
const size_t max_count = 1000;
if( m_first )
{
typename odeint::unwrap_reference< System >::type &sys = system;
sys( get_current_state() , get_current_deriv() , m_t );
}
controlled_step_result res = fail;
m_t_last = m_t;
size_t count = 0;
while( res == fail )
{
res = try_step( system , get_current_state() , get_current_deriv() , m_t , get_old_state() , get_old_deriv() , m_dt );
m_first = false;
if( count++ == max_count )
throw std::overflow_error( "bulirsch_stoer : too much iterations!");
}
toggle_current_state();
return std::make_pair( m_t_last , m_t );
}
/* performs the interpolation from a calculated step */
template< class StateOut >
void calc_state( time_type t , StateOut &x ) const
{
do_interpolation( t , x );
}
const state_type& current_state( void ) const
{
return get_current_state();
}
time_type current_time( void ) const
{
return m_t;
}
const state_type& previous_state( void ) const
{
return get_old_state();
}
time_type previous_time( void ) const
{
return m_t_last;
}
time_type current_time_step( void ) const
{
return m_dt;
}
/** \brief Resets the internal state of the stepper. */
void reset()
{
m_first = true;
m_last_step_rejected = false;
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
m_midpoint.adjust_size();
}
private:
template< class StateInOut , class StateVector >
void extrapolate( size_t k , StateVector &table , const value_matrix &coeff , StateInOut &xest , size_t order_start_index = 0 )
//polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
{
static const value_type val1( 1.0 );
for( int j=k-1 ; j>0 ; --j )
{
m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index] ,
-coeff[k + order_start_index][j + order_start_index] ) );
}
m_algebra.for_each3( xest , table[0].m_v , xest ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][0 + order_start_index] ,
-coeff[k + order_start_index][0 + order_start_index]) );
}
template< class StateVector >
void extrapolate_dense_out( size_t k , StateVector &table , const value_matrix &coeff , size_t order_start_index = 0 )
//polynomial extrapolation, see http://www.nr.com/webnotes/nr3web21.pdf
{
// result is written into table[0]
static const value_type val1( 1.0 );
for( int j=k ; j>1 ; --j )
{
m_algebra.for_each3( table[j-1].m_v , table[j].m_v , table[j-1].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][j + order_start_index - 1] ,
-coeff[k + order_start_index][j + order_start_index - 1] ) );
}
m_algebra.for_each3( table[0].m_v , table[1].m_v , table[0].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( val1 + coeff[k + order_start_index][order_start_index] ,
-coeff[k + order_start_index][order_start_index]) );
}
time_type calc_h_opt( time_type h , value_type error , size_t k ) const
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
value_type expo=1.0/(m_interval_sequence[k-1]);
value_type facmin = pow BOOST_PREVENT_MACRO_SUBSTITUTION( STEPFAC3 , expo );
value_type fac;
if (error == 0.0)
fac=1.0/facmin;
else
{
fac = STEPFAC2 / pow BOOST_PREVENT_MACRO_SUBSTITUTION( error / STEPFAC1 , expo );
fac = max BOOST_PREVENT_MACRO_SUBSTITUTION( facmin/STEPFAC4 , min BOOST_PREVENT_MACRO_SUBSTITUTION( 1.0/facmin , fac ) );
}
return h*fac;
}
bool in_convergence_window( size_t k ) const
{
if( (k == m_current_k_opt-1) && !m_last_step_rejected )
return true; // decrease order only if last step was not rejected
return ( (k == m_current_k_opt) || (k == m_current_k_opt+1) );
}
bool should_reject( value_type error , size_t k ) const
{
if( k == m_current_k_opt-1 )
{
const value_type d = m_interval_sequence[m_current_k_opt] * m_interval_sequence[m_current_k_opt+1] /
(m_interval_sequence[0]*m_interval_sequence[0]);
//step will fail, criterion 17.3.17 in NR
return ( error > d*d );
}
else if( k == m_current_k_opt )
{
const value_type d = m_interval_sequence[m_current_k_opt+1] / m_interval_sequence[0];
return ( error > d*d );
} else
return error > 1.0;
}
template< class StateIn1 , class DerivIn1 , class StateIn2 , class DerivIn2 >
value_type prepare_dense_output( int k , const StateIn1 &x_start , const DerivIn1 &dxdt_start ,
const StateIn2 & /* x_end */ , const DerivIn2 & /*dxdt_end */ , time_type dt )
/* k is the order to which the result was approximated */
{
/* compute the coefficients of the interpolation polynomial
* we parametrize the interval t .. t+dt by theta = -1 .. 1
* we use 2k+3 values at the interval center theta=0 to obtain the interpolation coefficients
* the values are x(t+dt/2) and the derivatives dx/dt , ... d^(2k+2) x / dt^(2k+2) at the midpoints
* the derivatives are approximated via finite differences
* all values are obtained from interpolation of the results from the increasing orders of the midpoint calls
*/
// calculate finite difference approximations to derivatives at the midpoint
for( int j = 0 ; j<=k ; j++ )
{
/* not working with boost units... */
const value_type d = m_interval_sequence[j] / ( static_cast<value_type>(2) * dt );
value_type f = 1.0; //factor 1/2 here because our interpolation interval has length 2 !!!
for( int kappa = 0 ; kappa <= 2*j+1 ; ++kappa )
{
calculate_finite_difference( j , kappa , f , dxdt_start );
f *= d;
}
if( j > 0 )
extrapolate_dense_out( j , m_mp_states , m_coeff );
}
time_type d = dt/2;
// extrapolate finite differences
for( int kappa = 0 ; kappa<=2*k+1 ; kappa++ )
{
for( int j=1 ; j<=(k-kappa/2) ; ++j )
extrapolate_dense_out( j , m_diffs[kappa] , m_coeff , kappa/2 );
// extrapolation results are now stored in m_diffs[kappa][0]
// divide kappa-th derivative by kappa because we need these terms for dense output interpolation
m_algebra.for_each1( m_diffs[kappa][0].m_v , typename operations_type::template scale< time_type >( static_cast<time_type>(d) ) );
d *= dt/(2*(kappa+2));
}
// dense output coefficients a_0 is stored in m_mp_states[0], a_i for i = 1...2k are stored in m_diffs[i-1][0]
// the error is just the highest order coefficient of the interpolation polynomial
// this is because we use only the midpoint theta=0 as support for the interpolation (remember that theta = -1 .. 1)
value_type error = 0.0;
if( m_control_interpolation )
{
boost::numeric::odeint::copy( m_diffs[2*k+1][0].m_v , m_err.m_v );
error = m_error_checker.error( m_algebra , x_start , dxdt_start , m_err.m_v , dt );
}
return error;
}
template< class DerivIn >
void calculate_finite_difference( size_t j , size_t kappa , value_type fac , const DerivIn &dxdt )
{
const int m = m_interval_sequence[j]/2-1;
if( kappa == 0) // no calculation required for 0th derivative of f
{
m_algebra.for_each2( m_diffs[0][j].m_v , m_derivs[j][m].m_v ,
typename operations_type::template scale_sum1< value_type >( fac ) );
}
else
{
// calculate the index of m_diffs for this kappa-j-combination
const int j_diffs = j - kappa/2;
m_algebra.for_each2( m_diffs[kappa][j_diffs].m_v , m_derivs[j][m+kappa].m_v ,
typename operations_type::template scale_sum1< value_type >( fac ) );
value_type sign = -1.0;
int c = 1;
//computes the j-th order finite difference for the kappa-th derivative of f at t+dt/2 using function evaluations stored in m_derivs
for( int i = m+static_cast<int>(kappa)-2 ; i >= m-static_cast<int>(kappa) ; i -= 2 )
{
if( i >= 0 )
{
m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , m_derivs[j][i].m_v ,
typename operations_type::template scale_sum2< value_type , value_type >( 1.0 ,
sign * fac * boost::math::binomial_coefficient< value_type >( kappa , c ) ) );
}
else
{
m_algebra.for_each3( m_diffs[kappa][j_diffs].m_v , m_diffs[kappa][j_diffs].m_v , dxdt ,
typename operations_type::template scale_sum2< value_type , value_type >( 1.0 , sign * fac ) );
}
sign *= -1;
++c;
}
}
}
template< class StateOut >
void do_interpolation( time_type t , StateOut &out ) const
{
// interpolation polynomial is defined for theta = -1 ... 1
// m_k_final is the number of order-iterations done for the last step - it governs the order of the interpolation polynomial
const value_type theta = 2 * get_unit_value( (t - m_t_last) / (m_t - m_t_last) ) - 1;
// we use only values at interval center, that is theta=0, for interpolation
// our interpolation polynomial is thus of order 2k+2, hence we have 2k+3 terms
boost::numeric::odeint::copy( m_mp_states[0].m_v , out );
// add remaining terms: x += a_1 theta + a2 theta^2 + ... + a_{2k} theta^{2k}
value_type theta_pow( theta );
for( size_t i=0 ; i<=2*m_k_final+1 ; ++i )
{
m_algebra.for_each3( out , out , m_diffs[i][0].m_v ,
typename operations_type::template scale_sum2< value_type >( static_cast<value_type>(1) , theta_pow ) );
theta_pow *= theta;
}
}
/* Resizer methods */
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdt1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdt2 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_err , x , typename is_resizeable<state_type>::type() );
for( size_t i = 0 ; i < m_k_max ; ++i )
resized |= adjust_size_by_resizeability( m_table[i] , x , typename is_resizeable<state_type>::type() );
for( size_t i = 0 ; i < m_k_max+1 ; ++i )
resized |= adjust_size_by_resizeability( m_mp_states[i] , x , typename is_resizeable<state_type>::type() );
for( size_t i = 0 ; i < m_k_max+1 ; ++i )
for( size_t j = 0 ; j < m_derivs[i].size() ; ++j )
resized |= adjust_size_by_resizeability( m_derivs[i][j] , x , typename is_resizeable<deriv_type>::type() );
for( size_t i = 0 ; i < 2*m_k_max+1 ; ++i )
for( size_t j = 0 ; j < m_diffs[i].size() ; ++j )
resized |= adjust_size_by_resizeability( m_diffs[i][j] , x , typename is_resizeable<deriv_type>::type() );
return resized;
}
state_type& get_current_state( void )
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
const state_type& get_current_state( void ) const
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
state_type& get_old_state( void )
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
const state_type& get_old_state( void ) const
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
deriv_type& get_current_deriv( void )
{
return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
}
const deriv_type& get_current_deriv( void ) const
{
return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
}
deriv_type& get_old_deriv( void )
{
return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
}
const deriv_type& get_old_deriv( void ) const
{
return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
}
void toggle_current_state( void )
{
m_current_state_x1 = ! m_current_state_x1;
}
default_error_checker< value_type, algebra_type , operations_type > m_error_checker;
modified_midpoint_dense_out< state_type , value_type , deriv_type , time_type , algebra_type , operations_type , resizer_type > m_midpoint;
bool m_control_interpolation;
bool m_last_step_rejected;
bool m_first;
time_type m_t;
time_type m_dt;
time_type m_dt_last;
time_type m_t_last;
size_t m_current_k_opt;
size_t m_k_final;
algebra_type m_algebra;
resizer_type m_resizer;
wrapped_state_type m_x1 , m_x2;
wrapped_deriv_type m_dxdt1 , m_dxdt2;
wrapped_state_type m_err;
bool m_current_state_x1;
value_vector m_error; // errors of repeated midpoint steps and extrapolations
int_vector m_interval_sequence; // stores the successive interval counts
value_matrix m_coeff;
int_vector m_cost; // costs for interval count
state_vector_type m_table; // sequence of states for extrapolation
//for dense output:
state_vector_type m_mp_states; // sequence of approximations of x at distance center
deriv_table_type m_derivs; // table of function values
deriv_table_type m_diffs; // table of function values
//wrapped_state_type m_a1 , m_a2 , m_a3 , m_a4;
const value_type STEPFAC1 , STEPFAC2 , STEPFAC3 , STEPFAC4 , KFAC1 , KFAC2;
};
/********** DOXYGEN **********/
/**
* \class bulirsch_stoer_dense_out
* \brief The Bulirsch-Stoer algorithm.
*
* The Bulirsch-Stoer is a controlled stepper that adjusts both step size
* and order of the method. The algorithm uses the modified midpoint and
* a polynomial extrapolation compute the solution. This class also provides
* dense output facility.
*
* \tparam State The state type.
* \tparam Value The value type.
* \tparam Deriv The type representing the time derivative of the state.
* \tparam Time The time representing the independent variable - the time.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
*/
/**
* \fn bulirsch_stoer_dense_out::bulirsch_stoer_dense_out( value_type eps_abs , value_type eps_rel , value_type factor_x , value_type factor_dxdt , bool control_interpolation )
* \brief Constructs the bulirsch_stoer class, including initialization of
* the error bounds.
*
* \param eps_abs Absolute tolerance level.
* \param eps_rel Relative tolerance level.
* \param factor_x Factor for the weight of the state.
* \param factor_dxdt Factor for the weight of the derivative.
* \param control_interpolation Set true to additionally control the error of
* the interpolation.
*/
/**
* \fn bulirsch_stoer_dense_out::try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , DerivOut &dxdt_new , time_type &dt )
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed. Also, the internal order of the stepper is adjusted if required.
*
* \param system The system function to solve, hence the r.h.s. of the ODE.
* It must fulfill the Simple System concept.
* \param in The state of the ODE which should be solved.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
/**
* \fn bulirsch_stoer_dense_out::initialize( const StateType &x0 , const time_type &t0 , const time_type &dt0 )
* \brief Initializes the dense output stepper.
*
* \param x0 The initial state.
* \param t0 The initial time.
* \param dt0 The initial time step.
*/
/**
* \fn bulirsch_stoer_dense_out::do_step( System system )
* \brief Does one time step. This is the main method that should be used to
* integrate an ODE with this stepper.
* \note initialize has to be called before using this method to set the
* initial conditions x,t and the stepsize.
* \param system The system function to solve, hence the r.h.s. of the
* ordinary differential equation. It must fulfill the Simple System concept.
* \return Pair with start and end time of the integration step.
*/
/**
* \fn bulirsch_stoer_dense_out::calc_state( time_type t , StateOut &x ) const
* \brief Calculates the solution at an intermediate point within the last step
* \param t The time at which the solution should be calculated, has to be
* in the current time interval.
* \param x The output variable where the result is written into.
*/
/**
* \fn bulirsch_stoer_dense_out::current_state( void ) const
* \brief Returns the current state of the solution.
* \return The current state of the solution x(t).
*/
/**
* \fn bulirsch_stoer_dense_out::current_time( void ) const
* \brief Returns the current time of the solution.
* \return The current time of the solution t.
*/
/**
* \fn bulirsch_stoer_dense_out::previous_state( void ) const
* \brief Returns the last state of the solution.
* \return The last state of the solution x(t-dt).
*/
/**
* \fn bulirsch_stoer_dense_out::previous_time( void ) const
* \brief Returns the last time of the solution.
* \return The last time of the solution t-dt.
*/
/**
* \fn bulirsch_stoer_dense_out::current_time_step( void ) const
* \brief Returns the current step size.
* \return The current step size.
*/
/**
* \fn bulirsch_stoer_dense_out::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_BULIRSCH_STOER_HPP_INCLUDED

939
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/controlled_runge_kutta.hpp Executable file
View File

@@ -0,0 +1,939 @@
/* [auto_generated]
boost/numeric/odeint/stepper/controlled_runge_kutta.hpp
[begin_description]
The default controlled stepper which can be used with all explicit Runge-Kutta error steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#include <cmath>
#include <boost/config.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template
<
class Value ,
class Algebra = range_algebra ,
class Operations = default_operations
>
class default_error_checker
{
public:
typedef Value value_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
default_error_checker(
value_type eps_abs = static_cast< value_type >( 1.0e-6 ) ,
value_type eps_rel = static_cast< value_type >( 1.0e-6 ) ,
value_type a_x = static_cast< value_type >( 1 ) ,
value_type a_dxdt = static_cast< value_type >( 1 ) )
: m_eps_abs( eps_abs ) , m_eps_rel( eps_rel ) , m_a_x( a_x ) , m_a_dxdt( a_dxdt )
{ }
template< class State , class Deriv , class Err , class Time >
value_type error( const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
{
return error( algebra_type() , x_old , dxdt_old , x_err , dt );
}
template< class State , class Deriv , class Err , class Time >
value_type error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
{
// this overwrites x_err !
algebra.for_each3( x_err , x_old , dxdt_old ,
typename operations_type::template rel_error< value_type >( m_eps_abs , m_eps_rel , m_a_x , m_a_dxdt * get_unit_value( dt ) ) );
value_type res = algebra.reduce( x_err ,
typename operations_type::template maximum< value_type >() , static_cast< value_type >( 0 ) );
return res;
}
private:
value_type m_eps_abs;
value_type m_eps_rel;
value_type m_a_x;
value_type m_a_dxdt;
};
/*
* error stepper category dispatcher
*/
template<
class ErrorStepper ,
class ErrorChecker = default_error_checker< typename ErrorStepper::value_type ,
typename ErrorStepper::algebra_type ,
typename ErrorStepper::operations_type > ,
class Resizer = typename ErrorStepper::resizer_type ,
class ErrorStepperCategory = typename ErrorStepper::stepper_category
>
class controlled_runge_kutta ;
/*
* explicit stepper version
*
* this class introduces the following try_step overloads
* try_step( sys , x , t , dt )
* try_step( sys , x , dxdt , t , dt )
* try_step( sys , in , t , out , dt )
* try_step( sys , in , dxdt , t , out , dt )
*/
/**
* \brief Implements step size control for Runge-Kutta steppers with error
* estimation.
*
* This class implements the step size control for standard Runge-Kutta
* steppers with error estimation.
*
* \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
* \tparam ErrorChecker The error checker
* \tparam Resizer The resizer policy type.
*/
template<
class ErrorStepper ,
class ErrorChecker ,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , Resizer , explicit_error_stepper_tag >
{
public:
typedef ErrorStepper stepper_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::algebra_type algebra_type;
typedef typename stepper_type::operations_type operations_type;
typedef Resizer resizer_type;
typedef ErrorChecker error_checker_type;
typedef explicit_controlled_stepper_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif //DOXYGEN_SKIP
/**
* \brief Constructs the controlled Runge-Kutta stepper.
* \param error_checker An instance of the error checker.
* \param stepper An instance of the underlying stepper.
*/
controlled_runge_kutta(
const error_checker_type &error_checker = error_checker_type( ) ,
const stepper_type &stepper = stepper_type( )
)
: m_stepper( stepper ) , m_error_checker( error_checker )
{ }
/*
* Version 1 : try_step( sys , x , t , dt )
*
* The overloads are needed to solve the forwarding problem
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t, dt );
}
/**
* \brief Tries to perform one step. Solves the forwarding problem and
* allows for using boost range as state_type.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful. Can be a boost range.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t, dt );
}
/*
* Version 2 : try_step( sys , x , dxdt , t , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut , class DerivIn >
controlled_step_result try_step( System system , StateInOut &x , const DerivIn &dxdt , time_type &t , time_type &dt )
{
m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , dt );
if( res == success )
{
boost::numeric::odeint::copy( m_xnew.m_v , x );
}
return res;
}
/*
* Version 3 : try_step( sys , in , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*
* the disable is needed to avoid ambiguous overloads if state_type = time_type
*/
/**
* \brief Tries to perform one step.
*
* \note This method is disabled if state_type=time_type to avoid ambiguity.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateIn , class StateOut >
typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
sys( in , m_dxdt.m_v , t );
return try_step( system , in , m_dxdt.m_v , t , out , dt );
}
/*
* Version 4 : try_step( sys , in , dxdt , t , out , dt )
*
* this version does not solve the forwarding problem, boost.range can not be used
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateIn , class DerivIn , class StateOut >
controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt , time_type &t , StateOut &out , time_type &dt )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
// do one step with error calculation
m_stepper.do_step( system , in , dxdt , t , out , dt , m_xerr.m_v );
m_max_rel_error = m_error_checker.error( m_stepper.algebra() , in , dxdt , m_xerr.m_v , dt );
if( m_max_rel_error > 1.0 )
{
// error too large - decrease dt ,limit scaling factor to 0.2 and reset state
dt *= max BOOST_PREVENT_MACRO_SUBSTITUTION ( static_cast<value_type>(9)/static_cast<value_type>(10) * pow( m_max_rel_error ,
static_cast<value_type>(-1) / ( m_stepper.error_order() - 1 ) ) ,
static_cast<value_type>(1)/static_cast<value_type> (5) );
return fail;
}
else
{
if( m_max_rel_error < 0.5 )
{
// error should be > 0
m_max_rel_error = max BOOST_PREVENT_MACRO_SUBSTITUTION ( pow( 5.0 , -m_stepper.stepper_order() ) , m_max_rel_error );
//error too small - increase dt and keep the evolution and limit scaling factor to 5.0
t += dt;
dt *= static_cast<value_type>(9)/static_cast<value_type>(10) * pow( m_max_rel_error ,
static_cast<value_type>(-1) / m_stepper.stepper_order() );
return success;
}
else
{
t += dt;
return success;
}
}
}
/**
* \brief Returns the error of the last step.
*
* returns The last error of the step.
*/
value_type last_error( void ) const
{
return m_max_rel_error;
}
/**
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
template< class StateType >
void adjust_size( const StateType &x )
{
resize_m_xerr_impl( x );
resize_m_dxdt_impl( x );
resize_m_xnew_impl( x );
m_stepper.adjust_size( x );
}
/**
* \brief Returns the instance of the underlying stepper.
* \returns The instance of the underlying stepper.
*/
stepper_type& stepper( void )
{
return m_stepper;
}
/**
* \brief Returns the instance of the underlying stepper.
* \returns The instance of the underlying stepper.
*/
const stepper_type& stepper( void ) const
{
return m_stepper;
}
private:
template< class System , class StateInOut >
controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
{
typename odeint::unwrap_reference< System >::type &sys = system;
m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
sys( x , m_dxdt.m_v ,t );
return try_step( system , x , m_dxdt.m_v , t , dt );
}
template< class StateIn >
bool resize_m_xerr_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
}
template< class StateIn >
bool resize_m_dxdt_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
template< class StateIn >
bool resize_m_xnew_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
}
stepper_type m_stepper;
error_checker_type m_error_checker;
resizer_type m_dxdt_resizer;
resizer_type m_xerr_resizer;
resizer_type m_xnew_resizer;
wrapped_deriv_type m_dxdt;
wrapped_state_type m_xerr;
wrapped_state_type m_xnew;
value_type m_max_rel_error;
};
/*
* explicit stepper fsal version
*
* the class introduces the following try_step overloads
* try_step( sys , x , t , dt )
* try_step( sys , in , t , out , dt )
* try_step( sys , x , dxdt , t , dt )
* try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
*/
/**
* \brief Implements step size control for Runge-Kutta FSAL steppers with
* error estimation.
*
* This class implements the step size control for FSAL Runge-Kutta
* steppers with error estimation.
*
* \tparam ErrorStepper The stepper type with error estimation, has to fulfill the ErrorStepper concept.
* \tparam ErrorChecker The error checker
* \tparam Resizer The resizer policy type.
*/
template<
class ErrorStepper ,
class ErrorChecker ,
class Resizer
>
class controlled_runge_kutta< ErrorStepper , ErrorChecker , Resizer , explicit_error_stepper_fsal_tag >
{
public:
typedef ErrorStepper stepper_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::algebra_type algebra_type;
typedef typename stepper_type::operations_type operations_type;
typedef Resizer resizer_type;
typedef ErrorChecker error_checker_type;
typedef explicit_controlled_stepper_fsal_tag stepper_category;
#ifndef DOXYGEN_SKIP
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef controlled_runge_kutta< ErrorStepper , ErrorChecker , Resizer , explicit_error_stepper_tag > controlled_stepper_type;
#endif // DOXYGEN_SKIP
/**
* \brief Constructs the controlled Runge-Kutta stepper.
* \param error_checker An instance of the error checker.
* \param stepper An instance of the underlying stepper.
*/
controlled_runge_kutta(
const error_checker_type &error_checker = error_checker_type() ,
const stepper_type &stepper = stepper_type()
)
: m_stepper( stepper ) , m_error_checker( error_checker ) ,
m_first_call( true )
{ }
/*
* Version 1 : try_step( sys , x , t , dt )
*
* The two overloads are needed in order to solve the forwarding problem
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t , dt );
}
/**
* \brief Tries to perform one step. Solves the forwarding problem and
* allows for using boost range as state_type.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful. Can be a boost range.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut >
controlled_step_result try_step( System system , const StateInOut &x , time_type &t , time_type &dt )
{
return try_step_v1( system , x , t , dt );
}
/*
* Version 2 : try_step( sys , in , t , out , dt );
*
* This version does not solve the forwarding problem, boost::range can not be used.
*
* The disabler is needed to solve ambiguous overloads
*/
/**
* \brief Tries to perform one step.
*
* \note This method is disabled if state_type=time_type to avoid ambiguity.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateIn , class StateOut >
typename boost::disable_if< boost::is_same< StateIn , time_type > , controlled_step_result >::type
try_step( System system , const StateIn &in , time_type &t , StateOut &out , time_type &dt )
{
if( m_dxdt_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateIn > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , in , t );
}
return try_step( system , in , m_dxdt.m_v , t , out , dt );
}
/*
* Version 3 : try_step( sys , x , dxdt , t , dt )
*
* This version does not solve the forwarding problem, boost::range can not be used.
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The state of the ODE which should be solved. Overwritten if
* the step is successful.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateInOut , class DerivInOut >
controlled_step_result try_step( System system , StateInOut &x , DerivInOut &dxdt , time_type &t , time_type &dt )
{
m_xnew_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_xnew_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
m_dxdt_new_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_new_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) );
controlled_step_result res = try_step( system , x , dxdt , t , m_xnew.m_v , m_dxdtnew.m_v , dt );
if( res == success )
{
boost::numeric::odeint::copy( m_xnew.m_v , x );
boost::numeric::odeint::copy( m_dxdtnew.m_v , dxdt );
}
return res;
}
/*
* Version 4 : try_step( sys , in , dxdt_in , t , out , dxdt_out , dt )
*
* This version does not solve the forwarding problem, boost::range can not be used.
*/
/**
* \brief Tries to perform one step.
*
* This method tries to do one step with step size dt. If the error estimate
* is to large, the step is rejected and the method returns fail and the
* step size dt is reduced. If the error estimate is acceptably small, the
* step is performed, success is returned and dt might be increased to make
* the steps as large as possible. This method also updates t if a step is
* performed.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved.
* \param dxdt The derivative of state.
* \param t The value of the time. Updated if the step is successful.
* \param out Used to store the result of the step.
* \param dt The step size. Updated.
* \return success if the step was accepted, fail otherwise.
*/
template< class System , class StateIn , class DerivIn , class StateOut , class DerivOut >
controlled_step_result try_step( System system , const StateIn &in , const DerivIn &dxdt_in , time_type &t ,
StateOut &out , DerivOut &dxdt_out , time_type &dt )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
m_xerr_resizer.adjust_size( in , detail::bind( &controlled_runge_kutta::template resize_m_xerr_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
//fsal: m_stepper.get_dxdt( dxdt );
//fsal: m_stepper.do_step( sys , x , dxdt , t , dt , m_x_err );
m_stepper.do_step( system , in , dxdt_in , t , out , dxdt_out , dt , m_xerr.m_v );
// this potentially overwrites m_x_err! (standard_error_checker does, at least)
value_type max_rel_err = m_error_checker.error( m_stepper.algebra() , in , dxdt_in , m_xerr.m_v , dt );
if( max_rel_err > 1.0 )
{
// error too large - decrease dt ,limit scaling factor to 0.2 and reset state
dt *= max BOOST_PREVENT_MACRO_SUBSTITUTION ( static_cast<value_type>( static_cast<value_type>(9)/static_cast<value_type>(10) * pow( max_rel_err , static_cast<value_type>(-1) / ( m_stepper.error_order() - 1 ) ) ) , static_cast<value_type>( static_cast<value_type>(1)/static_cast<value_type> (5)) );
return fail;
}
else
{
if( max_rel_err < 0.5 )
{ //error too small - increase dt and keep the evolution and limit scaling factor to 5.0
// error should be > 0
max_rel_err = max BOOST_PREVENT_MACRO_SUBSTITUTION ( pow( 5.0 , -m_stepper.stepper_order() ) , max_rel_err );
t += dt;
dt *= static_cast<value_type>( static_cast<value_type>(9)/static_cast<value_type>(10) * pow( max_rel_err , static_cast<value_type>(-1) / m_stepper.stepper_order() ) );
return success;
}
else
{
t += dt;
return success;
}
}
}
/**
* \brief Resets the internal state of the underlying FSAL stepper.
*/
void reset( void )
{
m_first_call = true;
}
/**
* \brief Initializes the internal state storing an internal copy of the derivative.
*
* \param deriv The initial derivative of the ODE.
*/
template< class DerivIn >
void initialize( const DerivIn &deriv )
{
boost::numeric::odeint::copy( deriv , m_dxdt.m_v );
m_first_call = false;
}
/**
* \brief Initializes the internal state storing an internal copy of the derivative.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param x The initial state of the ODE which should be solved.
* \param t The initial time.
*/
template< class System , class StateIn >
void initialize( System system , const StateIn &x , time_type t )
{
typename odeint::unwrap_reference< System >::type &sys = system;
sys( x , m_dxdt.m_v , t );
m_first_call = false;
}
/**
* \brief Returns true if the stepper has been initialized, false otherwise.
*
* \return true, if the stepper has been initialized, false otherwise.
*/
bool is_initialized( void ) const
{
return ! m_first_call;
}
/**
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
template< class StateType >
void adjust_size( const StateType &x )
{
resize_m_xerr_impl( x );
resize_m_dxdt_impl( x );
resize_m_dxdt_new_impl( x );
resize_m_xnew_impl( x );
}
/**
* \brief Returns the instance of the underlying stepper.
* \returns The instance of the underlying stepper.
*/
stepper_type& stepper( void )
{
return m_stepper;
}
/**
* \brief Returns the instance of the underlying stepper.
* \returns The instance of the underlying stepper.
*/
const stepper_type& stepper( void ) const
{
return m_stepper;
}
private:
template< class StateIn >
bool resize_m_xerr_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
}
template< class StateIn >
bool resize_m_dxdt_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
}
template< class StateIn >
bool resize_m_dxdt_new_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
}
template< class StateIn >
bool resize_m_xnew_impl( const StateIn &x )
{
return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
}
template< class System , class StateInOut >
controlled_step_result try_step_v1( System system , StateInOut &x , time_type &t , time_type &dt )
{
if( m_dxdt_resizer.adjust_size( x , detail::bind( &controlled_runge_kutta::template resize_m_dxdt_impl< StateInOut > , detail::ref( *this ) , detail::_1 ) ) || m_first_call )
{
initialize( system , x , t );
}
return try_step( system , x , m_dxdt.m_v , t , dt );
}
stepper_type m_stepper;
error_checker_type m_error_checker;
resizer_type m_dxdt_resizer;
resizer_type m_xerr_resizer;
resizer_type m_xnew_resizer;
resizer_type m_dxdt_new_resizer;
wrapped_deriv_type m_dxdt;
wrapped_state_type m_xerr;
wrapped_state_type m_xnew;
wrapped_deriv_type m_dxdtnew;
bool m_first_call;
};
/********** DOXYGEN **********/
/**** DEFAULT ERROR CHECKER ****/
/**
* \class default_error_checker
* \brief The default error checker to be used with Runge-Kutta error steppers
*
* This class provides the default mechanism to compare the error estimates
* reported by Runge-Kutta error steppers with user defined error bounds.
* It is used by the controlled_runge_kutta steppers.
*
* \tparam Value The value type.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
*/
/**
* \fn default_error_checker( value_type eps_abs , value_type eps_rel , value_type a_x , value_type a_dxdt )
* \brief Constructs the error checker.
*
* The error is calculated as follows: ????
*
* \param eps_abs Absolute tolerance level.
* \param eps_rel Relative tolerance level.
* \param a_x Factor for the weight of the state.
* \param a_dxdt Factor for the weight of the derivative.
*/
/**
* \fn error( const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
* \brief Calculates the error level.
*
* If the returned error level is greater than 1, the estimated error was
* larger than the permitted error bounds and the step should be repeated
* with a smaller step size.
*
* \param x_old State at the beginning of the step.
* \param dxdt_old Derivative at the beginning of the step.
* \param x_err Error estimate.
* \param dt Time step.
* \return error
*/
/**
* \fn error( algebra_type &algebra , const State &x_old , const Deriv &dxdt_old , Err &x_err , Time dt ) const
* \brief Calculates the error level using a given algebra.
*
* If the returned error level is greater than 1, the estimated error was
* larger than the permitted error bounds and the step should be repeated
* with a smaller step size.
*
* \param algebra The algebra used for calculation of the error.
* \param x_old State at the beginning of the step.
* \param dxdt_old Derivative at the beginning of the step.
* \param x_err Error estimate.
* \param dt Time step.
* \return error
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED

42
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/controlled_step_result.hpp Executable file
View File

@@ -0,0 +1,42 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/controlled_step_result.hpp
[begin_description]
Defines the result type for all controlled stepper.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_STEP_RESULT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_STEP_RESULT_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
/**
* \enum controlled_step_result
*
* \brief Enum representing the return values of the controlled steppers.
*/
typedef enum
{
success , /**< The trial step was successful, hence the state and the time have been advanced. */
fail /**< The step was not successful and might possibly be repeated with a small step size. */
} controlled_step_result;
} // namespace odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_CONTROLLED_STEP_RESULT_HPP_INCLUDED

461
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp Executable file
View File

@@ -0,0 +1,461 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp
[begin_description]
Implementation of the Dense-output stepper for all steppers. Note, that this class does
not computes the result but serves as an interface.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED
#include <utility>
#include <stdexcept>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class Stepper , class StepperCategory = typename Stepper::stepper_category >
class dense_output_runge_kutta;
/**
* \brief The class representing dense-output Runge-Kutta steppers.
* \note In this stepper, the initialize method has to be called before using
* the do_step method.
*
* The dense-output functionality allows to interpolate the solution between
* subsequent integration points using intermediate results obtained during the
* computation. This version works based on a normal stepper without step-size
* control.
*
*
* \tparam Stepper The stepper type of the underlying algorithm.
*/
template< class Stepper >
class dense_output_runge_kutta< Stepper , stepper_tag >
{
public:
/*
* We do not need all typedefs.
*/
typedef Stepper stepper_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::algebra_type algebra_type;
typedef typename stepper_type::operations_type operations_type;
typedef typename stepper_type::resizer_type resizer_type;
typedef dense_output_stepper_tag stepper_category;
typedef dense_output_runge_kutta< Stepper > dense_output_stepper_type;
/**
* \brief Constructs the dense_output_runge_kutta class. An instance of the
* underlying stepper can be provided.
* \param stepper An instance of the underlying stepper.
*/
dense_output_runge_kutta( const stepper_type &stepper = stepper_type() )
: m_stepper( stepper ) , m_resizer() ,
m_x1() , m_x2() , m_current_state_x1( true ) ,
m_t() , m_t_old() , m_dt()
{ }
/**
* \brief Initializes the stepper. Has to be called before do_step can be
* used to set the initial conditions and the step size.
* \param x0 The initial state of the ODE which should be solved.
* \param t0 The initial time, at which the step should be performed.
* \param dt0 The step size.
*/
template< class StateType >
void initialize( const StateType &x0 , time_type t0 , time_type dt0 )
{
m_resizer.adjust_size( x0 , detail::bind( &dense_output_stepper_type::template resize_impl< StateType > , detail::ref( *this ) , detail::_1 ) );
boost::numeric::odeint::copy( x0 , get_current_state() );
m_t = t0;
m_dt = dt0;
}
/**
* \brief Does one time step.
* \note initialize has to be called before using this method to set the
* initial conditions x,t and the stepsize.
* \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
* Simple System concept.
* \return Pair with start and end time of the integration step.
*/
template< class System >
std::pair< time_type , time_type > do_step( System system )
{
m_stepper.do_step( system , get_current_state() , m_t , get_old_state() , m_dt );
m_t_old = m_t;
m_t += m_dt;
toggle_current_state();
return std::make_pair( m_t_old , m_dt );
}
/*
* The next two overloads are needed to solve the forwarding problem
*/
/**
* \brief Calculates the solution at an intermediate point.
* \param t The time at which the solution should be calculated, has to be
* in the current time interval.
* \param x The output variable where the result is written into.
*/
template< class StateOut >
void calc_state( time_type t , StateOut &x ) const
{
m_stepper.calc_state( x , t , get_old_state() , m_t_old , get_current_state() , m_t );
}
/**
* \brief Calculates the solution at an intermediate point. Solves the forwarding problem
* \param t The time at which the solution should be calculated, has to be
* in the current time interval.
* \param x The output variable where the result is written into, can be a boost range.
*/
template< class StateOut >
void calc_state( time_type t , const StateOut &x ) const
{
m_stepper.calc_state( x , t , get_old_state() , m_t_old , get_current_state() , m_t );
}
/**
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
m_stepper.stepper().resize( x );
}
/**
* \brief Returns the current state of the solution.
* \return The current state of the solution x(t).
*/
const state_type& current_state( void ) const
{
return get_current_state();
}
/**
* \brief Returns the current time of the solution.
* \return The current time of the solution t.
*/
time_type current_time( void ) const
{
return m_t;
}
/**
* \brief Returns the last state of the solution.
* \return The last state of the solution x(t-dt).
*/
const state_type& previous_state( void ) const
{
return get_old_state();
}
/**
* \brief Returns the last time of the solution.
* \return The last time of the solution t-dt.
*/
time_type previous_time( void ) const
{
return m_t_old;
}
private:
state_type& get_current_state( void )
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
const state_type& get_current_state( void ) const
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
state_type& get_old_state( void )
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
const state_type& get_old_state( void ) const
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
void toggle_current_state( void )
{
m_current_state_x1 = ! m_current_state_x1;
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
return resized;
}
stepper_type m_stepper;
resizer_type m_resizer;
wrapped_state_type m_x1 , m_x2;
bool m_current_state_x1; // if true, the current state is m_x1
time_type m_t , m_t_old , m_dt;
};
/**
* \brief The class representing dense-output Runge-Kutta steppers with FSAL property.
*
* The interface is the same as for dense_output_runge_kutta< Stepper , stepper_tag >.
* This class provides dense output functionality based on methods with step size controlled
*
*
* \tparam Stepper The stepper type of the underlying algorithm.
*/
template< class Stepper >
class dense_output_runge_kutta< Stepper , explicit_controlled_stepper_fsal_tag >
{
public:
/*
* We do not need all typedefs.
*/
typedef Stepper controlled_stepper_type;
typedef typename controlled_stepper_type::stepper_type stepper_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::algebra_type algebra_type;
typedef typename stepper_type::operations_type operations_type;
typedef typename stepper_type::resizer_type resizer_type;
typedef dense_output_stepper_tag stepper_category;
typedef dense_output_runge_kutta< Stepper > dense_output_stepper_type;
dense_output_runge_kutta( const controlled_stepper_type &stepper = controlled_stepper_type() )
: m_stepper( stepper ) , m_resizer() ,
m_current_state_x1( true ) ,
m_x1() , m_x2() , m_dxdt1() , m_dxdt2() ,
m_t() , m_t_old() , m_dt() ,
m_is_deriv_initialized( false )
{ }
template< class StateType >
void initialize( const StateType &x0 , time_type t0 , time_type dt0 )
{
m_resizer.adjust_size( x0 , detail::bind( &dense_output_stepper_type::template resize< StateType > , detail::ref( *this ) , detail::_1 ) );
boost::numeric::odeint::copy( x0 , get_current_state() );
m_t = t0;
m_dt = dt0;
m_is_deriv_initialized = false;
}
template< class System >
std::pair< time_type , time_type > do_step( System system )
{
const size_t max_count = 1000;
if( !m_is_deriv_initialized )
{
typename odeint::unwrap_reference< System >::type &sys = system;
sys( get_current_state() , get_current_deriv() , m_t );
m_is_deriv_initialized = true;
}
controlled_step_result res = fail;
m_t_old = m_t;
size_t count = 0;
do
{
res = m_stepper.try_step( system , get_current_state() , get_current_deriv() , m_t ,
get_old_state() , get_old_deriv() , m_dt );
if( count++ == max_count )
throw std::overflow_error( "dense_output_controlled_explicit : too much iterations!");
}
while( res == fail );
toggle_current_state();
return std::make_pair( m_t_old , m_t );
}
/*
* The two overloads are needed in order to solve the forwarding problem.
*/
template< class StateOut >
void calc_state( time_type t , StateOut &x ) const
{
m_stepper.stepper().calc_state( t , x , get_old_state() , get_old_deriv() , m_t_old ,
get_current_state() , get_current_deriv() , m_t );
}
template< class StateOut >
void calc_state( time_type t , const StateOut &x ) const
{
m_stepper.stepper().calc_state( t , x , get_old_state() , get_old_deriv() , m_t_old ,
get_current_state() , get_current_deriv() , m_t );
}
template< class StateIn >
bool resize( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdt1 , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdt2 , x , typename is_resizeable<deriv_type>::type() );
return resized;
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize( x );
m_stepper.stepper().resize( x );
}
const state_type& current_state( void ) const
{
return get_current_state();
}
time_type current_time( void ) const
{
return m_t;
}
const state_type& previous_state( void ) const
{
return get_old_state();
}
time_type previous_time( void ) const
{
return m_t_old;
}
time_type current_time_step( void ) const
{
return m_dt;
}
private:
state_type& get_current_state( void )
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
const state_type& get_current_state( void ) const
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
state_type& get_old_state( void )
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
const state_type& get_old_state( void ) const
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
deriv_type& get_current_deriv( void )
{
return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
}
const deriv_type& get_current_deriv( void ) const
{
return m_current_state_x1 ? m_dxdt1.m_v : m_dxdt2.m_v ;
}
deriv_type& get_old_deriv( void )
{
return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
}
const deriv_type& get_old_deriv( void ) const
{
return m_current_state_x1 ? m_dxdt2.m_v : m_dxdt1.m_v ;
}
void toggle_current_state( void )
{
m_current_state_x1 = ! m_current_state_x1;
}
controlled_stepper_type m_stepper;
resizer_type m_resizer;
bool m_current_state_x1;
wrapped_state_type m_x1 , m_x2;
wrapped_deriv_type m_dxdt1 , m_dxdt2;
time_type m_t , m_t_old , m_dt;
bool m_is_deriv_initialized;
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED

151
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/adams_bashforth_call_algebra.hpp Executable file
View File

@@ -0,0 +1,151 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/adams_bashforth_call_algebra.hpp
[begin_description]
Algebra caller for the Adams Bashforth stepper.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED
#include <boost/assert.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< size_t Step , class Algebra , class Operations >
struct adams_bashforth_call_algebra;
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 1 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each3( out , in , steps[0].m_v , typename Operations::template scale_sum2< value_type , Time >( 1.0 , dt * coef[0] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 2 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each4( out , in , steps[0].m_v , steps[1].m_v ,
typename Operations::template scale_sum3< value_type , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 3 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each5( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v ,
typename Operations::template scale_sum4< value_type , Time , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 4 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each6( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v ,
typename Operations::template scale_sum5< value_type , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 5 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each7( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v ,
typename Operations::template scale_sum6< value_type , Time , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 6 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each8( out , in , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v , steps[5].m_v ,
typename Operations::template scale_sum7< value_type , Time , Time , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 7 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
BOOST_ASSERT( false ); // not implemented
// typedef typename Coefficients::value_type value_type;
// Algebra::for_each9( out , in , steps[0] , steps[1] , steps[2] , steps[3] , steps[4] , steps[5] , steps[6]
// typename Operations::template scale_sum8< value_type , Time , Time , Time , Time , Time , Time >(
// 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] , dt * coef[6] ) );
}
};
template< class Algebra , class Operations >
struct adams_bashforth_call_algebra< 8 , Algebra , Operations >
{
template< class StateIn , class StateOut , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
BOOST_ASSERT( false ); // not implemented
// typedef typename Coefficients::value_type value_type;
// Algebra::for_each10( out , in , steps[0] , steps[1] , steps[2] , steps[3] , steps[4] , steps[5] , steps[6] , steps[7] ,
// typename Operations::template scale_sum9< value_type , Time , Time , Time , Time , Time , Time , Time >(
// 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] , dt * coef[6] , dt * coef[7] ) );
}
};
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_CALL_ALGEBRA_HPP_INCLUDED

168
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp Executable file
View File

@@ -0,0 +1,168 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/adams_bashforth_coefficients.hpp
[begin_description]
Definition of the coefficients for the Adams-Bashforth method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED
#include <boost/array.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class Value , size_t Steps >
class adams_bashforth_coefficients ;
template< class Value >
class adams_bashforth_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 1 >()
{
(*this)[0] = static_cast< Value >( 1 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 2 >()
{
(*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 2 );
(*this)[1] = -static_cast< Value >( 1 ) / static_cast< Value >( 2 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 3 >()
{
(*this)[0] = static_cast< Value >( 23 ) / static_cast< Value >( 12 );
(*this)[1] = -static_cast< Value >( 4 ) / static_cast< Value >( 3 );
(*this)[2] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 4 >()
{
(*this)[0] = static_cast< Value >( 55 ) / static_cast< Value >( 24 );
(*this)[1] = -static_cast< Value >( 59 ) / static_cast< Value >( 24 );
(*this)[2] = static_cast< Value >( 37 ) / static_cast< Value >( 24 );
(*this)[3] = -static_cast< Value >( 3 ) / static_cast< Value >( 8 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 5 >()
{
(*this)[0] = static_cast< Value >( 1901 ) / static_cast< Value >( 720 );
(*this)[1] = -static_cast< Value >( 1387 ) / static_cast< Value >( 360 );
(*this)[2] = static_cast< Value >( 109 ) / static_cast< Value >( 30 );
(*this)[3] = -static_cast< Value >( 637 ) / static_cast< Value >( 360 );
(*this)[4] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 6 >()
{
(*this)[0] = static_cast< Value >( 4277 ) / static_cast< Value >( 1440 );
(*this)[1] = -static_cast< Value >( 2641 ) / static_cast< Value >( 480 );
(*this)[2] = static_cast< Value >( 4991 ) / static_cast< Value >( 720 );
(*this)[3] = -static_cast< Value >( 3649 ) / static_cast< Value >( 720 );
(*this)[4] = static_cast< Value >( 959 ) / static_cast< Value >( 480 );
(*this)[5] = -static_cast< Value >( 95 ) / static_cast< Value >( 288 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 7 >()
{
(*this)[0] = static_cast< Value >( 198721 ) / static_cast< Value >( 60480 );
(*this)[1] = -static_cast< Value >( 18637 ) / static_cast< Value >( 2520 );
(*this)[2] = static_cast< Value >( 235183 ) / static_cast< Value >( 20160 );
(*this)[3] = -static_cast< Value >( 10754 ) / static_cast< Value >( 945 );
(*this)[4] = static_cast< Value >( 135713 ) / static_cast< Value >( 20160 );
(*this)[5] = -static_cast< Value >( 5603 ) / static_cast< Value >( 2520 );
(*this)[6] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
}
};
template< class Value >
class adams_bashforth_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
adams_bashforth_coefficients( void )
: boost::array< Value , 8 >()
{
(*this)[0] = static_cast< Value >( 16083 ) / static_cast< Value >( 4480 );
(*this)[1] = -static_cast< Value >( 1152169 ) / static_cast< Value >( 120960 );
(*this)[2] = static_cast< Value >( 242653 ) / static_cast< Value >( 13440 );
(*this)[3] = -static_cast< Value >( 296053 ) / static_cast< Value >( 13440 );
(*this)[4] = static_cast< Value >( 2102243 ) / static_cast< Value >( 120960 );
(*this)[5] = -static_cast< Value >( 115747 ) / static_cast< Value >( 13440 );
(*this)[6] = static_cast< Value >( 32863 ) / static_cast< Value >( 13440 );
(*this)[7] = -static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
}
};
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_BASHFORTH_COEFFICIENTS_HPP_INCLUDED

142
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/adams_moulton_call_algebra.hpp Executable file
View File

@@ -0,0 +1,142 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/adams_moulton_call_algebra.hpp
[begin_description]
Algebra caller for the Adams Moulton method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_CALL_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_CALL_ALGEBRA_HPP_INCLUDED
#include <boost/assert.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< size_t Step , class Algebra , class Operations >
struct adams_moulton_call_algebra;
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 1 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each3( out , in , dxdt , typename Operations::template scale_sum2< value_type , Time >( 1.0 , dt * coef[0] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 2 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each4( out , in , dxdt , steps[0].m_v ,
typename Operations::template scale_sum3< value_type , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 3 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each5( out , in , dxdt , steps[0].m_v , steps[1].m_v ,
typename Operations::template scale_sum4< value_type , Time , Time >( 1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 4 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each6( out , in , dxdt , steps[0].m_v , steps[1].m_v , steps[2].m_v ,
typename Operations::template scale_sum5< value_type , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 5 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each7( out , in , dxdt , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v ,
typename Operations::template scale_sum6< value_type , Time , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 6 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
typedef typename Coefficients::value_type value_type;
algebra.for_each8( out , in , dxdt , steps[0].m_v , steps[1].m_v , steps[2].m_v , steps[3].m_v , steps[4].m_v ,
typename Operations::template scale_sum7< value_type , Time , Time , Time , Time , Time >(
1.0 , dt * coef[0] , dt * coef[1] , dt * coef[2] , dt * coef[3] , dt * coef[4] , dt * coef[5] ) );
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 7 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
BOOST_ASSERT( false ); // not implemented
}
};
template< class Algebra , class Operations >
struct adams_moulton_call_algebra< 8 , Algebra , Operations >
{
template< class StateIn , class StateOut , class DerivIn , class StepStorage , class Coefficients , class Time >
void operator()( Algebra &algebra , const StateIn &in , StateOut &out , const DerivIn &dxdt , const StepStorage &steps , const Coefficients &coef , Time dt ) const
{
BOOST_ASSERT( false ); // not implemented
}
};
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_CALL_ALGEBRA_HPP_INCLUDED

168
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp Executable file
View File

@@ -0,0 +1,168 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/adams_moulton_coefficients.hpp
[begin_description]
Coefficients for the Adams Moulton method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED
#include <boost/array.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class Value , size_t Steps >
class adams_moulton_coefficients ;
template< class Value >
class adams_moulton_coefficients< Value , 1 > : public boost::array< Value , 1 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 1 >()
{
(*this)[0] = static_cast< Value >( 1 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 2 > : public boost::array< Value , 2 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 2 >()
{
(*this)[0] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
(*this)[1] = static_cast< Value >( 1 ) / static_cast< Value >( 2 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 3 > : public boost::array< Value , 3 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 3 >()
{
(*this)[0] = static_cast< Value >( 5 ) / static_cast< Value >( 12 );
(*this)[1] = static_cast< Value >( 2 ) / static_cast< Value >( 3 );
(*this)[2] = -static_cast< Value >( 1 ) / static_cast< Value >( 12 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 4 > : public boost::array< Value , 4 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 4 >()
{
(*this)[0] = static_cast< Value >( 3 ) / static_cast< Value >( 8 );
(*this)[1] = static_cast< Value >( 19 ) / static_cast< Value >( 24 );
(*this)[2] = -static_cast< Value >( 5 ) / static_cast< Value >( 24 );
(*this)[3] = static_cast< Value >( 1 ) / static_cast< Value >( 24 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 5 > : public boost::array< Value , 5 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 5 >()
{
(*this)[0] = static_cast< Value >( 251 ) / static_cast< Value >( 720 );
(*this)[1] = static_cast< Value >( 323 ) / static_cast< Value >( 360 );
(*this)[2] = -static_cast< Value >( 11 ) / static_cast< Value >( 30 );
(*this)[3] = static_cast< Value >( 53 ) / static_cast< Value >( 360 );
(*this)[4] = -static_cast< Value >( 19 ) / static_cast< Value >( 720 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 6 > : public boost::array< Value , 6 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 6 >()
{
(*this)[0] = static_cast< Value >( 95 ) / static_cast< Value >( 288 );
(*this)[1] = static_cast< Value >( 1427 ) / static_cast< Value >( 1440 );
(*this)[2] = -static_cast< Value >( 133 ) / static_cast< Value >( 240 );
(*this)[3] = static_cast< Value >( 241 ) / static_cast< Value >( 720 );
(*this)[4] = -static_cast< Value >( 173 ) / static_cast< Value >( 1440 );
(*this)[5] = static_cast< Value >( 3 ) / static_cast< Value >( 160 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 7 > : public boost::array< Value , 7 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 7 >()
{
(*this)[0] = static_cast< Value >( 19087 ) / static_cast< Value >( 60480 );
(*this)[1] = static_cast< Value >( 2713 ) / static_cast< Value >( 2520 );
(*this)[2] = -static_cast< Value >( 15487 ) / static_cast< Value >( 20160 );
(*this)[3] = static_cast< Value >( 586 ) / static_cast< Value >( 945 );
(*this)[4] = -static_cast< Value >( 6737 ) / static_cast< Value >( 20160 );
(*this)[5] = static_cast< Value >( 263 ) / static_cast< Value >( 2520 );
(*this)[6] = -static_cast< Value >( 863 ) / static_cast< Value >( 60480 );
}
};
template< class Value >
class adams_moulton_coefficients< Value , 8 > : public boost::array< Value , 8 >
{
public:
adams_moulton_coefficients( void )
: boost::array< Value , 8 >()
{
(*this)[0] = static_cast< Value >( 5257 ) / static_cast< Value >( 17280 );
(*this)[1] = static_cast< Value >( 139849 ) / static_cast< Value >( 120960 );
(*this)[2] = -static_cast< Value >( 4511 ) / static_cast< Value >( 4480 );
(*this)[3] = static_cast< Value >( 123133 ) / static_cast< Value >( 120960 );
(*this)[4] = -static_cast< Value >( 88547 ) / static_cast< Value >( 120960 );
(*this)[5] = static_cast< Value >( 1537 ) / static_cast< Value >( 4480 );
(*this)[6] = -static_cast< Value >( 11351 ) / static_cast< Value >( 120960 );
(*this)[7] = static_cast< Value >( 275 ) / static_cast< Value >( 24192 );
}
};
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ADAMS_MOULTON_COEFFICIENTS_HPP_INCLUDED

246
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp Executable file
View File

@@ -0,0 +1,246 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp
[begin_description]
Implementation of the generic Runge-Kutta method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED
#include <boost/static_assert.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/range_c.hpp>
#include <boost/mpl/copy.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/fusion/algorithm.hpp>
#include <boost/fusion/iterator.hpp>
#include <boost/fusion/mpl.hpp>
#include <boost/fusion/sequence.hpp>
#include <boost/array.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class T , class Constant >
struct array_wrapper
{
typedef const typename boost::array< T , Constant::value > type;
};
template< class T , size_t i >
struct stage
{
T c;
boost::array< T , i > a;
};
template< class T , class Constant >
struct stage_wrapper
{
typedef stage< T , Constant::value > type;
};
template<
size_t StageCount,
class Value = double ,
class Algebra = range_algebra,
class Operations = default_operations
>
class generic_rk_algorithm {
public:
typedef mpl::range_c< size_t , 1 , StageCount > stage_indices;
typedef typename boost::fusion::result_of::as_vector
<
typename boost::mpl::copy
<
stage_indices ,
boost::mpl::inserter
<
boost::mpl::vector0< > ,
boost::mpl::push_back< boost::mpl::_1 , array_wrapper< Value , boost::mpl::_2 > >
>
>::type
>::type coef_a_type;
typedef boost::array< Value , StageCount > coef_b_type;
typedef boost::array< Value , StageCount > coef_c_type;
typedef typename boost::fusion::result_of::as_vector
<
typename boost::mpl::push_back
<
typename boost::mpl::copy
<
stage_indices,
boost::mpl::inserter
<
boost::mpl::vector0<> ,
boost::mpl::push_back< boost::mpl::_1 , stage_wrapper< Value , boost::mpl::_2 > >
>
>::type ,
stage< Value , StageCount >
>::type
>::type stage_vector_base;
struct stage_vector : public stage_vector_base
{
struct do_insertion
{
stage_vector_base &m_base;
const coef_a_type &m_a;
const coef_c_type &m_c;
do_insertion( stage_vector_base &base , const coef_a_type &a , const coef_c_type &c )
: m_base( base ) , m_a( a ) , m_c( c ) { }
template< class Index >
void operator()( Index ) const
{
//boost::fusion::at< Index >( m_base ) = stage< double , Index::value+1 , intermediate_stage >( m_c[ Index::value ] , boost::fusion::at< Index >( m_a ) );
boost::fusion::at< Index >( m_base ).c = m_c[ Index::value ];
boost::fusion::at< Index >( m_base ).a = boost::fusion::at< Index >( m_a );
}
};
struct print_butcher
{
const stage_vector_base &m_base;
std::ostream &m_os;
print_butcher( const stage_vector_base &base , std::ostream &os )
: m_base( base ) , m_os( os )
{ }
template<class Index>
void operator()(Index) const {
m_os << boost::fusion::at<Index>(m_base).c << " | ";
for( size_t i=0 ; i<Index::value ; ++i )
m_os << boost::fusion::at<Index>(m_base).a[i] << " ";
m_os << std::endl;
}
};
stage_vector( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c )
{
typedef boost::mpl::range_c< size_t , 0 , StageCount-1 > indices;
boost::mpl::for_each< indices >( do_insertion( *this , a , c ) );
boost::fusion::at_c< StageCount - 1 >( *this ).c = c[ StageCount - 1 ];
boost::fusion::at_c< StageCount - 1 >( *this ).a = b;
}
void print( std::ostream &os ) const
{
typedef boost::mpl::range_c< size_t , 0 , StageCount > indices;
boost::mpl::for_each< indices >( print_butcher( *this , os ) );
}
};
template< class System , class StateIn , class StateTemp , class DerivIn , class Deriv ,
class StateOut , class Time >
struct calculate_stage
{
Algebra &algebra;
System &system;
const StateIn &x;
StateTemp &x_tmp;
StateOut &x_out;
const DerivIn &dxdt;
Deriv *F;
Time t;
Time dt;
calculate_stage( Algebra &_algebra , System &_system , const StateIn &_x , const DerivIn &_dxdt , StateOut &_out ,
StateTemp &_x_tmp , Deriv *_F , Time _t , Time _dt )
: algebra( _algebra ) , system( _system ) , x( _x ) , x_tmp( _x_tmp ) , x_out( _out) , dxdt( _dxdt ) , F( _F ) , t( _t ) , dt( _dt )
{}
template< typename T , size_t stage_number >
void inline operator()( stage< T , stage_number > const &stage ) const
//typename stage_fusion_wrapper< T , mpl::size_t< stage_number > , intermediate_stage >::type const &stage ) const
{
if( stage_number > 1 )
{
#ifdef BOOST_MSVC
#pragma warning( disable : 4307 34 )
#endif
system( x_tmp , F[stage_number-2].m_v , t + stage.c * dt );
#ifdef BOOST_MSVC
#pragma warning( default : 4307 34 )
#endif
}
//std::cout << stage_number-2 << ", t': " << t + stage.c * dt << std::endl;
if( stage_number < StageCount )
detail::template generic_rk_call_algebra< stage_number , Algebra >()( algebra , x_tmp , x , dxdt , F ,
detail::generic_rk_scale_sum< stage_number , Operations , Value , Time >( stage.a , dt) );
// algebra_type::template for_eachn<stage_number>( x_tmp , x , dxdt , F ,
// typename operations_type::template scale_sumn< stage_number , time_type >( stage.a , dt ) );
else
detail::template generic_rk_call_algebra< stage_number , Algebra >()( algebra , x_out , x , dxdt , F ,
detail::generic_rk_scale_sum< stage_number , Operations , Value , Time >( stage.a , dt ) );
// algebra_type::template for_eachn<stage_number>( x_out , x , dxdt , F ,
// typename operations_type::template scale_sumn< stage_number , time_type >( stage.a , dt ) );
}
};
generic_rk_algorithm( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c )
: m_stages( a , b , c )
{ }
template< class System , class StateIn , class DerivIn , class Time , class StateOut , class StateTemp , class Deriv >
void inline do_step( Algebra &algebra , System system , const StateIn &in , const DerivIn &dxdt ,
Time t , StateOut &out , Time dt ,
StateTemp &x_tmp , Deriv F[StageCount-1] ) const
{
typedef typename odeint::unwrap_reference< System >::type unwrapped_system_type;
unwrapped_system_type &sys = system;
boost::fusion::for_each( m_stages , calculate_stage<
unwrapped_system_type , StateIn , StateTemp , DerivIn , Deriv , StateOut , Time >
( algebra , sys , in , dxdt , out , x_tmp , F , t , dt ) );
}
private:
stage_vector m_stages;
};
}
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_ALGORITHM_HPP_INCLUDED

263
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp Executable file
View File

@@ -0,0 +1,263 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp
[begin_description]
Algebra caller for the generic Runge-Kutta methods.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_CALL_ALGEBRA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_CALL_ALGEBRA_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< size_t StageNumber , class Algebra >
struct generic_rk_call_algebra;
template< class Algebra >
struct generic_rk_call_algebra< 1 , Algebra >
{
typedef Algebra algebra_type;
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( algebra_type &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 * /* s4_array */ , Op op ) const
{
algebra.for_each3( s1 , s2 , s3 , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( algebra_type &algebra , S1 &s1 , S2 &s2 , S4 * /* s4_array */ , Op op ) const
{
algebra.for_each2( s1 , s2 , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 2 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[1] , Op op ) const
{
algebra.for_each4( s1 , s2 , s3 , s4_array[0].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[1] , Op op ) const
{
algebra.for_each3( s1 , s2 , s4_array[0].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 3 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[2] , Op op ) const
{
algebra.for_each5( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[2] , Op op ) const
{
algebra.for_each4( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 4 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[3] , Op op ) const
{
algebra.for_each6( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[3] , Op op ) const
{
algebra.for_each5( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 5 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[4] , Op op ) const
{
algebra.for_each7( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[4] , Op op ) const
{
algebra.for_each6( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 6 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[5] , Op op ) const
{
algebra.for_each8( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[5] , Op op ) const
{
algebra.for_each7( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 7 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[6] , Op op ) const
{
algebra.for_each9( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[6] , Op op ) const
{
algebra.for_each8( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 8 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[7] , Op op ) const
{
algebra.for_each10( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[7] , Op op ) const
{
algebra.for_each9( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 9 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[8] , Op op ) const
{
algebra.for_each11( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[8] , Op op ) const
{
algebra.for_each10( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 10 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[9] , Op op ) const
{
algebra.for_each12( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[9] , Op op ) const
{
algebra.for_each11( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 11 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[10] , Op op ) const
{
algebra.for_each13( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[10] , Op op ) const
{
algebra.for_each12( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 12 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[11] , Op op ) const
{
algebra.for_each14( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , s4_array[10].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[11] , Op op ) const
{
algebra.for_each13( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , s4_array[10].m_v , op );
}
};
template< class Algebra >
struct generic_rk_call_algebra< 13 , Algebra >
{
template< class S1 , class S2 , class S3 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S3 &s3 , S4 s4_array[12] , Op op ) const
{
algebra.for_each15( s1 , s2 , s3 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , s4_array[10].m_v , s4_array[11].m_v , op );
}
template< class S1 , class S2 , class S4 , class Op>
void operator()( Algebra &algebra , S1 &s1 , S2 &s2 , S4 s4_array[12] , Op op ) const
{
algebra.for_each14( s1 , s2 , s4_array[0].m_v , s4_array[1].m_v , s4_array[2].m_v , s4_array[3].m_v , s4_array[4].m_v ,
s4_array[5].m_v , s4_array[6].m_v , s4_array[7].m_v , s4_array[8].m_v , s4_array[9].m_v , s4_array[10].m_v , s4_array[11].m_v , op );
}
};
}
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_CALL_ALGEBRA_HPP_INCLUDED

252
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp Executable file
View File

@@ -0,0 +1,252 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp
[begin_description]
Operations caller for the generic Runge Kutta method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_OPERATIONS_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_OPERATIONS_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< size_t StageNumber , class Operations , class Fac , class Time >
struct generic_rk_scale_sum;
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 1 , Operations , Fac , Time > : public Operations::template scale_sum2< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,1> &a , Time dt ) : Operations::template scale_sum2< Fac , Time >( 1.0 , a[0]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 2 , Operations , Fac , Time > : public Operations::template scale_sum3< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,2> &a , Time dt )
: Operations::template scale_sum3< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 3 , Operations , Fac , Time > : public Operations::template scale_sum4< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,3> &a , Time dt )
: Operations::template scale_sum4< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 4 , Operations , Fac , Time > : public Operations::template scale_sum5< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,4> &a , Time dt )
: Operations::template scale_sum5< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 5 , Operations , Fac , Time > : public Operations::template scale_sum6< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,5> &a , Time dt )
: Operations::template scale_sum6< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 6 , Operations , Fac , Time > : public Operations::template scale_sum7< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,6> &a , Time dt )
: Operations::template scale_sum7< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt , a[5]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 7 , Operations , Fac , Time > : public Operations::template scale_sum8< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,7> &a , Time dt )
: Operations::template scale_sum8< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt , a[5]*dt , a[6]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 8 , Operations , Fac , Time > : public Operations::template scale_sum9< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,8> &a , Time dt )
: Operations::template scale_sum9< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 9 , Operations , Fac , Time > : public Operations::template scale_sum10< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,9> &a , Time dt )
: Operations::template scale_sum10< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt , a[8]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 10 , Operations , Fac , Time > : public Operations::template scale_sum11< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,10> &a , Time dt )
: Operations::template scale_sum11< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt , a[8]*dt , a[9]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 11 , Operations , Fac , Time > : public Operations::template scale_sum12< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,11> &a , Time dt )
: Operations::template scale_sum12< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt , a[8]*dt , a[9]*dt , a[10]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 12 , Operations , Fac , Time > : public Operations::template scale_sum13< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,12> &a , Time dt )
: Operations::template scale_sum13< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt , a[8]*dt , a[9]*dt , a[10]*dt , a[11]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum< 13 , Operations , Fac , Time > : public Operations::template scale_sum14< Fac , Time >
{
generic_rk_scale_sum( const boost::array<Fac,13> &a , Time dt )
: Operations::template scale_sum14< Fac , Time >( 1.0 , a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt ,
a[5]*dt , a[6]*dt , a[7]*dt , a[8]*dt , a[9]*dt , a[10]*dt , a[11]*dt , a[12]*dt )
{ }
typedef void result_type;
};
// for error estimates
template< size_t StageNumber , class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err;
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 1 , Operations , Fac , Time > : public Operations::template scale_sum1< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,1> &a , Time dt ) : Operations::template scale_sum1< Time >( a[0]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 2 , Operations , Fac , Time > : public Operations::template scale_sum2< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,2> &a , Time dt )
: Operations::template scale_sum2< Time >( a[0]*dt , a[1]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 3 , Operations , Fac , Time > : public Operations::template scale_sum3< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,3> &a , Time dt )
: Operations::template scale_sum3< Time >( a[0]*dt , a[1]*dt , a[2]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 4 , Operations , Fac , Time > : public Operations::template scale_sum4< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,4> &a , Time dt )
: Operations::template scale_sum4< Time >( a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 5 , Operations , Fac , Time > : public Operations::template scale_sum5< Fac >
{
generic_rk_scale_sum_err( const boost::array<Fac,5> &a , Time dt )
: Operations::template scale_sum5< Time >( a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt )
{ }
typedef void result_type;
};
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 6 , Operations , Fac , Time > : public Operations::template scale_sum6< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,6> &a , Time dt )
: Operations::template scale_sum6< Time >( a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt , a[5]*dt )
{ }
typedef void result_type;
};
// for rk87
template< class Operations , class Fac , class Time >
struct generic_rk_scale_sum_err< 13 , Operations , Fac , Time > : public Operations::template scale_sum13< Time >
{
generic_rk_scale_sum_err( const boost::array<Fac,13> &a , Time dt )
: Operations::template scale_sum13< Time >( a[0]*dt , a[1]*dt , a[2]*dt , a[3]*dt , a[4]*dt , a[5]*dt ,
a[6]*dt , a[7]*dt , a[8]*dt , a[9]*dt , a[10]*dt , a[11]*dt , a[12]*dt )
{ }
typedef void result_type;
};
}
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_GENERIC_RK_OPERATIONS_HPP_INCLUDED

84
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/detail/rotating_buffer.hpp Executable file
View File

@@ -0,0 +1,84 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/detail/rotating_buffer.hpp
[begin_description]
Implemetation of a rotating (cyclic) buffer for use in the Adam Bashforth stepper
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ROTATING_BUFFER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ROTATING_BUFFER_HPP_INCLUDED
#include <boost/array.hpp>
namespace boost {
namespace numeric {
namespace odeint {
namespace detail {
template< class T , size_t N >
class rotating_buffer
{
public:
typedef T value_type;
const static size_t dim = N;
rotating_buffer( void ) : m_first( 0 )
{ }
size_t size( void ) const
{
return dim;
}
value_type& operator[]( size_t i )
{
return m_data[ get_index( i ) ];
}
const value_type& operator[]( size_t i ) const
{
return m_data[ get_index( i ) ];
}
void rotate( void )
{
if( m_first == 0 )
m_first = dim-1;
else
--m_first;
}
protected:
value_type m_data[N];
private:
size_t get_index( size_t i ) const
{
return ( ( i + m_first ) % dim );
}
size_t m_first;
};
} // detail
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_DETAIL_ROTATING_BUFFER_HPP_INCLUDED

164
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/euler.hpp Executable file
View File

@@ -0,0 +1,164 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/euler.hpp
[begin_description]
Implementation of the classical explicit Euler stepper. This method is really simple and should only
be used for demonstration purposes.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class euler
: public explicit_stepper_base<
euler< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
1 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class euler : public explicit_stepper_base
#endif
{
public :
#ifndef DOXYGEN_SKIP
typedef explicit_stepper_base< euler< State , Value , Deriv , Time , Algebra , Operations , Resizer > , 1 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
#else
typedef explicit_stepper_base< euler< ... > , ... > stepper_base_type;
#endif
typedef typename stepper_base_type::state_type state_type;
typedef typename stepper_base_type::value_type value_type;
typedef typename stepper_base_type::deriv_type deriv_type;
typedef typename stepper_base_type::time_type time_type;
typedef typename stepper_base_type::algebra_type algebra_type;
typedef typename stepper_base_type::operations_type operations_type;
typedef typename stepper_base_type::resizer_type resizer_type;
#ifndef DOXYGEN_SKIP
typedef typename stepper_base_type::stepper_type stepper_type;
typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
#endif
euler( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
{ }
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
{
stepper_base_type::m_algebra.for_each3( out , in , dxdt ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , dt ) );
}
template< class StateOut , class StateIn1 , class StateIn2 >
void calc_state( StateOut &x , time_type t , const StateIn1 &old_state , time_type t_old , const StateIn2 &current_state , time_type t_new ) const
{
const time_type delta = t - t_old;
stepper_base_type::m_algebra.for_each3( x , old_state , stepper_base_type::m_dxdt.m_v ,
typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , delta ) );
}
template< class StateType >
void adjust_size( const StateType &x )
{
stepper_base_type::adjust_size( x );
}
};
/********** DOXYGEN ***********/
/**
* \class euler
* \brief An implementation of the Euler method.
*
* The Euler method is a very simply solver for ordinary differential equations. This method should not be used
* for real applications. It is only useful for demonstration purposes. Step size control is not provided but
* trivial continuous output is available.
*
* This class derives from explicit_stepper_base and inherits its interface via CRTP (current recurring template pattern),
* see explicit_stepper_base
*
* \tparam State The state type.
* \tparam Value The value type.
* \tparam Deriv The type representing the time derivative of the state.
* \tparam Time The time representing the independent variable - the time.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
*/
/**
* \fn euler::euler( const algebra_type &algebra )
* \brief Constructs the euler class. This constructor can be used as a default
* constructor of the algebra has a default constructor.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn euler::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
* \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
* The result is updated out of place, hence the input is in `in` and the output in `out`.
* Access to this step functionality is provided by explicit_stepper_base and
* `do_step_impl` should not be called directly.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn euler::calc_state( StateOut &x , time_type t , const StateIn1 &old_state , time_type t_old , const StateIn2 &current_state , time_type t_new ) const
* \brief This method is used for continuous output and it calculates the state `x` at a time `t` from the
* knowledge of two states `old_state` and `current_state` at time points `t_old` and `t_new`.
*/
/**
* \fn euler::adjust_size( const StateType &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_EULER_HPP_INCLUDED

252
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/explicit_error_generic_rk.hpp Executable file
View File

@@ -0,0 +1,252 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/explicit_error_generic_rk.hpp
[begin_description]
Implementation of the generic Runge Kutta error stepper. Base class for many RK error steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/base/explicit_error_stepper_base.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_call_algebra.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_operations.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
size_t StageCount,
size_t Order,
size_t StepperOrder ,
size_t ErrorOrder ,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class explicit_error_generic_rk
: public explicit_error_stepper_base<
explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
Value , Deriv , Time , Algebra , Operations , Resizer > ,
Order , StepperOrder , ErrorOrder , State , Value , Deriv , Time , Algebra ,
Operations , Resizer >
#else
class explicit_error_generic_rk : public explicit_error_stepper_base
#endif
{
public:
#ifndef DOXYGEN_SKIP
typedef explicit_error_stepper_base<
explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
Value , Deriv , Time , Algebra , Operations , Resizer > ,
Order , StepperOrder , ErrorOrder , State , Value , Deriv , Time , Algebra ,
Operations , Resizer > stepper_base_type;
#else
typedef explicit_stepper_base< ... > stepper_base_type;
#endif
typedef typename stepper_base_type::state_type state_type;
typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_base_type::value_type value_type;
typedef typename stepper_base_type::deriv_type deriv_type;
typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_base_type::time_type time_type;
typedef typename stepper_base_type::algebra_type algebra_type;
typedef typename stepper_base_type::operations_type operations_type;
typedef typename stepper_base_type::resizer_type resizer_type;
#ifndef DOXYGEN_SKIP
typedef explicit_error_generic_rk< StageCount , Order , StepperOrder , ErrorOrder , State ,
Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;
#endif
typedef detail::generic_rk_algorithm< StageCount , Value , Algebra , Operations > rk_algorithm_type;
typedef typename rk_algorithm_type::coef_a_type coef_a_type;
typedef typename rk_algorithm_type::coef_b_type coef_b_type;
typedef typename rk_algorithm_type::coef_c_type coef_c_type;
static const size_t stage_count = StageCount;
private:
public:
// we use an explicit_generic_rk to do the normal rk step
// and add a separate calculation of the error estimate afterwards
explicit_error_generic_rk( const coef_a_type &a ,
const coef_b_type &b ,
const coef_b_type &b2 ,
const coef_c_type &c ,
const algebra_type &algebra = algebra_type() )
: stepper_base_type( algebra ) , m_rk_algorithm( a , b , c ) , m_b2( b2 )
{ }
template< class System , class StateIn , class DerivIn , class StateOut , class Err >
void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
time_type t , StateOut &out , time_type dt , Err &xerr )
{
// normal step
do_step_impl( system , in , dxdt , t , out , dt );
// additionally, perform the error calculation
detail::template generic_rk_call_algebra< StageCount , algebra_type >()( stepper_base_type::m_algebra ,
xerr , dxdt , m_F , detail::generic_rk_scale_sum_err< StageCount , operations_type , value_type , time_type >( m_b2 , dt) );
}
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
time_type t , StateOut &out , time_type dt )
{
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
// actual calculation done in generic_rk.hpp
m_rk_algorithm.do_step( stepper_base_type::m_algebra , system , in , dxdt , t , out , dt , m_x_tmp.m_v , m_F );
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
stepper_base_type::adjust_size( x );
}
private:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
for( size_t i = 0 ; i < StageCount-1 ; ++i )
{
resized |= adjust_size_by_resizeability( m_F[i] , x , typename is_resizeable<deriv_type>::type() );
}
return resized;
}
rk_algorithm_type m_rk_algorithm;
coef_b_type m_b2;
resizer_type m_resizer;
wrapped_state_type m_x_tmp;
wrapped_deriv_type m_F[StageCount-1];
};
/********* DOXYGEN *********/
/**
* \class explicit_error_generic_rk
* \brief A generic implementation of explicit Runge-Kutta algorithms with error estimation. This class is as a
* base class for all explicit Runge-Kutta steppers with error estimation.
*
* This class implements the explicit Runge-Kutta algorithms with error estimation in a generic way.
* The Butcher tableau is passed to the stepper which constructs the stepper scheme with the help of a
* template-metaprogramming algorithm. ToDo : Add example!
*
* This class derives explicit_error_stepper_base which provides the stepper interface.
*
* \tparam StageCount The number of stages of the Runge-Kutta algorithm.
* \tparam Order The order of a stepper if the stepper is used without error estimation.
* \tparam StepperOrder The order of a step if the stepper is used with error estimation. Usually Order and StepperOrder have
* the same value.
* \tparam ErrorOrder The order of the error step if the stepper is used with error estimation.
* \tparam State The type representing the state of the ODE.
* \tparam Value The floating point type which is used in the computations.
* \tparam Time The type representing the independent variable - the time - of the ODE.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
*/
/**
* \fn explicit_error_generic_rk::explicit_error_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_b_type &b2 , const coef_c_type &c , const algebra_type &algebra )
* \brief Constructs the explicit_error_generik_rk class with the given parameters a, b, b2 and c. See examples section for details on the coefficients.
*
* \param a Triangular matrix of parameters b in the Butcher tableau.
* \param b Last row of the butcher tableau.
* \param b2 Parameters for lower-order evaluation to estimate the error.
* \param c Parameters to calculate the time points in the Butcher tableau.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn explicit_error_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
* \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
* The result is updated out-of-place, hence the input is in `in` and the output in `out`. Futhermore, an
* estimation of the error is stored in `xerr`. `do_step_impl` is used by explicit_error_stepper_base.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
* \param xerr The result of the error estimation is written in xerr.
*/
/**
* \fn explicit_error_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
* \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
* The result is updated out-of-place, hence the input is in `in` and the output in `out`.
* Access to this step functionality is provided by explicit_stepper_base and
* `do_step_impl` should not be called directly.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn explicit_error_generic_rk::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_ERROR_GENERIC_RK_HPP_INCLUDED

243
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/explicit_generic_rk.hpp Executable file
View File

@@ -0,0 +1,243 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/explicit_generic_rk.hpp
[begin_description]
Implementation of the generic Runge-Kutta steppers. This is the base class for many Runge-Kutta steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED
#include <boost/array.hpp>
#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/stepper/detail/generic_rk_algorithm.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
namespace boost {
namespace numeric {
namespace odeint {
//forward declarations
#ifndef DOXYGEN_SKIP
template<
size_t StageCount,
size_t Order,
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class explicit_generic_rk;
struct stage_vector;
template< class T , class Constant >
struct array_wrapper
{
typedef const typename boost::array< T , Constant::value > type;
};
template< class T , size_t i >
struct stage
{
T c;
boost::array< T , i > a;
};
template< class T , class Constant >
struct stage_wrapper
{
typedef stage< T , Constant::value > type;
};
#endif
template<
size_t StageCount,
size_t Order,
class State ,
class Value ,
class Deriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
#ifndef DOXYGEN_SKIP
class explicit_generic_rk : public explicit_stepper_base<
explicit_generic_rk< StageCount , Order , State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
Order , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class explicit_generic_rk : public explicit_stepper_base
#endif
{
public:
#ifndef DOXYGEN_SKIP
typedef explicit_stepper_base<
explicit_generic_rk< StageCount , Order , State , Value , Deriv ,Time , Algebra , Operations , Resizer > ,
Order , State , Value , Deriv , Time , Algebra ,
Operations , Resizer > stepper_base_type;
#else
typedef explicit_stepper_base< ... > stepper_base_type;
#endif
typedef typename stepper_base_type::state_type state_type;
typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_base_type::value_type value_type;
typedef typename stepper_base_type::deriv_type deriv_type;
typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_base_type::time_type time_type;
typedef typename stepper_base_type::algebra_type algebra_type;
typedef typename stepper_base_type::operations_type operations_type;
typedef typename stepper_base_type::resizer_type resizer_type;
#ifndef DOXYGEN_SKIP
typedef explicit_generic_rk< StageCount , Order , State , Value , Deriv ,Time , Algebra , Operations , Resizer > stepper_type;
#endif
typedef detail::generic_rk_algorithm< StageCount , Value , Algebra , Operations > rk_algorithm_type;
typedef typename rk_algorithm_type::coef_a_type coef_a_type;
typedef typename rk_algorithm_type::coef_b_type coef_b_type;
typedef typename rk_algorithm_type::coef_c_type coef_c_type;
#ifndef DOXYGEN_SKIP
static const size_t stage_count = StageCount;
#endif
public:
explicit_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c ,
const algebra_type &algebra = algebra_type() )
: stepper_base_type( algebra ) , m_rk_algorithm( a , b , c )
{ }
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt ,
time_type t , StateOut &out , time_type dt )
{
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
// actual calculation done in generic_rk.hpp
m_rk_algorithm.do_step( stepper_base_type::m_algebra , system , in , dxdt , t , out , dt , m_x_tmp.m_v , m_F );
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
stepper_base_type::adjust_size( x );
}
private:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
for( size_t i = 0 ; i < StageCount-1 ; ++i )
{
resized |= adjust_size_by_resizeability( m_F[i] , x , typename is_resizeable<deriv_type>::type() );
}
return resized;
}
rk_algorithm_type m_rk_algorithm;
resizer_type m_resizer;
wrapped_state_type m_x_tmp;
wrapped_deriv_type m_F[StageCount-1];
};
/*********** DOXYGEN *************/
/**
* \class explicit_generic_rk
* \brief A generic implementation of explicit Runge-Kutta algorithms. This class is as a base class
* for all explicit Runge-Kutta steppers.
*
* This class implements the explicit Runge-Kutta algorithms without error estimation in a generic way.
* The Butcher tableau is passed to the stepper which constructs the stepper scheme with the help of a
* template-metaprogramming algorithm. ToDo : Add example!
*
* This class derives explicit_stepper_base which provides the stepper interface.
*
* \tparam StageCount The number of stages of the Runge-Kutta algorithm.
* \tparam Order The order of the stepper.
* \tparam State The type representing the state of the ODE.
* \tparam Value The floating point type which is used in the computations.
* \tparam Time The type representing the independent variable - the time - of the ODE.
* \tparam Algebra The algebra type.
* \tparam Operations The operations type.
* \tparam Resizer The resizer policy type.
*/
/**
* \fn explicit_generic_rk::explicit_generic_rk( const coef_a_type &a , const coef_b_type &b , const coef_c_type &c , const algebra_type &algebra )
* \brief Constructs the explicit_generic_rk class. See examples section for details on the coefficients.
* \param a Triangular matrix of parameters b in the Butcher tableau.
* \param b Last row of the butcher tableau.
* \param c Parameters to calculate the time points in the Butcher tableau.
* \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
*/
/**
* \fn explicit_generic_rk::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
* \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
* The result is updated out of place, hence the input is in `in` and the output in `out`.
* Access to this step functionality is provided by explicit_stepper_base and
* `do_step_impl` should not be called directly.
*
* \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
* Simple System concept.
* \param in The state of the ODE which should be solved. in is not modified in this method
* \param dxdt The derivative of x at t.
* \param t The value of the time, at which the step should be performed.
* \param out The result of the step is written in out.
* \param dt The step size.
*/
/**
* \fn explicit_generic_rk::adjust_size( const StateIn &x )
* \brief Adjust the size of all temporaries in the stepper manually.
* \param x A state from which the size of the temporaries to be resized is deduced.
*/
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_EXPLICIT_GENERIC_RK_HPP_INCLUDED

36
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation.hpp Executable file
View File

@@ -0,0 +1,36 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation.hpp
[begin_description]
Forward header for the factory functions. Includes all files from the generation directory.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
#include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54_classic.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_runge_kutta_fehlberg78.hpp>
#include <boost/numeric/odeint/stepper/generation/generation_rosenbrock4.hpp>
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_HPP_INCLUDED

53
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_controlled_runge_kutta.hpp Executable file
View File

@@ -0,0 +1,53 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_controlled_runge_kutta.hpp
[begin_description]
Specialization of the controller factory for the controlled_runge_kutta class.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
namespace boost {
namespace numeric {
namespace odeint {
// controller factory for controlled_runge_kutta
template< class Stepper >
struct controller_factory< Stepper , controlled_runge_kutta< Stepper > >
{
typedef Stepper stepper_type;
typedef controlled_runge_kutta< stepper_type > controller_type;
typedef typename controller_type::error_checker_type error_checker_type;
typedef typename stepper_type::value_type value_type;
controller_type operator()( value_type abs_error , value_type rel_error , const stepper_type &stepper )
{
return controller_type( error_checker_type( abs_error , rel_error ) , stepper );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_CONTROLLED_RUNGE_KUTTA_HPP_INCLUDED

54
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_dense_output_runge_kutta.hpp Executable file
View File

@@ -0,0 +1,54 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_dense_output_runge_kutta.hpp
[begin_description]
Specialization of the controller factory for the dense_output_runge_kutta class.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
namespace boost {
namespace numeric {
namespace odeint {
// controller factory for controlled_runge_kutta
template< class Stepper >
struct dense_output_factory< Stepper , dense_output_runge_kutta< controlled_runge_kutta< Stepper > > >
{
typedef Stepper stepper_type;
typedef controlled_runge_kutta< stepper_type > controller_type;
typedef typename controller_type::error_checker_type error_checker_type;
typedef typename stepper_type::value_type value_type;
typedef dense_output_runge_kutta< controller_type > dense_output_type;
dense_output_type operator()( value_type abs_error , value_type rel_error , const stepper_type &stepper )
{
return dense_output_type( controller_type( error_checker_type( abs_error , rel_error ) , stepper ) );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_DENSE_OUTPUT_RUNGE_KUTTA_HPP_INCLUDED

72
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_rosenbrock4.hpp Executable file
View File

@@ -0,0 +1,72 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_rosenbrock4.hpp
[begin_description]
Enable the factory functions for the controller and the dense output of the Rosenbrock4 method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_ROSENBROCK4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_ROSENBROCK4_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class Value , class Coefficients , class Resize >
struct get_controller< rosenbrock4< Value , Coefficients , Resize > >
{
typedef rosenbrock4< Value , Coefficients , Resize > stepper_type;
typedef rosenbrock4_controller< stepper_type > type;
};
template< class Value , class Coefficients , class Resize >
struct get_dense_output< rosenbrock4< Value , Coefficients , Resize > >
{
typedef rosenbrock4< Value , Coefficients , Resize > stepper_type;
typedef rosenbrock4_controller< stepper_type > controller_type;
typedef rosenbrock4_dense_output< controller_type > type;
};
// controller factory for controlled_runge_kutta
template< class Stepper >
struct dense_output_factory< Stepper , rosenbrock4_dense_output< rosenbrock4_controller< Stepper > > >
{
typedef Stepper stepper_type;
typedef rosenbrock4_controller< stepper_type > controller_type;
typedef typename stepper_type::value_type value_type;
typedef rosenbrock4_dense_output< controller_type > dense_output_type;
dense_output_type operator()( value_type abs_error , value_type rel_error , const stepper_type &stepper )
{
return dense_output_type( controller_type( abs_error , rel_error , stepper ) );
}
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_ROSENBROCK4_HPP_INCLUDED

47
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54.hpp Executable file
View File

@@ -0,0 +1,47 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54.hpp
[begin_description]
Enable the factory functions for the controller and the dense output of the Runge-Kutta-Cash-Karp 54 method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
namespace boost {
namespace numeric {
namespace odeint {
// Specializations for runge_kutta_cash_karp54
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_controller< runge_kutta_cash_karp54< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
typedef runge_kutta_cash_karp54< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
typedef controlled_runge_kutta< stepper_type > type;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_HPP_INCLUDED

48
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54_classic.hpp Executable file
View File

@@ -0,0 +1,48 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_runge_kutta_cash_karp54_classic.hpp
[begin_description]
Enable the factory functions for the controller and the dense output of the
Runge-Kutta-Cash-Karp 54 method with the classical implementation.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
namespace boost {
namespace numeric {
namespace odeint {
// Specializations for runge_kutta_cash_karp54
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_controller< runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
typedef runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
typedef controlled_runge_kutta< stepper_type > type;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED

56
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_runge_kutta_dopri5.hpp Executable file
View File

@@ -0,0 +1,56 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_runge_kutta_dopri5.hpp
[begin_description]
Enable the factory functions for the controller and the dense output of the Runge-Kutta-Dormand-Prince5 method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/dense_output_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
#include <boost/numeric/odeint/stepper/generation/make_dense_output.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_controller< runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
typedef runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
typedef controlled_runge_kutta< stepper_type > type;
};
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_dense_output< runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
typedef runge_kutta_dopri5< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
typedef controlled_runge_kutta< stepper_type > controller_type;
typedef dense_output_runge_kutta< controller_type > type;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_DOPRI5_HPP_INCLUDED

46
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/generation_runge_kutta_fehlberg78.hpp Executable file
View File

@@ -0,0 +1,46 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/generation_runge_kutta_fehlberg78.hpp
[begin_description]
Enable the factory functions for the controller and the dense output of the Runge-Kutta-Fehlberg 78 method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_FEHLBERG78_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_FEHLBERG78_HPP_INCLUDED
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_fehlberg78.hpp>
#include <boost/numeric/odeint/stepper/generation/make_controlled.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class State , class Value , class Deriv , class Time , class Algebra , class Operations , class Resize >
struct get_controller< runge_kutta_fehlberg78< State , Value , Deriv , Time , Algebra , Operations , Resize > >
{
typedef runge_kutta_fehlberg78< State , Value , Deriv , Time , Algebra , Operations , Resize > stepper_type;
typedef controlled_runge_kutta< stepper_type > type;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_GENERATION_RUNGE_KUTTA_FEHLBERG78_HPP_INCLUDED

81
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/make_controlled.hpp Executable file
View File

@@ -0,0 +1,81 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/make_controlled.hpp
[begin_description]
Factory function to simplify the creation of controlled steppers from error steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_CONTROLLED_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_CONTROLLED_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
// default template for the controller
template< class Stepper > struct get_controller { };
// default controller factory
template< class Stepper , class Controller >
struct controller_factory
{
Controller operator()(
typename Stepper::value_type abs_error ,
typename Stepper::value_type rel_error ,
const Stepper &stepper )
{
return Controller( abs_error , rel_error , stepper );
}
};
namespace result_of
{
template< class Stepper >
struct make_controlled
{
typedef typename get_controller< Stepper >::type type;
};
}
template< class Stepper >
typename result_of::make_controlled< Stepper >::type make_controlled(
typename Stepper::value_type abs_error ,
typename Stepper::value_type rel_error ,
const Stepper & stepper = Stepper() )
{
typedef Stepper stepper_type;
typedef typename result_of::make_controlled< stepper_type >::type controller_type;
typedef controller_factory< stepper_type , controller_type > factory_type;
factory_type factory;
return factory( abs_error , rel_error , stepper );
}
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_CONTROLLED_HPP_INCLUDED

78
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/generation/make_dense_output.hpp Executable file
View File

@@ -0,0 +1,78 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/generation/make_dense_output.hpp
[begin_description]
Factory function to simplify the creation of dense output steppers from error steppers.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_DENSE_OUTPUT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_DENSE_OUTPUT_HPP_INCLUDED
namespace boost {
namespace numeric {
namespace odeint {
// default template for the dense output
template< class Stepper > struct get_dense_output { };
// default dense output factory
template< class Stepper , class DenseOutput >
struct dense_output_factory
{
DenseOutput operator()(
typename Stepper::value_type abs_error ,
typename Stepper::value_type rel_error ,
const Stepper &stepper )
{
return DenseOutput( abs_error , rel_error , stepper );
}
};
namespace result_of
{
template< class Stepper >
struct make_dense_output
{
typedef typename get_dense_output< Stepper >::type type;
};
}
template< class Stepper >
typename result_of::make_dense_output< Stepper >::type make_dense_output(
typename Stepper::value_type abs_error ,
typename Stepper::value_type rel_error ,
const Stepper &stepper = Stepper() )
{
typedef Stepper stepper_type;
typedef typename result_of::make_dense_output< stepper_type >::type dense_output_type;
typedef dense_output_factory< stepper_type , dense_output_type > factory_type;
factory_type factory;
return factory( abs_error , rel_error , stepper );
}
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_GENERATION_MAKE_DENSE_OUTPUT_HPP_INCLUDED

169
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/implicit_euler.hpp Executable file
View File

@@ -0,0 +1,169 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/implicit_euler.hpp
[begin_description]
Impementation of the implicit Euler method. Works with ublas::vector as state type.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED
#include <utility>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class ValueType , class Resizer = initially_resizer >
class implicit_euler
{
public:
typedef ValueType value_type;
typedef value_type time_type;
typedef boost::numeric::ublas::vector< value_type > state_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_type deriv_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;
typedef state_wrapper< matrix_type > wrapped_matrix_type;
typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;
typedef Resizer resizer_type;
typedef stepper_tag stepper_category;
typedef implicit_euler< ValueType , Resizer > stepper_type;
implicit_euler( value_type epsilon = 1E-6 )
: m_epsilon( epsilon )
{ }
template< class System >
void do_step( System system , state_type &x , time_type t , time_type dt )
{
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );
for( size_t i=0 ; i<x.size() ; ++i )
m_pm.m_v[i] = i;
t += dt;
// apply first Newton step
deriv_func( x , m_dxdt.m_v , t );
m_b.m_v = dt * m_dxdt.m_v;
jacobi_func( x , m_jacobi.m_v , t );
m_jacobi.m_v *= dt;
m_jacobi.m_v -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v = x - m_b.m_v;
// iterate Newton until some precision is reached
// ToDo: maybe we should apply only one Newton step -> linear implicit one-step scheme
while( boost::numeric::ublas::norm_2( m_b.m_v ) > m_epsilon )
{
deriv_func( m_x.m_v , m_dxdt.m_v , t );
m_b.m_v = x - m_x.m_v + dt*m_dxdt.m_v;
// simplified version, only the first Jacobian is used
// jacobi( m_x , m_jacobi , t );
// m_jacobi *= dt;
// m_jacobi -= boost::numeric::ublas::identity_matrix< value_type >( x.size() );
solve( m_b.m_v , m_jacobi.m_v );
m_x.m_v -= m_b.m_v;
}
x = m_x.m_v;
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
}
private:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_x , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_b , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_jacobi , x , typename is_resizeable<matrix_type>::type() );
resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
return resized;
}
void solve( state_type &x , matrix_type &m )
{
int res = boost::numeric::ublas::lu_factorize( m , m_pm.m_v );
if( res != 0 ) exit(0);
boost::numeric::ublas::lu_substitute( m , m_pm.m_v , x );
}
private:
value_type m_epsilon;
resizer_type m_resizer;
wrapped_deriv_type m_dxdt;
wrapped_state_type m_x;
wrapped_deriv_type m_b;
wrapped_matrix_type m_jacobi;
wrapped_pmatrix_type m_pm;
};
} // odeint
} // numeric
} // boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_IMPLICIT_EULER_HPP_INCLUDED

312
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/modified_midpoint.hpp Executable file
View File

@@ -0,0 +1,312 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/modified_midpoint.hpp
[begin_description]
Modified midpoint method for the use in Burlish-Stoer stepper.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED
#include <vector>
#include <boost/numeric/odeint/stepper/base/explicit_stepper_base.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class modified_midpoint
: public explicit_stepper_base<
modified_midpoint< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
2 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class modified_midpoint : public explicit_stepper_base
#endif
{
public :
typedef explicit_stepper_base<
modified_midpoint< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
2 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
typedef typename stepper_base_type::state_type state_type;
typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_base_type::value_type value_type;
typedef typename stepper_base_type::deriv_type deriv_type;
typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_base_type::time_type time_type;
typedef typename stepper_base_type::algebra_type algebra_type;
typedef typename stepper_base_type::operations_type operations_type;
typedef typename stepper_base_type::resizer_type resizer_type;
typedef typename stepper_base_type::stepper_type stepper_type;
modified_midpoint( unsigned short steps = 2 , const algebra_type &algebra = algebra_type() )
: stepper_base_type( algebra ) , m_steps( steps )
{ }
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
{
static const value_type val1 = static_cast< value_type >( 1 );
static const value_type val05 = static_cast< value_type >( 1 ) / static_cast< value_type >( 2 );
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl< StateIn > , detail::ref( *this ) , detail::_1 ) );
const time_type h = dt / static_cast<value_type>( m_steps );
const time_type h2 = static_cast<value_type>(2) * h;
typename odeint::unwrap_reference< System >::type &sys = system;
time_type th = t + h;
// m_x1 = x + h*dxdt
stepper_base_type::m_algebra.for_each3( m_x1.m_v , in , dxdt ,
typename operations_type::template scale_sum2< value_type , time_type >( val1 , h ) );
sys( m_x1.m_v , m_dxdt.m_v , th );
boost::numeric::odeint::copy( in , m_x0.m_v );
unsigned short i = 1;
while( i != m_steps )
{
// general step
//tmp = m_x1; m_x1 = m_x0 + h2*m_dxdt; m_x0 = tmp
stepper_base_type::m_algebra.for_each3( m_x1.m_v , m_x0.m_v , m_dxdt.m_v ,
typename operations_type::template scale_sum_swap2< value_type , time_type >( val1 , h2 ) );
th += h;
sys( m_x1.m_v , m_dxdt.m_v , th);
i++;
}
// last step
// x = 0.5*( m_x0 + m_x1 + h*m_dxdt )
stepper_base_type::m_algebra.for_each4( out , m_x0.m_v , m_x1.m_v , m_dxdt.m_v ,
typename operations_type::template scale_sum3< value_type , value_type , time_type >( val05 , val05 , val05*h ) );
}
void set_steps( unsigned short steps )
{ m_steps = steps; }
unsigned short steps( void ) const
{ return m_steps; }
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize_impl( x );
stepper_base_type::adjust_size( x );
}
private:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized( false );
resized |= adjust_size_by_resizeability( m_x0 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
return resized;
}
unsigned short m_steps;
resizer_type m_resizer;
wrapped_state_type m_x0;
wrapped_state_type m_x1;
wrapped_deriv_type m_dxdt;
};
/* Modified midpoint which stores derivatives and state at dt/2 in some external storage for later usage in dense output calculation
* This Stepper is for use in Bulirsch Stoer only. It DOES NOT meet any stepper concept.
*/
template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = range_algebra ,
class Operations = default_operations ,
class Resizer = initially_resizer
>
class modified_midpoint_dense_out
{
public :
typedef State state_type;
typedef Value value_type;
typedef Deriv deriv_type;
typedef Time time_type;
typedef Algebra algebra_type;
typedef Operations operations_type;
typedef Resizer resizer_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef modified_midpoint_dense_out< State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_type;
typedef std::vector< wrapped_deriv_type > deriv_table_type;
modified_midpoint_dense_out( unsigned short steps = 2 , const algebra_type &algebra = algebra_type() )
: m_algebra( algebra ) , m_steps( steps )
{ }
/*
* performs a modified midpoint step with m_steps intermediate points
* stores approximation for x(t+dt/2) in x_mp and all evaluated function results in derivs
*
*/
template< class System , class StateIn , class DerivIn , class StateOut >
void do_step( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt ,
state_type &x_mp , deriv_table_type &derivs )
{
static const value_type val1 = static_cast< value_type >( 1 );
static const value_type val05 = static_cast< value_type >( 1 ) / static_cast< value_type >( 2 );
m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize< StateIn > , detail::ref( *this ) , detail::_1 ) );
const time_type h = dt / static_cast<value_type>( m_steps );
const time_type h2 = static_cast<value_type>( 2 ) * h;
typename odeint::unwrap_reference< System >::type &sys = system;
time_type th = t + h;
// m_x1 = x + h*dxdt
m_algebra.for_each3( m_x1.m_v , in , dxdt ,
typename operations_type::template scale_sum2< value_type , time_type >( val1 , h ) );
if( m_steps == 2 )
// result of first step already gives approximation at the center of the interval
boost::numeric::odeint::copy( m_x1.m_v , x_mp );
sys( m_x1.m_v , derivs[0].m_v , th );
boost::numeric::odeint::copy( in , m_x0.m_v );
unsigned short i = 1;
while( i != m_steps )
{
// general step
//tmp = m_x1; m_x1 = m_x0 + h2*m_dxdt; m_x0 = tmp
m_algebra.for_each3( m_x1.m_v , m_x0.m_v , derivs[i-1].m_v ,
typename operations_type::template scale_sum_swap2< value_type , time_type >( val1 , h2 ) );
if( i == m_steps/2-1 )
// save approximation at the center of the interval
boost::numeric::odeint::copy( m_x1.m_v , x_mp );
th += h;
sys( m_x1.m_v , derivs[i].m_v , th);
i++;
}
// last step
// x = 0.5*( m_x0 + m_x1 + h*m_dxdt )
m_algebra.for_each4( out , m_x0.m_v , m_x1.m_v , derivs[m_steps-1].m_v ,
typename operations_type::template scale_sum3< value_type , value_type , time_type >( val05 , val05 , val05*h ) );
}
void set_steps( unsigned short steps )
{ m_steps = steps; }
unsigned short steps( void ) const
{ return m_steps; }
template< class StateIn >
bool resize( const StateIn &x )
{
bool resized( false );
resized |= adjust_size_by_resizeability( m_x0 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
return resized;
}
template< class StateIn >
void adjust_size( const StateIn &x )
{
resize( x );
}
private:
algebra_type m_algebra;
unsigned short m_steps;
resizer_type m_resizer;
wrapped_state_type m_x0;
wrapped_state_type m_x1;
};
/********** DOXYGEN ***********/
/**
* \class modified_midpoint
*
* Implementation of the modified midpoint method with a configurable
* number of intermediate steps. This class is used by the Bulirsch-Stoer
* algorithm and is not meant for direct usage.
*/
/**
* \class modified_midpoint_dense_out
*
* Implementation of the modified midpoint method with a configurable
* number of intermediate steps. This class is used by the dense output
* Bulirsch-Stoer algorithm and is not meant for direct usage.
* \note This stepper is for internal use only and does not meet
* any stepper concept.
*/
}
}
}
#endif // BOOST_NUMERIC_ODEINT_STEPPER_MODIFIED_MIDPOINT_HPP_INCLUDED

345
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/rosenbrock4.hpp Executable file
View File

@@ -0,0 +1,345 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/rosenbrock4.hpp
[begin_description]
Implementation of the Rosenbrock 4 method for solving stiff ODEs. Note, that a
controller and a dense-output stepper exist for this method,
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>
namespace boost {
namespace numeric {
namespace odeint {
/*
* ToDo:
*
* 2. Interfacing for odeint, check if controlled_error_stepper can be used
* 3. dense output
*/
template< class Value >
struct default_rosenbrock_coefficients
{
typedef Value value_type;
typedef unsigned short order_type;
default_rosenbrock_coefficients( void )
: gamma ( static_cast< value_type >( 0.25 ) ) ,
d1 ( static_cast< value_type >( 0.25 ) ) ,
d2 ( static_cast< value_type >( -0.1043 ) ) ,
d3 ( static_cast< value_type >( 0.1035 ) ) ,
d4 ( static_cast< value_type >( 0.3620000000000023e-01 ) ) ,
c2 ( static_cast< value_type >( 0.386 ) ) ,
c3 ( static_cast< value_type >( 0.21 ) ) ,
c4 ( static_cast< value_type >( 0.63 ) ) ,
c21 ( static_cast< value_type >( -0.5668800000000000e+01 ) ) ,
a21 ( static_cast< value_type >( 0.1544000000000000e+01 ) ) ,
c31 ( static_cast< value_type >( -0.2430093356833875e+01 ) ) ,
c32 ( static_cast< value_type >( -0.2063599157091915e+00 ) ) ,
a31 ( static_cast< value_type >( 0.9466785280815826e+00 ) ) ,
a32 ( static_cast< value_type >( 0.2557011698983284e+00 ) ) ,
c41 ( static_cast< value_type >( -0.1073529058151375e+00 ) ) ,
c42 ( static_cast< value_type >( -0.9594562251023355e+01 ) ) ,
c43 ( static_cast< value_type >( -0.2047028614809616e+02 ) ) ,
a41 ( static_cast< value_type >( 0.3314825187068521e+01 ) ) ,
a42 ( static_cast< value_type >( 0.2896124015972201e+01 ) ) ,
a43 ( static_cast< value_type >( 0.9986419139977817e+00 ) ) ,
c51 ( static_cast< value_type >( 0.7496443313967647e+01 ) ) ,
c52 ( static_cast< value_type >( -0.1024680431464352e+02 ) ) ,
c53 ( static_cast< value_type >( -0.3399990352819905e+02 ) ) ,
c54 ( static_cast< value_type >( 0.1170890893206160e+02 ) ) ,
a51 ( static_cast< value_type >( 0.1221224509226641e+01 ) ) ,
a52 ( static_cast< value_type >( 0.6019134481288629e+01 ) ) ,
a53 ( static_cast< value_type >( 0.1253708332932087e+02 ) ) ,
a54 ( static_cast< value_type >( -0.6878860361058950e+00 ) ) ,
c61 ( static_cast< value_type >( 0.8083246795921522e+01 ) ) ,
c62 ( static_cast< value_type >( -0.7981132988064893e+01 ) ) ,
c63 ( static_cast< value_type >( -0.3152159432874371e+02 ) ) ,
c64 ( static_cast< value_type >( 0.1631930543123136e+02 ) ) ,
c65 ( static_cast< value_type >( -0.6058818238834054e+01 ) ) ,
d21 ( static_cast< value_type >( 0.1012623508344586e+02 ) ) ,
d22 ( static_cast< value_type >( -0.7487995877610167e+01 ) ) ,
d23 ( static_cast< value_type >( -0.3480091861555747e+02 ) ) ,
d24 ( static_cast< value_type >( -0.7992771707568823e+01 ) ) ,
d25 ( static_cast< value_type >( 0.1025137723295662e+01 ) ) ,
d31 ( static_cast< value_type >( -0.6762803392801253e+00 ) ) ,
d32 ( static_cast< value_type >( 0.6087714651680015e+01 ) ) ,
d33 ( static_cast< value_type >( 0.1643084320892478e+02 ) ) ,
d34 ( static_cast< value_type >( 0.2476722511418386e+02 ) ) ,
d35 ( static_cast< value_type >( -0.6594389125716872e+01 ) )
{}
const value_type gamma;
const value_type d1 , d2 , d3 , d4;
const value_type c2 , c3 , c4;
const value_type c21 ;
const value_type a21;
const value_type c31 , c32;
const value_type a31 , a32;
const value_type c41 , c42 , c43;
const value_type a41 , a42 , a43;
const value_type c51 , c52 , c53 , c54;
const value_type a51 , a52 , a53 , a54;
const value_type c61 , c62 , c63 , c64 , c65;
const value_type d21 , d22 , d23 , d24 , d25;
const value_type d31 , d32 , d33 , d34 , d35;
static const order_type stepper_order = 4;
static const order_type error_order = 3;
};
template< class Value , class Coefficients = default_rosenbrock_coefficients< Value > , class Resizer = initially_resizer >
class rosenbrock4
{
private:
public:
typedef Value value_type;
typedef boost::numeric::ublas::vector< value_type > state_type;
typedef state_type deriv_type;
typedef value_type time_type;
typedef boost::numeric::ublas::matrix< value_type > matrix_type;
typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
typedef Resizer resizer_type;
typedef Coefficients rosenbrock_coefficients;
typedef stepper_tag stepper_category;
typedef unsigned short order_type;
typedef state_wrapper< state_type > wrapped_state_type;
typedef state_wrapper< deriv_type > wrapped_deriv_type;
typedef state_wrapper< matrix_type > wrapped_matrix_type;
typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;
typedef rosenbrock4< Value , Coefficients , Resizer > stepper_type;
const static order_type stepper_order = rosenbrock_coefficients::stepper_order;
const static order_type error_order = rosenbrock_coefficients::error_order;
rosenbrock4( void )
: m_resizer() , m_x_err_resizer() ,
m_jac() , m_pm() ,
m_dfdt() , m_dxdt() , m_dxdtnew() ,
m_g1() , m_g2() , m_g3() , m_g4() , m_g5() ,
m_cont3() , m_cont4() , m_xtmp() , m_x_err() ,
m_coef()
{ }
order_type order() const { return stepper_order; }
template< class System >
void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt , state_type &xerr )
{
// get the system and jacobi function
typedef typename odeint::unwrap_reference< System >::type system_type;
typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
system_type &sys = system;
deriv_func_type &deriv_func = sys.first;
jacobi_func_type &jacobi_func = sys.second;
const size_t n = x.size();
m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );
for( size_t i=0 ; i<n ; ++i )
m_pm.m_v( i ) = i;
deriv_func( x , m_dxdt.m_v , t );
jacobi_func( x , m_jac.m_v , t , m_dfdt.m_v );
m_jac.m_v *= -1.0;
m_jac.m_v += 1.0 / m_coef.gamma / dt * boost::numeric::ublas::identity_matrix< value_type >( n );
boost::numeric::ublas::lu_factorize( m_jac.m_v , m_pm.m_v );
for( size_t i=0 ; i<n ; ++i )
m_g1.m_v[i] = m_dxdt.m_v[i] + dt * m_coef.d1 * m_dfdt.m_v[i];
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g1.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a21 * m_g1.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c2 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g2.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d2 * m_dfdt.m_v[i] + m_coef.c21 * m_g1.m_v[i] / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g2.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a31 * m_g1.m_v[i] + m_coef.a32 * m_g2.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c3 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g3.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d3 * m_dfdt.m_v[i] + ( m_coef.c31 * m_g1.m_v[i] + m_coef.c32 * m_g2.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g3.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a41 * m_g1.m_v[i] + m_coef.a42 * m_g2.m_v[i] + m_coef.a43 * m_g3.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c4 * dt );
for( size_t i=0 ; i<n ; ++i )
m_g4.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d4 * m_dfdt.m_v[i] + ( m_coef.c41 * m_g1.m_v[i] + m_coef.c42 * m_g2.m_v[i] + m_coef.c43 * m_g3.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g4.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] = x[i] + m_coef.a51 * m_g1.m_v[i] + m_coef.a52 * m_g2.m_v[i] + m_coef.a53 * m_g3.m_v[i] + m_coef.a54 * m_g4.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
for( size_t i=0 ; i<n ; ++i )
m_g5.m_v[i] = m_dxdtnew.m_v[i] + ( m_coef.c51 * m_g1.m_v[i] + m_coef.c52 * m_g2.m_v[i] + m_coef.c53 * m_g3.m_v[i] + m_coef.c54 * m_g4.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g5.m_v );
for( size_t i=0 ; i<n ; ++i )
m_xtmp.m_v[i] += m_g5.m_v[i];
deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
for( size_t i=0 ; i<n ; ++i )
xerr[i] = m_dxdtnew.m_v[i] + ( m_coef.c61 * m_g1.m_v[i] + m_coef.c62 * m_g2.m_v[i] + m_coef.c63 * m_g3.m_v[i] + m_coef.c64 * m_g4.m_v[i] + m_coef.c65 * m_g5.m_v[i] ) / dt;
boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , xerr );
for( size_t i=0 ; i<n ; ++i )
xout[i] = m_xtmp.m_v[i] + xerr[i];
}
template< class System >
void do_step( System system , state_type &x , time_type t , time_type dt , state_type &xerr )
{
do_step( system , x , t , x , dt , xerr );
}
/*
* do_step without error output - just calls above functions with and neglects the error estimate
*/
template< class System >
void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt )
{
m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
do_step( system , x , t , xout , dt , m_x_err.m_v );
}
template< class System >
void do_step( System system , state_type &x , time_type t , time_type dt )
{
m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
do_step( system , x , t , dt , m_x_err.m_v );
}
void prepare_dense_output()
{
const size_t n = m_g1.m_v.size();
for( size_t i=0 ; i<n ; ++i )
{
m_cont3.m_v[i] = m_coef.d21 * m_g1.m_v[i] + m_coef.d22 * m_g2.m_v[i] + m_coef.d23 * m_g3.m_v[i] + m_coef.d24 * m_g4.m_v[i] + m_coef.d25 * m_g5.m_v[i];
m_cont4.m_v[i] = m_coef.d31 * m_g1.m_v[i] + m_coef.d32 * m_g2.m_v[i] + m_coef.d33 * m_g3.m_v[i] + m_coef.d34 * m_g4.m_v[i] + m_coef.d35 * m_g5.m_v[i];
}
}
void calc_state( time_type t , state_type &x ,
const state_type &x_old , time_type t_old ,
const state_type &x_new , time_type t_new )
{
const size_t n = m_g1.m_v.size();
time_type dt = t_new - t_old;
time_type s = ( t - t_old ) / dt;
time_type s1 = 1.0 - s;
for( size_t i=0 ; i<n ; ++i )
x[i] = x_old[i] * s1 + s * ( x_new[i] + s1 * ( m_cont3.m_v[i] + s * m_cont4.m_v[i] ) );
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_impl( x );
resize_x_err( x );
}
protected:
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_dfdt , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
resized |= adjust_size_by_resizeability( m_xtmp , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_g1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_g2 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_g3 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_g4 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_g5 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_cont3 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_cont4 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_jac , x , typename is_resizeable<matrix_type>::type() );
resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
return resized;
}
template< class StateIn >
bool resize_x_err( const StateIn &x )
{
return adjust_size_by_resizeability( m_x_err , x , typename is_resizeable<state_type>::type() );
}
private:
resizer_type m_resizer;
resizer_type m_x_err_resizer;
wrapped_matrix_type m_jac;
wrapped_pmatrix_type m_pm;
wrapped_deriv_type m_dfdt , m_dxdt , m_dxdtnew;
wrapped_state_type m_g1 , m_g2 , m_g3 , m_g4 , m_g5;
wrapped_state_type m_cont3 , m_cont4;
wrapped_state_type m_xtmp;
wrapped_state_type m_x_err;
const rosenbrock_coefficients m_coef;
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED

208
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/rosenbrock4_controller.hpp Executable file
View File

@@ -0,0 +1,208 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/rosenbrock4_controller.hpp
[begin_description]
Controller for the Rosenbrock4 method.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_CONTROLLER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_CONTROLLER_HPP_INCLUDED
#include <boost/config.hpp>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/stepper/controlled_step_result.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class Stepper >
class rosenbrock4_controller
{
private:
public:
typedef Stepper stepper_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_type::resizer_type resizer_type;
typedef controlled_stepper_tag stepper_category;
typedef rosenbrock4_controller< Stepper > controller_type;
rosenbrock4_controller( value_type atol = 1.0e-6 , value_type rtol = 1.0e-6 , const stepper_type &stepper = stepper_type() )
: m_stepper( stepper ) , m_atol( atol ) , m_rtol( rtol ) ,
m_first_step( true ) , m_err_old( 0.0 ) , m_dt_old( 0.0 ) ,
m_last_rejected( false )
{ }
value_type error( const state_type &x , const state_type &xold , const state_type &xerr )
{
BOOST_USING_STD_MAX();
using std::abs;
const size_t n = x.size();
value_type err = 0.0 , sk = 0.0;
for( size_t i=0 ; i<n ; ++i )
{
sk = m_atol + m_rtol * max BOOST_PREVENT_MACRO_SUBSTITUTION ( abs( xold[i] ) , abs( x[i] ) );
err += xerr[i] * xerr[i] / sk / sk;
}
return sqrt( err / value_type( n ) );
}
value_type last_error( void ) const
{
return m_err_old;
}
template< class System >
boost::numeric::odeint::controlled_step_result
try_step( System sys , state_type &x , time_type &t , time_type &dt )
{
m_xnew_resizer.adjust_size( x , detail::bind( &controller_type::template resize_m_xnew< state_type > , detail::ref( *this ) , detail::_1 ) );
boost::numeric::odeint::controlled_step_result res = try_step( sys , x , t , m_xnew.m_v , dt );
if( res == success )
{
boost::numeric::odeint::copy( m_xnew.m_v , x );
}
return res;
}
template< class System >
boost::numeric::odeint::controlled_step_result
try_step( System sys , const state_type &x , time_type &t , state_type &xout , time_type &dt )
{
BOOST_USING_STD_MIN();
BOOST_USING_STD_MAX();
using std::pow;
static const value_type safe = 0.9 , fac1 = 5.0 , fac2 = 1.0 / 6.0;
m_xerr_resizer.adjust_size( x , detail::bind( &controller_type::template resize_m_xerr< state_type > , detail::ref( *this ) , detail::_1 ) );
m_stepper.do_step( sys , x , t , xout , dt , m_xerr.m_v );
value_type err = error( xout , x , m_xerr.m_v );
value_type fac = max BOOST_PREVENT_MACRO_SUBSTITUTION ( fac2 , min BOOST_PREVENT_MACRO_SUBSTITUTION ( fac1 , pow( err , 0.25 ) / safe ) );
value_type dt_new = dt / fac;
if ( err <= 1.0 )
{
if( m_first_step )
{
m_first_step = false;
}
else
{
value_type fac_pred = ( m_dt_old / dt ) * pow( err * err / m_err_old , 0.25 ) / safe;
fac_pred = max BOOST_PREVENT_MACRO_SUBSTITUTION ( fac2 , min BOOST_PREVENT_MACRO_SUBSTITUTION ( fac1 , fac_pred ) );
fac = max BOOST_PREVENT_MACRO_SUBSTITUTION ( fac , fac_pred );
dt_new = dt / fac;
}
m_dt_old = dt;
m_err_old = max BOOST_PREVENT_MACRO_SUBSTITUTION ( 0.01 , err );
if( m_last_rejected )
dt_new = ( dt >= 0.0 ? min BOOST_PREVENT_MACRO_SUBSTITUTION ( dt_new , dt ) : max BOOST_PREVENT_MACRO_SUBSTITUTION ( dt_new , dt ) );
t += dt;
dt = dt_new;
m_last_rejected = false;
return success;
}
else
{
dt = dt_new;
m_last_rejected = true;
return fail;
}
}
template< class StateType >
void adjust_size( const StateType &x )
{
resize_m_xerr( x );
resize_m_xnew( x );
}
stepper_type& stepper( void )
{
return m_stepper;
}
const stepper_type& stepper( void ) const
{
return m_stepper;
}
private:
template< class StateIn >
bool resize_m_xerr( const StateIn &x )
{
return adjust_size_by_resizeability( m_xerr , x , typename is_resizeable<state_type>::type() );
}
template< class StateIn >
bool resize_m_xnew( const StateIn &x )
{
return adjust_size_by_resizeability( m_xnew , x , typename is_resizeable<state_type>::type() );
}
stepper_type m_stepper;
resizer_type m_xerr_resizer;
resizer_type m_xnew_resizer;
wrapped_state_type m_xerr;
wrapped_state_type m_xnew;
value_type m_atol , m_rtol;
bool m_first_step;
value_type m_err_old , m_dt_old;
bool m_last_rejected;
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_CONTROLLER_HPP_INCLUDED

199
Controller/src/so3_quadrotor_simulator/include/ode/boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp Executable file
View File

@@ -0,0 +1,199 @@
/*
[auto_generated]
boost/numeric/odeint/stepper/rosenbrock4_dense_output.hpp
[begin_description]
Dense output for Rosenbrock 4.
[end_description]
Copyright 2009-2011 Karsten Ahnert
Copyright 2009-2011 Mario Mulansky
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE_1_0.txt or
copy at http://www.boost.org/LICENSE_1_0.txt)
*/
#ifndef BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_DENSE_OUTPUT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_DENSE_OUTPUT_HPP_INCLUDED
#include <utility>
#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/stepper/rosenbrock4_controller.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
namespace boost {
namespace numeric {
namespace odeint {
template< class ControlledStepper >
class rosenbrock4_dense_output
{
public:
typedef ControlledStepper controlled_stepper_type;
typedef typename controlled_stepper_type::stepper_type stepper_type;
typedef typename stepper_type::value_type value_type;
typedef typename stepper_type::state_type state_type;
typedef typename stepper_type::wrapped_state_type wrapped_state_type;
typedef typename stepper_type::time_type time_type;
typedef typename stepper_type::deriv_type deriv_type;
typedef typename stepper_type::wrapped_deriv_type wrapped_deriv_type;
typedef typename stepper_type::resizer_type resizer_type;
typedef dense_output_stepper_tag stepper_category;
typedef rosenbrock4_dense_output< ControlledStepper > dense_output_stepper_type;
rosenbrock4_dense_output( const controlled_stepper_type &stepper = controlled_stepper_type() )
: m_stepper( stepper ) ,
m_x1() , m_x2() ,
m_current_state_x1( true ) ,
m_t() , m_t_old() , m_dt()
{
}
template< class StateType >
void initialize( const StateType &x0 , time_type t0 , time_type dt0 )
{
m_resizer.adjust_size( x0 , detail::bind( &dense_output_stepper_type::template resize_impl< StateType > , detail::ref( *this ) , detail::_1 ) );
get_current_state() = x0;
m_t = t0;
m_dt = dt0;
}
template< class System >
std::pair< time_type , time_type > do_step( System system )
{
const size_t max_count = 1000;
controlled_step_result res = fail;
m_t_old = m_t;
size_t count = 0;
do
{
res = m_stepper.try_step( system , get_current_state() , m_t , get_old_state() , m_dt );
if( count++ == max_count )
throw std::overflow_error( "rosenbrock4 : too much iterations!");
}
while( res == fail );
m_stepper.stepper().prepare_dense_output();
this->toggle_current_state();
return std::make_pair( m_t_old , m_t );
}
/*
* The two overloads are needed in order to solve the forwarding problem.
*/
template< class StateOut >
void calc_state( time_type t , StateOut &x )
{
m_stepper.stepper().calc_state( t , x , get_old_state() , m_t_old , get_current_state() , m_t );
}
template< class StateOut >
void calc_state( time_type t , const StateOut &x )
{
m_stepper.stepper().calc_state( t , x , get_old_state() , m_t_old , get_current_state() , m_t );
}
template< class StateType >
void adjust_size( const StateType &x )
{
m_stepper.adjust_size( x );
resize_impl( x );
}
const state_type& current_state( void ) const
{
return get_current_state();
}
time_type current_time( void ) const
{
return m_t;
}
const state_type& previous_state( void ) const
{
return get_old_state();
}
time_type previous_time( void ) const
{
return m_t_old;
}
time_type current_time_step( void ) const
{
return m_dt;
}
private:
state_type& get_current_state( void )
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
const state_type& get_current_state( void ) const
{
return m_current_state_x1 ? m_x1.m_v : m_x2.m_v ;
}
state_type& get_old_state( void )
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
const state_type& get_old_state( void ) const
{
return m_current_state_x1 ? m_x2.m_v : m_x1.m_v ;
}
void toggle_current_state( void )
{
m_current_state_x1 = ! m_current_state_x1;
}
template< class StateIn >
bool resize_impl( const StateIn &x )
{
bool resized = false;
resized |= adjust_size_by_resizeability( m_x1 , x , typename is_resizeable<state_type>::type() );
resized |= adjust_size_by_resizeability( m_x2 , x , typename is_resizeable<state_type>::type() );
return resized;
}
controlled_stepper_type m_stepper;
resizer_type m_resizer;
wrapped_state_type m_x1 , m_x2;
bool m_current_state_x1;
time_type m_t , m_t_old , m_dt;
};
} // namespace odeint
} // namespace numeric
} // namespace boost
#endif // BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_DENSE_OUTPUT_HPP_INCLUDED

Some files were not shown because too many files have changed in this diff Show More