// The given `check' function must be called every iteration.
// It may call setPreempted() or perform other tasks.
bool BaseStrategy::execute(std::function<bool()> check)
{
ros::Rate r(2); // Hz
while (node_.ok() && check() && !isPreempted() && !finished_)
{
callback_queue_.callAvailable();
// TODO: check map age.
// TODO: getNumPublishers here sometimes segfaults on
// succeeded/preempted, for no apparent reason.
if (slam_map_subscriber_.getNumPublishers() == 0)
{
// SLAM is not running.
ROS_INFO("Waiting for SLAM to start publishing a map...");
}
else if (map_ && pose_initialized_)
{
if (obstacle_map_publisher_.getNumSubscribers() > 0)
{
obstacle_map_publisher_.publish(createOccupancyGrid(*map_));
}
if (!map_ready_)
{
const int Rfree = std::ceil(initial_free_radius_ / map_->resolution);
freeRobotPose(*map_, map_->getIdxForPosition(pose_.position), Rfree);
// TODO: This is too conservative. "!= -1" should be used instead, but
// for now it's like this to handle the point of a map where the
// only frontier is the robot position.
if (map_updated_ && map_->isBoxKnown(pose_.position, safety_radius_) == 1)
{
map_ready_ = true;
}
else
{
ROS_INFO_COND(map_updated_, "Position is outside map. Initialization manoeuvres...");
executeInitStep(map_updated_);
}
}
if (map_ready_)
{
if (map_updated_)
{
executeStep();
}
else
{
executeControlStep();
}
}
map_updated_ = false;
}
else
{
if (map_updated_)
{
ROS_INFO("Received map is empty. Initialization manoeuvres...");
}
executeInitStep(map_updated_);
}
r.sleep();
}
return finished_;
}
int main(int argc, char* argv[])
{
//srand(time(NULL));
LOG_INFO("Initializing problem parameters");
std::vector<Matrix<P_DIM> > l(NUM_LANDMARKS);
l[0][0] = 30; l[0][1] = -10;
l[1][0] = 70; l[1][1] = 12.5;
l[2][0] = 20; l[2][1] = 10;
initProblemParams(l);
stateMPC_params problem;
stateMPC_output output;
stateMPC_info info;
setupStateVars(problem, output);
util::Timer solveTimer, trajTimer;
double totalSolveTime = 0, trajTime = 0;
double totalTrajCost = 0;
std::vector<Matrix<B_DIM> > B_total(T*NUM_WAYPOINTS);
std::vector<Matrix<U_DIM> > U_total((T-1)*NUM_WAYPOINTS);
int Bidx = 0, Uidx = 0;
vec(x0, SqrtSigma0, B_total[0]);
std::vector<Matrix<B_DIM> > B(T);
Matrix<B_DIM> b, b_tp1;
Matrix<X_DIM> x;
Matrix<X_DIM,X_DIM> s;
Matrix<X_DIM> x_t_real = x0, x_tp1_real;
Matrix<B_DIM> b_tp1_tp1;
Matrix<U_DIM> uinit;
std::vector<Matrix<C_DIM> > X(T), X_lookahead(T), X_current_mpc(T);
std::vector<Matrix<U_DIM> > U(T-1), U_lookahead(T-1), U_current_mpc(T-1);
planBspPath(x0.subMatrix<C_DIM,1>(0,0), 0, SqrtSigma0, problem, output, info, X_current_mpc, U_current_mpc);
for(int i=0; i < NUM_WAYPOINTS; ++i) {
if (i < NUM_WAYPOINTS - 1) {
// integrate from waypoint i to waypoint i+1
planBspPath(X_current_mpc[T-1].subMatrix<C_DIM,1>(0,0), i+1, finalSqrtSigma(x0, SqrtSigma0, U_current_mpc, T), problem, output, info, X_lookahead, U_lookahead);
} else {
X_current_mpc = X_lookahead;
U_current_mpc = U_lookahead;
}
xMidpoint = X_current_mpc[T-1];
std::cout << "xMidpoint: " << ~xMidpoint.subMatrix<C_DIM,1>(0,0);
//std::cout << "Display lookahead\n";
//pythonDisplayTrajectory(U_lookahead, T, false);
x0.insert(0, 0, X_current_mpc[0]);
for(int t=0; t < T - 1; ++t) {
std::cout << "Going to waypoint " << i << ", timestep " << t << "\n";
//x0[2] = nearestAngleFromTo(0, x0[2]); // need to remod back to near 0
T_MPC = T - t;
xGoal.insert(0,0,X_lookahead[t]);
xGoal.insert(C_DIM, 0, x0.subMatrix<L_DIM,1>(C_DIM,0));
cfg::initial_trust_box_factor = 1;
cfg::initial_penalty_coeff_factor = 1;
std::vector<Matrix<C_DIM> > X_past_mpc = X_current_mpc;
std::vector<Matrix<U_DIM> > U_past_mpc = U_current_mpc;
double cost = 0;
int iter = 0;
while(true) {
try {
cost = statePenaltyCollocation(X_current_mpc, U_current_mpc, problem, output, info);
break;
}
catch (forces_exception &e) {
if (iter > 3) {
LOG_ERROR("Tried too many times, giving up");
std::cout << "X\n";
for(int j=0; j < T; ++j) {
std::cout << ~X_current_mpc[j];
}
std::cout << "U\n";
for(int j=0; j < T-1; ++j) {
std::cout << ~U_current_mpc[j];
}
pythonDisplayTrajectory(U_current_mpc, T, true);
exit(-1);
}
LOG_ERROR("Forces exception, trying again");
cfg::initial_trust_box_factor *= .5;
//cfg::initial_penalty_coeff_factor += 1;
//X_current_mpc[0] = x0;
//for(int j=0; j < T-1; ++j) {
// X_current_mpc[j+1] = dynfunc(X_current_mpc[j], U_current_mpc[j], zeros<Q_DIM,1>());
//}
//X_current_mpc[T-1] = xGoal;
X_current_mpc = X_past_mpc;
U_current_mpc = U_past_mpc;
iter++;
}
}
//std::cout << "x0: " << ~x0.subMatrix<C_DIM,1>(0,0);
//std::cout << "xMidpoint: " << ~xMidpoint.subMatrix<C_DIM,1>(0,0);
//std::cout << "xGoal: " << ~xGoal.subMatrix<C_DIM,1>(0,0);
X_current_mpc[0] = x0.subMatrix<C_DIM,1>(0,0);
for(int j=0; j < T - 1; ++j) {
X_current_mpc[j+1] = dynfunccar(X_current_mpc[j], U_current_mpc[j]);
}
//std::cout << "X_current_mpc:\n";
//for(int t=0; t < T; ++t) { std::cout << ~X_current_mpc[t].subMatrix<C_DIM,1>(0,0); }
//std::cout << "\n";
//std::cout << "U_current_mpc:\n";
//for(int t=0; t < T-1; ++t) { std::cout << ~U_current_mpc[t]; }
//std::cout << "\n";
std::cout << "\nInner loop counter: " << counter_inner_loop << "\n";
std::cout << "Approx hess neg counter: " << counter_approx_hess_neg << "\n\n";
counter_inner_loop = 0;
counter_approx_hess_neg = 0;
// if (i >= 2) {
// std::cout << "Displaying mpc\n";
// pythonDisplayTrajectory(U_current_mpc, T, false);
// }
vec(x0, SqrtSigma0, b);
#define SIMULATE_NOISE
#ifdef SIMULATE_NOISE
executeControlStep(x_t_real, B_total[Bidx], U_current_mpc[0], x_tp1_real, b_tp1_tp1);
x_t_real = x_tp1_real;
B_total[++Bidx] = b_tp1_tp1;
unVec(b_tp1_tp1, x0, SqrtSigma0);
#else
b_tp1 = beliefDynamics(b, U_current_mpc[0]);
B_total[++Bidx] = b_tp1;
unVec(b_tp1, x0, SqrtSigma0);
#endif
U_total[Uidx++] = U_current_mpc[0];
// shift controls forward in time by one, then reintegrate trajectory
// should give a better initialization for next iteration
for(int j=0; j < T-2; ++j) { U_current_mpc[j] = U_current_mpc[j+1]; }
if (i < NUM_WAYPOINTS - 1) {
U_current_mpc[T-2] = U_lookahead[t];
} else {
U_current_mpc[T-2][0] = uMin[0];
U_current_mpc[T-2][1] = 0;
}
X_current_mpc[0] = x0.subMatrix<C_DIM,1>(0,0);
for(int j=0; j < T-1; ++j) {
X_current_mpc[j+1] = dynfunccar(X_current_mpc[j], U_current_mpc[j]);
}
//pythonDisplayTrajectory(B_total, U_total, waypoints, landmarks, Bidx, false);
}
X_current_mpc = X_lookahead;
U_current_mpc = U_lookahead;
//pythonDisplayTrajectory(B_total, U_total, waypoints, landmarks, T*(i+1), false);
}
pythonDisplayTrajectory(B_total, U_total, waypoints, landmarks, T*NUM_WAYPOINTS, true);
cleanupStateMPCVars();
return 0;
}
int main(int argc, char* argv[])
{
// actual: 0.15, 0.15, 0.1, 0.1
double length1_est = .3,
length2_est = .7,
mass1_est = .3,
mass2_est = .35;
// position, then velocity
x0[0] = M_PI*0.5; x0[1] = M_PI*0.5; x0[2] = 0; x0[3] = 0;
// parameter start estimates (alphabetical, then numerical order)
x0[4] = 1/length1_est; x0[5] = 1/length2_est; x0[6] = 1/mass1_est; x0[7] = 1/mass2_est;
Matrix<X_DIM> x_real;
x_real[0] = M_PI*0.45; x_real[1] = M_PI*0.55; x_real[2] = -0.01; x_real[3] = 0.01;
x_real[4] = 1/dynamics::length1; x_real[5] = 1/dynamics::length2; x_real[6] = 1/dynamics::mass1; x_real[7] = 1/dynamics::mass2;
// from original file, possibly change
SqrtSigma0(0,0) = 0.1;
SqrtSigma0(1,1) = 0.1;
SqrtSigma0(2,2) = 0.05;
SqrtSigma0(3,3) = 0.05;
SqrtSigma0(4,4) = 0.5;
SqrtSigma0(5,5) = 0.5;
SqrtSigma0(6,6) = 0.5;
SqrtSigma0(7,7) = 0.5;
//Matrix<U_DIM> uinit = (xGoal.subMatrix<U_DIM,1>(0,0) - x0.subMatrix<U_DIM,1>(0,0))/(double)(T-1);
Matrix<U_DIM> uinit;
uinit[0] = -.07;
uinit[1] = .07;
std::vector<Matrix<U_DIM> > U(T-1, uinit);
std::vector<Matrix<B_DIM> > B(T);
std::vector<Matrix<U_DIM> > HistoryU(HORIZON);
std::vector<Matrix<B_DIM> > HistoryB(HORIZON);
vec(x0, SqrtSigma0, B[0]);
for(int h = 0; h < HORIZON; ++h) {
for(int t = 0; t < T-1; ++t) {
for(int i = 0; i<U_DIM; i++){
U[t][i] = -U[t][i];
}
}
for (int t = 0; t < T-1; ++t) {
B[t+1] = beliefDynamics(B[t], U[t]);
}
//util::Timer solveTimer;
//util::Timer_tic(&solveTimer);
//pythonDisplayTrajectory(U, SqrtSigma0, x0, xGoal);
//pythonPlotRobot(U, SqrtSigma0, x0, xGoal);
//double solvetime = util::Timer_toc(&solveTimer);
//LOG_INFO("Optimized cost: %4.10f", cost);
//LOG_INFO("Solve time: %5.3f ms", solvetime*1000);
unVec(B[0], x0, SqrtSigma0);
//std::cout << "x0 before control step" << std::endl << ~x0;
//std::cout << "control u: " << std::endl << ~U[0];
//pythonDisplayTrajectory(U, SqrtSigma0, x0, xGoal);
LOG_INFO("Executing control step");
HistoryU[h] = U[0];
HistoryB[h] = B[0];
B[0] = executeControlStep(x_real, B[0], U[0]);
std::cout<<U[0]<<"\n";
unVec(B[0], x0, SqrtSigma0);
//std::cout << "x0 after control step" << std::endl << ~x0;
}
std::cout<<"done\n";
#define PLOT
#ifdef PLOT
pythonDisplayHistory(HistoryU,HistoryB, SqrtSigma0, x0, HORIZON);
//int k;
//std::cin >> k;
#endif
return 0;
}
