void midpoint_integrate_impl(
const StateSpace& space,
const StateSpaceSystem& sys,
const typename pp::topology_traits<StateSpace>::point_type& start_point,
typename pp::topology_traits<StateSpace>::point_type& end_point,
const InputTrajectory& u_traj,
double start_time,
double end_time,
double time_step) {
typedef typename pp::topology_traits<StateSpace>::point_type PointType;
typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
double t = start_time;
end_point = start_point;
while(((time_step > 0.0) && (t < end_time)) ||
((time_step < 0.0) && (t > end_time))) {
PointType mid_p = space.adjust(end_point, (0.5 * time_step) * dp);
t += 0.5 * time_step;
u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
dp = sys.get_state_derivative(space, mid_p, u_wp.second.pt, t);
end_point = space.adjust(end_point, time_step * dp);
t += 0.5 * time_step;
u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
};
};
typename boost::enable_if<
is_readable_matrix<Matrix>,
typename pp::topology_traits<StateSpace>::point_type >::type
sample_gaussian_point(const StateSpace& space,
const typename pp::topology_traits<StateSpace>::point_type& mean,
const Matrix& cov) {
typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
typedef typename mat_traits<Matrix>::value_type ValueType;
typedef typename mat_traits<Matrix>::size_type SizeType;
using std::sqrt;
using ReaK::to_vect;
using ReaK::from_vect;
mat< ValueType, mat_structure::square> L;
try {
decompose_Cholesky(cov,L);
} catch(singularity_error&) {
mat< ValueType, mat_structure::diagonal> E(cov.get_row_count());
mat< ValueType, mat_structure::square> U(cov.get_row_count()), V(cov.get_row_count());
decompose_SVD(cov,U,E,V);
for(SizeType i = 0; i < cov.get_row_count(); ++i)
E(i,i) = sqrt(E(i,i));
L = U * E;
};
boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
for(SizeType i = 0; i < z.size(); ++i)
z[i] = var_rnd();
return space.adjust(mean, from_vect<DiffType>(L * z));
};
void runge_kutta4_integrate_impl(
const StateSpace& space,
const StateSpaceSystem& sys,
const typename pp::topology_traits<StateSpace>::point_type& start_point,
typename pp::topology_traits<StateSpace>::point_type& end_point,
const InputTrajectory& u_traj,
double start_time,
double end_time,
double time_step) {
typedef typename pp::topology_traits<StateSpace>::point_type PointType;
typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
double t = start_time;
end_point = start_point;
while(((time_step > 0.0) && (t < end_time)) ||
((time_step < 0.0) && (t > end_time))) {
PointType w = end_point;
PointDiffType k1 = time_step * dp;
end_point = space.adjust(end_point, 0.5 * k1);
t += time_step * 0.5;
u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k2 = time_step * dp;
end_point = space.adjust(w, 0.5 * k2);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k3 = time_step * dp;
end_point = space.adjust(w, k3);
t += time_step * 0.5;
u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
end_point = space.adjust(end_point, (1.0 / 6.0) * k1 + (2.0 / 6.0) * k2 + (time_step / 6.0) * dp - (2.0/3.0) * k3);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
};
};
typename pp::topology_traits<StateSpace>::point_type
sample_gaussian_point(const StateSpace& space,
const typename pp::topology_traits<StateSpace>::point_type& mean,
const mat<ValueType, mat_structure::diagonal>& cov) {
typedef typename pp::topology_traits<StateSpace>::point_difference_type DiffType;
typedef typename mat_traits< mat<ValueType, mat_structure::diagonal> >::size_type SizeType;
using std::sqrt;
using ReaK::to_vect;
using ReaK::from_vect;
boost::variate_generator< global_rng_type&, boost::normal_distribution<ValueType> > var_rnd(get_global_rng(), boost::normal_distribution<ValueType>());
vect_n<ValueType> z = to_vect<ValueType>(space.difference(mean, mean));
for(SizeType i = 0; i < z.size(); ++i)
z[i] = var_rnd() * sqrt(cov(i,i));
return space.adjust(mean, from_vect<DiffType>(z));
};
void dormand_prince45_integrate_impl(
const StateSpace& space,
const StateSpaceSystem& sys,
const typename pp::topology_traits<StateSpace>::point_type& start_point,
typename pp::topology_traits<StateSpace>::point_type& end_point,
const InputTrajectory& u_traj,
double start_time,
double end_time,
double time_step,
double tolerance,
double min_step,
double max_step) {
using std::fabs;
using std::pow;
using ReaK::to_vect;
typedef typename pp::topology_traits<StateSpace>::point_type PointType;
typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
if ((time_step == 0.0) ||
((time_step > 0.0) && (start_time > end_time)) ||
((time_step < 0.0) && (end_time > start_time)) ||
(tolerance <= 0.0) ||
(min_step > max_step))
throw impossible_integration(start_time, end_time, time_step);
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
double t = start_time;
end_point = start_point;
while(((time_step > 0.0) && (t < end_time)) ||
((time_step < 0.0) && (t > end_time))) {
PointType prevY = end_point;
PointDiffType k1 = time_step * dp;
end_point = space.adjust(end_point, 0.2 * k1);
t += time_step / 5.0;
u_wp = u_traj.move_time_diff_from(u_wp, time_step / 5.0);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k2 = time_step * dp;
end_point = space.adjust(prevY, (3.0 / 40.0) * k1 + (9.0 / 40.0) * k2);
t += time_step / 10.0;
u_wp = u_traj.move_time_diff_from(u_wp, time_step / 10.0);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k3 = time_step * dp;
end_point = space.adjust(prevY, (44.0 / 45.0) * k1 - (56.0 / 15.0) * k2 + (32.0 / 9.0) * k3);
t += time_step / 2.0;
u_wp = u_traj.move_time_diff_from(u_wp, time_step / 2.0);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k4 = time_step * dp;
end_point = space.adjust(prevY, (19372.0 / 6561.0) * k1 - (25360.0 / 2187.0) * k2 + (64448.0 / 6561.0) * k3 - (212.0 / 729.0) * k4);
t += 4.0 * time_step / 45.0;
u_wp = u_traj.move_time_diff_from(u_wp, 4.0 * time_step / 45.0);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k5 = time_step * dp;
end_point = space.adjust(prevY, (9017.0 / 3168.0) * k1 - (355.0 / 33.0) * k2 - (46732.0 / 5247.0) * k3 + (49.0 / 176.0) * k4 - (5103.0 / 18656.0) * k5);
t += time_step / 9.0;
u_wp = u_traj.move_time_diff_from(u_wp, time_step / 9.0);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k6 = time_step * dp;
end_point = space.adjust(prevY, (35.0 / 384.0) * k1 + (500.0 / 1113.0) * k3 + (125.0 / 192.0) * k4 - (2187.0 / 6784.0) * k5 + (11.0 / 84.0) * k6);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
PointDiffType k7 = time_step * dp;
vect_n<double> err_vect = to_vect<double>((5170.0 / 57600.0) * k1 + (7571.0 / 16695.0) * k3 + (393.0 / 640.0) * k4 - (92097.0 / 339200.0) * k5 + (187.0 / 2100.0) * k6 - (39.0 / 40.0) * k7);
double Rmax = 0.0;
std::size_t worst_DOF = 0;
for(std::size_t i = 0; i < err_vect.size(); ++i) {
double R = fabs(err_vect[i] / time_step);
if(R > Rmax) {
Rmax = R;
worst_DOF = i;
};
};
if(Rmax > tolerance) {
if(fabs(time_step) <= min_step)
throw untolerable_integration(tolerance, Rmax, worst_DOF, time_step, t);
t -= time_step;
u_wp = u_traj.move_time_diff_from(u_wp, -time_step);
end_point = prevY;
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
double R = 0.84 * pow(tolerance / Rmax, 0.25);
if(R < 0.1)
time_step *= 0.1;
else
time_step *= R;
} else {
end_point = space.adjust(prevY, (5170.0 / 57600.0) * k1 + (7571.0 / 16695.0) * k3 + (393.0 / 640.0) * k4 - (92097.0 / 339200.0) * k5 + (187.0 / 2100.0) * k6 + (1.0 / 40.0) * k7);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
double R = 0.84 * pow(tolerance / Rmax, 0.25);
if(R >= 4.0)
time_step *= 4.0;
else if(R > 1.0)
time_step *= R;
//if(((time_step > 0.0) && (t + time_step > end_time)) || ((time_step < 0.0) && (t + time_step < end_time)))
//time_step = end_time - t;
};
if(fabs(time_step) < min_step)
time_step *= fabs(min_step / time_step);
if(fabs(time_step) > max_step)
time_step *= fabs(max_step / time_step);
};
};
void hamming_mod_integrate_impl(
const StateSpace& space,
const StateSpaceSystem& sys,
const typename pp::topology_traits<StateSpace>::point_type& start_point,
typename pp::topology_traits<StateSpace>::point_type& end_point,
const InputTrajectory& u_traj,
double start_time,
double end_time,
double time_step) {
if ((time_step == 0.0) ||
((time_step > 0.0) && (start_time > end_time)) ||
((time_step < 0.0) && (start_time < end_time)))
throw impossible_integration(start_time, end_time, time_step);
typedef typename pp::topology_traits<StateSpace>::point_type PointType;
typedef typename pp::topology_traits<StateSpace>::point_difference_type PointDiffType;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::const_waypoint_descriptor InputWaypoint;
typedef typename pp::spatial_trajectory_traits<InputTrajectory>::point_type InputType;
std::pair< InputWaypoint, InputType> u_wp = u_traj.get_waypoint_at_time(start_time);
double back_time = start_time;
PointDiffType dp = sys.get_state_derivative(space, start_point, u_wp.second.pt, start_time);
PointType w = start_point;
PointType Y_n[3];
PointDiffType Yp_n[3];
for(std::size_t i = 0; i < 3; ++i) {
PointDiffType k1 = time_step * dp;
Y_n[i] = space.adjust(w, -0.25 * k1);
back_time -= time_step * 0.25;
u_wp = u_traj.move_time_diff_from(u_wp, -0.25 * time_step);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
PointDiffType k2 = time_step * Yp_n_1;
Y_n[i] = space.adjust(Y_n[i], (5.0 / 32.0) * k1 - (9.0 / 32.0) * k2);
back_time -= time_step * 0.125;
u_wp = u_traj.move_time_diff_from(u_wp, -0.125 * time_step);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
PointDiffType k3 = time_step * Yp_n[i];
Y_n[i] = space.adjust(Y_n[i], (1250865.0 / 351520.0) * k2 - (276165.0 / 351520.0) * k1 - (1167360.0 / 351520.0) * k3);
back_time -= 57.0 * time_step / 104.0;
u_wp = u_traj.move_time_diff_from(u_wp, -57.0 / 104.0 * time_step);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
PointDiffType k4 = time_step * Yp_n[i];
Y_n[i] = space.adjust(w, 8.0 * k2 - (439.0 / 216.0) * k1 - (3680.0 / 513.0) * k3 + (845.0 / 4104.0) * k4);
back_time -= time_step / 13.0;
u_wp = u_traj.move_time_diff_from(u_wp, -time_step / 13.0);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
PointDiffType k5 = time_step * Yp_n[i];
Y_n[i] = space.adjust(w, (8.0 / 27.0) * k1 - 2.0 * k2 + (3544.0 / 2565.0) * k3 - (1859.0 / 4104.0) * k4 + (11.0 / 40.0) * k5);
back_time += time_step * 0.5;
u_wp = u_traj.move_time_diff_from(u_wp, 0.5 * time_step);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
Y_n[i] = space.adjust(w, (-16.0 / 135.0) * k1 - (6656.0 / 12825.0) * k3 - (28561.0 / 56430.0) * k4 + (9.0 / 50.0) * k5 - (2.0 * time_step / 55.0) * Yp_n[i]);
back_time -= time_step * 0.5;
u_wp = u_traj.move_time_diff_from(u_wp, -0.5 * time_step);
Yp_n[i] = sys.get_state_derivative(space, Y_n[i], u_wp.second.pt, back_time);
w = Y_n[i];
dp = Yp_n[i];
};
PointType C = start_point;
PointType P_n = start_point; // this makes sure the initial error sweep is zero.
double t = start_time;
end_point = start_point;
u_wp = u_traj.get_waypoint_at_time(t);
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
while(((time_step > 0.0) && (t < end_time)) ||
((time_step < 0.0) && (t > end_time))) {
PointType P = space.adjust(Y_n[2], (time_step * 4.0 / 3.0) * (2.0 * (dp + Yp_n[1]) - Yp_n[0]));
PointType M = space.adjust(P, (112.0 / 121.0) * space.difference(C, P_n));
t += time_step;
u_wp = u_traj.move_time_diff_from(u_wp, time_step);
PointDiffType Mp = sys.get_state_derivative(space, M, u_wp.second.pt, t);
C = space.adjust(end_point, 0.125 * space.difference(end_point, Y_n[1]) + (time_step * 3.0 / 8.0) * (Mp + 2.0 * dp - Yp_n[0]));
Y_n[2] = Y_n[1];
Y_n[1] = Y_n[0];
Y_n[0] = end_point;
end_point = space.adjust(C, (9.0 / 121.0) * space.difference(P, C));
P_n = P;
Yp_n[1] = Yp_n[0];
Yp_n[0] = dp;
dp = sys.get_state_derivative(space, end_point, u_wp.second.pt, t);
};
};