options set_config_option(options const & opts, char const * in) {
if (!in) return opts;
while (*in && std::isspace(*in))
++in;
std::string in_str(in);
auto pos = in_str.find('=');
if (pos == std::string::npos)
throw lean::exception("invalid -D parameter, argument must contain '='");
lean::name opt = lean::string_to_name(in_str.substr(0, pos));
std::string val = in_str.substr(pos+1);
auto decls = lean::get_option_declarations();
auto it = decls.find(opt);
if (it != decls.end()) {
switch (it->second.kind()) {
case lean::BoolOption:
if (val == "true")
return opts.update(opt, true);
else if (val == "false")
return opts.update(opt, false);
else
throw lean::exception(lean::sstream() << "invalid -D parameter, invalid configuration option '" << opt
<< "' value, it must be true/false");
case lean::IntOption:
case lean::UnsignedOption:
return opts.update(opt, atoi(val.c_str()));
default:
throw lean::exception(lean::sstream() << "invalid -D parameter, configuration option '" << opt
<< "' cannot be set in the command line, use set_option command");
}
} else {
throw lean::exception(lean::sstream() << "invalid -D parameter, unknown configuration option '" << opt << "'");
}
}
static bool has_show(options const & opts, name const & kind, unsigned & line, unsigned & col) {
if (opts.get_bool(kind)) {
line = opts.get_unsigned("line", 0);
col = opts.get_unsigned("column", 0);
return true;
} else {
return false;
}
}
context(const benchmark_info& info, const options& opts)
: d_timer_on(false)
, d_start()
, d_duration()
, d_benchtime(std::chrono::seconds(opts.get_benchtime()))
, d_num_iterations(1)
, d_num_threads(opts.get_cpu())
, d_num_bytes(0)
, d_benchmark(info)
{}
void display(options& opt, spec::runner::result const& res)
{
// stdout is default so this is valid
std::ostream output( std::cout.rdbuf() );
bool file_output = false;
std::ofstream file_stream;
std::string const& format = opt.format();
std::string const& output_method = opt.output_method();
boost::shared_ptr<output_format::base> outformat;
std::string file_extension;
if( output_method == "stderr" )
output.rdbuf( std::cerr.rdbuf() );
else if( output_method == "file" )
file_output = true;
// Add you output format the same way as bellow DON'T FORGET TO ADD
// FILE EXTENSION
// ---------------------------------------------------------------------------
if( format == "compiler" )
{
outformat.reset( new output_format::compiler() );
file_extension = ".txt";
}
// ---------------------------------------------------------------------------
else if( format == "xml" )
{
outformat.reset( new output_format::xml() );
file_extension = ".xml";
}
// ---------------------------------------------------------------------------
else
{
throw std::runtime_error( "output format not supported" );
}
if( file_output )
{
std::string filename = opt.filename()+file_extension;
file_stream.open(filename.c_str(), std::ios_base::out|std::ios_base::trunc);
if( file_stream.is_open() )
output.rdbuf( file_stream.rdbuf() );
}
spec::output out( outformat, res, output );
out.display();
}
std::size_t TestFixture::runTests(const options& args)
{
std::string classname(args.which_test());
std::string testname("");
if (classname.find("::") != std::string::npos) {
testname = classname.substr(classname.find("::") + 2);
classname.erase(classname.find("::"));
}
countTests = 0;
errmsg.str("");
const std::list<TestFixture *> &tests = TestRegistry::theInstance().tests();
for (std::list<TestFixture *>::const_iterator it = tests.begin(); it != tests.end(); ++it) {
if (classname.empty() || (*it)->classname == classname) {
(*it)->processOptions(args);
(*it)->run(testname);
}
}
std::cout << "\n\nTesting Complete\nNumber of tests: " << countTests << std::endl;
std::cout << "Number of todos: " << todos_counter;
if (succeeded_todos_counter > 0)
std::cout << " (" << succeeded_todos_counter << " succeeded)";
std::cout << std::endl;
// calling flush here, to do all output before the error messages (in case the output is buffered)
std::cout.flush();
std::cerr << "Tests failed: " << fails_counter << std::endl << std::endl;
std::cerr << errmsg.str();
std::cerr.flush();
return fails_counter;
}
options join(options const & opts1, options const & opts2) {
sexpr r = opts2.m_value;
for_each(opts1.m_value, [&](sexpr const & p) {
if (!opts2.contains(to_name(car(p))))
r = cons(p, r);
});
return options(r);
}
/*
* The run_benchmarks function will run the registered benchmarks.
*/
void run_benchmarks(const options& opts) {
std::regex match_r(opts.get_bench());
auto benchmarks = registration::get_ptr()->get_benchmarks();
for (auto& info : benchmarks) {
if (std::regex_match(info.get_name(), match_r)) {
context c(info, opts);
auto r = c.run();
std::cout << std::setw(35) << std::left << info.get_name() << r.to_string() << std::endl;
}
}
}
int parse_options(int argc, const char** argv, options& opts) {
eagine::program_args args(argc, argv);
for(auto a = args.first(); a; a = a.next()) {
if(a.is_help_arg()) {
opts.print_usage(std::cout);
return 1;
} else if(!parse_argument(a, opts)) {
opts.print_usage(std::cerr);
return 2;
}
}
if(!opts.check(std::cerr)) {
opts.print_usage(std::cerr);
return 3;
}
return 0;
}
bool parse_argument(eagine::program_arg& a, options& opts) {
if(!opts.parse(a, std::cerr)) {
std::cerr
<< "Failed to parse argument '"
<< a.get()
<< "'"
<< std::endl;
return false;
}
return true;
}
bool options::operator==(const options &rhs)
{
// iterate over options in the first list
for (entry *curentry = m_entrylist.first(); curentry != nullptr; curentry = curentry->next())
if (!curentry->is_header())
{
// if the values differ, return false
if (strcmp(curentry->value(), rhs.value(curentry->name())) != 0)
return false;
}
return true;
}
void display_pos(std::ostream & out, options const & o, char const * strm_name, unsigned line, unsigned pos) {
out << strm_name << ":";
if (o.get_bool("flycheck", false)) {
// generate valid line and column for flycheck mode
if (line == static_cast<unsigned>(-1))
line = 1;
if (pos == static_cast<unsigned>(-1))
pos = 0;
}
if (line != static_cast<unsigned>(-1))
out << line << ":";
if (pos != static_cast<unsigned>(-1))
out << pos << ":";
}
void
augd::createsslctxs(sslctxs& sslctxs, const options& options, char* frobpass)
{
const char* contexts = options.get("ssl.contexts", 0);
if (contexts) {
istringstream is(contexts);
string name;
while (is >> name)
sslctxs.insert(make_pair(name, createsslctx(name, options,
frobpass)));
}
}
int main (int argc, const char * argv[]) {
multiHMM models;
opt.set_parameters(commandline,opt_size,usage);
opt.parse_commandline(argc,argv);
if (!opt.optset("-seq")){
cerr << "No sequence file defined in command line input\n Please check syntax.\n";
//cout << usage << endl;
}
if (!opt.optset("-model")){
cerr << "No model/models file defined in command line input\n Please check syntax.\n";
//cout << usage << endl;
}
else{
models.importModels(opt.sopt("-model"));
}
//for(int i=0;i<models.size();i++){
// models[i].print_model();
//}
seqs.load_fastq(models, opt.sopt("-seq"));
//int count=1;
while(1){
seqSet *job = seqs.getJob();
job->getSidd();
//cout << "Seq:\t" << count <<endl;
//count++;
job->print_seq();
//cout << job->model <endl;
delete job;
}
return 0;
}
bool get_elaborator_lift_coercions(options const & opts) {
return opts.get_bool(*g_elaborator_lift_coercions, LEAN_DEFAULT_ELABORATOR_LIFT_COERCIONS);
}
bool get_elaborator_fail_missing_field(options const & opts) {
return opts.get_bool(*g_elaborator_fail_missing_field, LEAN_DEFAULT_ELABORATOR_FAIL_MISSING_FIELD);
}
bool get_elaborator_flycheck_goals(options const & opts) {
return opts.get_bool(*g_elaborator_flycheck_goals, LEAN_DEFAULT_ELABORATOR_FLYCHECK_GOALS);
}
bool get_elaborator_ignore_instances(options const & opts) {
return opts.get_bool(*g_elaborator_ignore_instances, LEAN_DEFAULT_ELABORATOR_IGNORE_INSTANCES);
}
bool get_elaborator_local_instances(options const & opts) {
return opts.get_bool(*g_elaborator_local_instances, LEAN_DEFAULT_ELABORATOR_LOCAL_INSTANCES);
}
int hpx_main(int argc, char* argv[]) {
printf("Running\n");
// auto test_fut = hpx::async([]() {
// while(1){hpx::this_thread::yield();}
// });
// test_fut.get();
try {
if (opts.process_options(argc, argv)) {
auto all_locs = hpx::find_all_localities();
std::vector<hpx::future<void>> futs;
futs.reserve(all_locs.size());
for (auto i = all_locs.begin(); i != all_locs.end(); ++i) {
futs.push_back(hpx::async < initialize_action > (*i, opts));
}
hpx::when_all(futs).get();
node_client root_id = hpx::new_ < node_server > (hpx::find_here());
node_client root_client(root_id);
if (opts.found_restart_file) {
set_problem(null_problem);
const std::string fname = opts.restart_filename;
printf("Loading from %s...\n", fname.c_str());
if (opts.output_only) {
const std::string oname = opts.output_filename;
root_client.get_ptr().get()->load_from_file_and_output(fname, oname);
} else {
root_client.get_ptr().get()->load_from_file(fname);
root_client.regrid(root_client.get_gid(), true).get();
}
printf("Done. \n");
} else {
for (integer l = 0; l < opts.max_level; ++l) {
root_client.regrid(root_client.get_gid(), false).get();
printf("---------------Created Level %i---------------\n\n", int(l + 1));
}
root_client.regrid(root_client.get_gid(), false).get();
printf("---------------Regridded Level %i---------------\n\n", int(opts.max_level));
}
std::vector < hpx::id_type > null_sibs(geo::direction::count());
printf("Forming tree connections------------\n");
root_client.form_tree(root_client.get_gid(), hpx::invalid_id, null_sibs).get();
if (gravity_on) {
//real tstart = MPI_Wtime();
root_client.solve_gravity(false).get();
// printf("Gravity Solve Time = %e\n", MPI_Wtime() - tstart);
}
printf("...done\n");
if (!opts.output_only) {
// set_problem(null_problem);
root_client.start_run(opts.problem == DWD && !opts.found_restart_file).get();
}
root_client.report_timing();
}
} catch (...) {
throw;
}
printf("Exiting...\n");
return hpx::finalize();
}
bool get_apply_class_instance(options const & opts) {
return opts.get_bool(*g_apply_class_instance, LEAN_DEFAULT_APPLY_CLASS_INSTANCE);
}
void TestFixture::processOptions(const options& args)
{
quiet_tests = args.quiet();
gcc_style_errors = args.gcc_style_errors();
}
void HDDP(options & probdata, options & nominal, int & go_to_step, int & current_iter, bool & converged){
double rho;
cout << "Iteration " << current_iter << ": ";
switch (go_to_step){
// Step 1: Computation of First and Second Order STMs
case 1:
cout << "Step 1: Computation of First and Second Order STMS \n";
ComputeSTMs(nominal);
go_to_step = 2;
break;
// Step 2: Backward Sweep
case 2:
cout << "Step 2: Backward Sweep \n";
BackwardSweep(nominal);
go_to_step = 3;
break;
// Step 3: Convergence Check
case 3:
cout << "Step 3: Convergence Check \n";
if ((abs(nominal.ER(0, 0)) < nominal.tol && (nominal.f < nominal.tol) && (nominal.delta <= nominal.delta_min + 1e-6)) || (nominal.delta <= nominal.delta_min + 1e-6)){
cout << "I think we're done here \n";
//cout << "Feasible Solution Within Tolerance \n";
converged = 1;
break;
}
cout << nominal.ER(0, 0) << " " << nominal.J << " " << nominal.h << " " << nominal.f << " " << nominal.delta << "\n";
cout << probdata.ER(0, 0) << " " << probdata.J << " " << probdata.h << " " << probdata.f << " " << probdata.delta << "\n";
go_to_step = 4;
break;
// Step 4: Forward Pass
case 4:
current_iter++;
probdata = nominal;
cout << "Step 4: Forward Pass \n";
ForwardPass(probdata, nominal);
go_to_step = 5;
break;
// Step 5: Trust Region Update
case 5:
cout << "Step 5: Trust Region Update \n";
eval_J(probdata);
rho = (probdata.J - nominal.J) / probdata.ER(0, 0);
if (current_iter < 0){
cout << "Accepted rho = " << rho << "\n";
go_to_step = 6;
break;
} //(.8 < rho && rho < 1.2){
else if (probdata.J < nominal.J){
cout << "Accepted rho = " << rho << "\n";
go_to_step = 6;
break;
} // second chance
else {
nominal.delta = max(nominal.delta*(1. - nominal.kappa), nominal.delta_min);
cout << "Rejected rho = " << rho << "\n";
go_to_step = 2;
break;
}
// Step 6: Nominal Solution Update
case 6:
cout << "Step 6: Nominal Solution Update \n";
rho = (probdata.J - nominal.J) / probdata.ER(0, 0);
probdata.Multipliers += nominal.delta_l;
/**/
// Update rule from Niu
if (probdata.psi.norm() >= nominal.psi.norm()/1.2){
probdata.s = max(1.2*probdata.s, 1.2*probdata.Multipliers.norm());
}
else {
probdata.s = max(probdata.s, 1.2*probdata.Multipliers.norm());
}
eval_J(probdata);
nominal = probdata;
if (.8 < rho && rho < 1.2){
nominal.delta = min(probdata.delta*pow((1. + probdata.kappa),2.0), probdata.delta_max);
go_to_step = 1; // 2 to re-use STM
}
else if (rho > 0){
nominal.delta = min(probdata.delta*pow((1. + probdata.kappa), 1.0), probdata.delta_max);
go_to_step = 1;
}
else {
nominal.delta = max(nominal.delta*(1. - nominal.kappa), nominal.delta_min);
go_to_step = 1;
}
WriteTrajectory(nominal, current_iter);
break;
}
}
transfer_nodes( size_t s,
vec3i const & fsize,
options const & op,
task_manager & tm,
size_t fwd_p,
size_t bwd_p,
bool is_out )
: nodes(s,fsize,op,tm,fwd_p,bwd_p,false,is_out)
, biases_(s)
, func_()
, fwd_dispatch_(s)
, bwd_dispatch_(s)
, fwd_accumulators_(s)
, bwd_accumulators_(s)
, fs_(s)
, fwd_done_(s)
, waiter_(s)
{
for ( size_t i = 0; i < nodes::size(); ++i )
{
fwd_accumulators_[i]
= std::make_unique<forward_accumulator>(fsize);
bwd_accumulators_[i]
= std::make_unique<backward_accumulator>(fsize);
}
auto type = op.require_as<std::string>("type");
if ( type == "transfer" )
{
func_ = get_transfer_function(op);
// initialize biases
real eta = op.optional_as<real>("eta", 0.0001);
real mom = op.optional_as<real>("momentum", 0.0);
real wd = op.optional_as<real>("weight_decay", 0.0);
for ( auto& b: biases_ )
{
b = std::make_unique<bias>(eta, mom, wd);
}
std::string bias_values;
if ( op.contains("biases") )
{
bias_values = op.require_as<std::string>("biases");
}
else
{
real biases_raw[nodes::size()];
if ( op.contains("init") )
{
auto initf = get_initializator(op);
initf->initialize( biases_raw, nodes::size() );
}
else
{
std::fill_n(biases_raw, nodes::size(), 0);
}
bias_values = std::string( reinterpret_cast<char*>(biases_raw),
sizeof(real) * nodes::size() );
}
load_biases(biases_, bias_values);
}
else
{
ZI_ASSERT(type=="sum");
}
}
bool get_parser_show_errors(options const & opts) { return opts.get_bool(g_parser_show_errors, LEAN_DEFAULT_PARSER_SHOW_ERRORS); }
bool get_find_expensive(options const & opts) {
return opts.get_bool(*g_find_expensive, LEAN_DEFAULT_FIND_EXPENSIVE);
}
unsigned get_find_max_steps(options const & opts) {
return opts.get_unsigned(*g_find_max_steps, LEAN_DEFAULT_FIND_MAX_STEPS);
}
option_base::option_base(options &parent, pstring help)
: m_help(help)
{
parent.register_option(this);
}
double get_meng_paulson_c(options const & o) {
return o.get_double(*g_meng_paulson_c, LEAN_DEFAULT_MENG_PAULSON_C);
}
inline edges::edges( nodes * in,
nodes * out,
options const & opts,
vec3i const & stride,
vec3i const & in_size,
task_manager & tm,
filter_tag )
: options_(opts)
, size_(in_size)
, tm_(tm)
{
size_t n = in->num_out_nodes();
size_t m = out->num_in_nodes();
ZI_ASSERT((n>0)&&m>0);
edges_.resize(n*m);
filters_.resize(n*m);
waiter_.set(n*m);
real eta = opts.optional_as<real>("eta", 0.1);
real mom = opts.optional_as<real>("momentum", 0.0);
real wd = opts.optional_as<real>("weight_decay", 0.0);
auto sz = opts.require_as<ovec3i>("size");
size_ = sz;
for ( size_t k = 0; k < n*m; ++k )
{
filters_[k] = std::make_unique<filter>(sz, eta, mom, wd);
}
std::string filter_values;
if ( opts.contains("filters") )
{
filter_values = opts.require_as<std::string>("filters");
}
else
{
size_t n_values = n*m*size_[0]*size_[1]*size_[2];
real * filters_raw = new real[n_values];
auto initf = get_initializator(opts);
initf->initialize( filters_raw, n*m*size_[0]*size_[1]*size_[2] );
filter_values = std::string( reinterpret_cast<char*>(filters_raw),
sizeof(real) * n_values );
delete [] filters_raw;
}
load_filters(filters_, size_, filter_values);
int does_fft = options_.optional_as<int>("fft", "1");
auto repeat = options_.optional_as<ovec3i>("repeat", "1,1,1");
if ( size_ == vec3i::one ) does_fft = 0;
for ( size_t i = 0, k = 0; i < n; ++i )
{
for ( size_t j = 0; j < m; ++j, ++k )
{
if ( repeat == ovec3i::one )
{
if ( does_fft )
{
edges_[k]
= std::make_unique<fft_filter_edge>
(in, i, out, j, tm_, stride, *filters_[k]);
}
else
{
edges_[k]
= std::make_unique<filter_edge>
(in, i, out, j, tm_, stride, *filters_[k]);
}
}
else
{
if ( does_fft )
{
edges_[k]
= std::make_unique<fft_filter_ds_edge>
(in, i, out, j, tm_, stride, repeat, *filters_[k]);
}
else
{
edges_[k]
= std::make_unique<filter_ds_edge>
(in, i, out, j, tm_, stride, repeat, *filters_[k]);
}
}
}
}
}
void BackwardSweep(options & nominal){
double x, y, z, xdot, ydot, zdot;
double xf, yf, zf, xdotf, ydotf, zdotf;
double l0, l1, l2, l3, l4, l5;
// Extract some information
x = nominal.traj[nominal.nstages][1];
y = nominal.traj[nominal.nstages][2];
z = nominal.traj[nominal.nstages][3];
xdot = nominal.traj[nominal.nstages][4];
ydot = nominal.traj[nominal.nstages][5];
zdot = nominal.traj[nominal.nstages][6];
xf = nominal.r_target[0];
yf = nominal.r_target[1];
zf = nominal.r_target[2];
xdotf = nominal.v_target[0];
ydotf = nominal.v_target[1];
zdotf = nominal.v_target[2];
l0 = nominal.Multipliers(0);
l1 = nominal.Multipliers(1);
l2 = nominal.Multipliers(2);
l3 = nominal.Multipliers(3);
l4 = nominal.Multipliers(4);
l5 = nominal.Multipliers(5);
// Compute Terminal Cost Derivatives
Matrix<double> Vl(6, 1), Vll(6, 6, 0.0), Vx(7, 1), Vxx(7, 7, 0.0), Vxl(7, 6);
Vl(0,0) = pow(x - xf, 2.0);
Vl(1,0) = pow(y - yf, 2.0);
Vl(2,0) = pow(z - zf, 2.0);
Vl(3,0) = pow(xdot - xdotf, 2.0);
Vl(4,0) = pow(ydot - ydotf, 2.0);
Vl(5,0) = pow(zdot - zdotf, 2.0);
Vx(0,0) = l0*(x*2.0 - xf*2.0);
Vx(1,0) = l1*(y*2.0 - yf*2.0);
Vx(2,0) = l2*(z*2.0 - zf*2.0);
Vx(3,0) = l3*(xdot*2.0 - xdotf*2.0);
Vx(4,0) = l4*(ydot*2.0 - ydotf*2.0);
Vx(5,0) = l5*(zdot*2.0 - zdotf*2.0);
Vx(6,0) = -1.0*nominal.s;
Vxx(0,0) = l0*2.0;
Vxx(1,1) = l1*2.0;
Vxx(2,2) = l2*2.0;
Vxx(3,3) = l3*2.0;
Vxx(4,4) = l4*2.0;
Vxx(5,5) = l5*2.0;
Vxl(0,0) = x*2.0 - xf*2.0;
Vxl(1,1) = y*2.0 - yf*2.0;
Vxl(2,2) = z*2.0 - zf*2.0;
Vxl(3,3) = xdot*2.0 - xdotf*2.0;
Vxl(4,4) = ydot*2.0 - ydotf*2.0;
Vxl(5,5) = zdot*2.0 - zdotf*2.0;
// Start Sweeping
Matrix<double> lx(7, 1), lxx(7,7), lu(3, 1), luu(3,3), lux(3, 7);
nominal.ER = 0.0;
for (int stage = nominal.nstages - 1; stage >= 0; stage--){
Matrix<double> identity(3, 3), G, K, A, B, C;
// Stage Cost Derivatives
lx = 0.0;
lxx = 0.0;
lux = 0.0;
lu = 0.0;
luu = 0.0;
Matrix<double> u_stage(3, 1);
u_stage(0, 0) = nominal.traj[stage][8];
u_stage(1, 0) = nominal.traj[stage][9];
u_stage(2, 0) = nominal.traj[stage][10];
/*
for (int i = 0; i < 3; i++){
lu(0, i) = .01*nominal.delta_u[stage](i,0);
luu(i, i) = .01*1.0;
}
*/
// Partition the STM
Matrix<double> STM = nominal.STMs[stage];
Matrix<double> STMx(7,7), STMu(7,3);
for (int i = 0; i < 7; i++)
for (int j = 0; j < 7; j++)
STMx(i, j) = STM(i, j);
for (int i = 0; i < 7; i++)
for (int j = 0; j < 3; j++)
STMu(i, j) = STM(i, 7 + j);
// Partition the STT
vector< Matrix<double> > STT, STTxx(7, Matrix<double>(7, 7)), STTuu(3, Matrix<double>(7, 3)), STTux(7, Matrix<double>(7, 3));
for (int i = 0; i < 10; i++)
STT.push_back(nominal.STTs[stage + i]);
for (int i = 0; i < 7; i++)
for (int j = 0; j < 7; j++)
for (int k = 0; k < 7; k++)
STTxx[i](j, k) = STT[i](j, k);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 7; j++)
for (int k = 0; k < 3; k++)
STTuu[i](j, k) = STT[7+i](j,7+k);
for (int i = 0; i < 7; i++)
for (int j = 0; j < 7; j++)
for (int k = 0; k < 3; k++)
STTux[i](j, k) = STT[i](j,7+k);
// Cost-to-go Derivative Mapping
Matrix<double> Jx(7, 1), Jxx(7, 7), Ju(3, 1), Juu(3, 3), Jux(3,7), Jl(1,6), Jll(6,6), Jxl(7,6), Jul(3,6);
Matrix<double> Jxx_temp(7, 7), Juu_temp(3, 3), Jux_temp(3, 7);
// tensor multiplication
for (int i = 0; i < 7; i++){
Jxx_temp.assign_column(i, (Vx.transpose()*STTxx[i]).transpose());
Jux_temp.assign_column(i, (Vx.transpose()*STTux[i]).transpose());
}
for (int i = 0; i < 3; i++){
Juu_temp.assign_column(i, (Vx.transpose()*STTuu[i]).transpose());
}
Jx = (lx.transpose() + Vx.transpose()*STMx).transpose();
Ju = (lu.transpose() + Vx.transpose()*STMu).transpose();
Jxx = lxx + STMx.transpose() * Vxx * STMx + Jxx_temp;
Juu.print_to_screen();
Juu = luu + STMu.transpose() * Vxx * STMu + Juu_temp;
Juu.print_to_screen();
Jux = lux + STMu.transpose() * Vxx * STMx + Jux_temp;
// Multiplier partials
Jl = Vl;
Jll = Vll;
Matrix<double> VXL(10, 6), JX(10, 6);
for (int i = 0; i < 7; i++)
for (int j = 0; j < 6; j++)
VXL(i, j) = Vxl(i, j);
JX = (VXL.transpose()*STM).transpose();
for (int i = 0; i < 7; i++)
for (int j = 0; j < 6; j++)
Jxl(i, j) = JX(i, j);
for (int i = 0; i < 3; i++)
for (int j = 0; j < 6; j++)
Jul(i, j) = JX(7 + i, j);
// TRQP
Matrix<double> Juu_inv(3,3);
Matrix<double> A_unc(3,1);
TRQP(Juu, Ju.transpose(), nominal.delta);
Juu_inv = Juu.inverse();
A_unc = Juu_inv*Ju*-1.0;
// unconstrained control update for dx = dl = 0
Matrix<double> u_unc(3,1);
double u_unc_mag;
u_unc = u_stage + A_unc;
u_unc_mag = u_unc.norm();
if (u_unc_mag > nominal.Tmax){
// enforce hard constraints with quadratic programming
double gc;
gc = u_unc_mag - nominal.Tmax;
// constraint derivatives
Matrix<double> gu(3, 1), gx(7, 1, 0.0);
gu(0, 0) = u_unc(0, 0) / u_unc_mag;
gu(1, 0) = u_unc(1, 0) / u_unc_mag;
gu(2, 0) = u_unc(2, 0) / u_unc_mag;
identity.construct_identity_matrix();
G = (gu.transpose()*Juu_inv*gu).inverse()*gu.transpose()*Juu_inv;
K = Juu_inv*(identity - gu*G);
A = K*Ju*-1.0 - G.transpose()*gc*nominal.delta / max(nominal.delta, (G.transpose()*gc).norm());
B = K.transpose()*Jux*-1.0 - G.transpose()*gx.transpose();
C = K.transpose()*Jul*-1.0;
/*
// interior-point method
Matrix<double> D(3, 3);
for (int i = 0; i < 3; i++){
if (Ju(0, i) < 0.0){
D(i, i) = 1.0 / sqrt(nominal.Tmax - (u_stage(i, 0) + A(i, 0)));
}
else if (Ju(0, i) >= 0.0){
D(i, i) = 1.0 / sqrt((u_stage(i, 0) + A(i, 0)) + nominal.Tmax);
}
}
Juu = (D.inverse())*Juu*(D.inverse().transpose());
TRQP(Juu, Ju, nominal.delta);
Juu_inv = Juu.inverse();
A = Juu_inv*Ju.transpose()*-1.0;
B = Juu_inv*Jxu.transpose()*-1.0;
C = Juu_inv*Jul*-1.0;
*/
} else {
A = A_unc;// -Juu_inv*Ju.transpose();
B = Juu_inv*Jux*-1.0;
C = Juu_inv*Jul*-1.0;
}
// Control Update
//B = B*nominal.delta / max(nominal.delta, (B*nominal.dx_prev[stage]).norm());
//C = C*nominal.delta / max(nominal.delta, (C*nominal.delta_l.transpose()).norm());
B = B*A.norm()*10. / max(A.norm()*10., (B*nominal.dx_prev[stage]).norm());
//C = C*A.norm()*10. / max(A.norm()*10., (C*nominal.delta_l.transpose()).norm());
nominal.As[stage] = A;
nominal.Bs[stage] = B;
nominal.Cs[stage] = C;
nominal.ER += Ju.transpose()*A + A.transpose()*Juu*A*0.5;
Vx = Jx + (Ju.transpose()*B + A.transpose()*Juu*B + A.transpose()*Jux).transpose();
Vxx = Jxx + B.transpose()*Juu*B + B.transpose()*Jux + Jux.transpose()*B;
Vxl = Jxl + B.transpose()*Juu*C + B.transpose()*Jul + Jux.transpose()*C;
Vl = Jl + (Ju.transpose()*C + A.transpose()*Juu*C + A.transpose()*Jul).transpose();
Vll = Jll + C.transpose()*Juu*C + C.transpose()*Jul + Jul.transpose()*C;
}
// TRQP for Multiplier Update
Matrix<double> mVll, mVl;
mVll = Vll*-1.0;
mVl = Vl*-1.0; //- mVl wasn't working
TRQP(mVll, mVl.transpose(), nominal.delta);
nominal.delta_l = mVll.inverse()*mVl*-1.0;
nominal.delta_l = nominal.delta_l.transpose();
Vll = mVll*-1.0;
nominal.ER += Vl.transpose()*nominal.delta_l.transpose() + nominal.delta_l*Vll*nominal.delta_l.transpose()*0.5;
}