USING_NAMESPACE_ACADO


/* >>> start tutorial code >>> */
int main( ){

    // DEFINE VARIABLES:
    // ----------------------
    DifferentialState      x,y;
    DifferentialEquation   f;

    f << dot(x) ==  y;
	f << dot(y) == -x;
	
	Tmatrix<Interval> x_init(2);
	x_init(0) = Interval(-1.0,1.0);
	x_init(1) = Interval(-1.0,1.0);
	
	EllipsoidalIntegrator integrator( f, 8 );

	integrator.set(INTEGRATOR_PRINTLEVEL, MEDIUM );
	integrator.set(INTEGRATOR_TOLERANCE, 1e-8 );
	integrator.set(ABSOLUTE_TOLERANCE, 1e-10 );
	
	integrator.integrate( 0.0, 2*M_PI, 1, x_init );
	
    return 0;
}
Example #2
0
//---------------------------------------------------------------
//for visualization of displacements and velocities:
//---------------------------------------------------------------
double Mass1D::GetFieldVariableValue(const FieldVariableDescriptor & fvd, const double & local_position, bool flagD)
{		
	double p0=local_position;

	if (flagD)
	{
		switch (fvd.VariableType())
		{
		case FieldVariableDescriptor::FVT_position:
			return GetPos1DD(p0);
		case FieldVariableDescriptor::FVT_velocity:
			return GetVel1DD(p0);
		case FieldVariableDescriptor::FVT_displacement:
			return GetPos1DD(p0) - x_init(1);
		}
	}
	else
	{
		switch (fvd.VariableType())
		{
		case FieldVariableDescriptor::FVT_position:
			return GetPos1D(p0);
		case FieldVariableDescriptor::FVT_velocity:
			return GetVel1D(p0);
		case FieldVariableDescriptor::FVT_displacement:
			return GetPos1D(p0) - x_init(1);
		case FieldVariableDescriptor::FVT_acceleration:	//$ DR 2013-07-01 added
			return XGPP(1); // $ MSax 2013-07-16 : changed from GetMBS()->GetAcceleration(LTG(1 + SOS())) to XGPP(1)
		}
	}
	return FIELD_VARIABLE_NO_VALUE;
}
Example #3
0
    GaussFitter::GaussFitResult GaussFitter::fit(vector<DPosition<2> > & input) const
    {
      Eigen::VectorXd x_init (3);
      x_init(0) = init_param_.A;
      x_init(1) = init_param_.x0;
      x_init(2) = init_param_.sigma;
      GaussFunctor functor (3, &input);
      Eigen::LevenbergMarquardt<GaussFunctor> lmSolver (functor);
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      // the states are poorly documented. after checking the source and
      // http://www.ultimatepp.org/reference%24Eigen_demo%24en-us.html we believe that
      // all states except TooManyFunctionEvaluation and ImproperInputParameters are good
      // termination states.
      if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters ||
          status == Eigen::LevenbergMarquardtSpace::TooManyFunctionEvaluation)
      {
          throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-GaussFitter", "Could not fit the Gaussian to the data: Error " + String(status));
      }
      
      x_init(2) = fabs(x_init(2)); // sigma can be negative, but |sigma| would actually be the correct solution

#ifdef GAUSS_FITTER_VERBOSE
      std::stringstream formula;
      formula << "f(x)=" << result.A << " * exp(-(x - " << result.x0 << ") ** 2 / 2 / (" << result.sigma << ") ** 2)";
      std::cout << formular.str() << std::endl;
#endif
      
      return GaussFitResult (x_init(0), x_init(1), x_init(2));
    }
Example #4
0
int		main(int ac, char **av)
{
  t_var		var;
  int		cren;
  int		fd;

  if (ac == 3 && comp(av[1], "-c") && check_bmp(av[2]))
    event(check_file(av[2]));
  else
    {
      x_init(&var);
      if ((fd = reduc_main(ac, av)) < 0)
	return (0);
      var.spot_list = NULL;
      if (fd == -1)
      	return (0);
      else
      	var.list = lex(&var.spot_list, fd, &var.item);
      parse(var.list, var.item);
      ((ac == 4 && comp(av[1], "-a")
	&& ((cren = atoi(av[2])) == 4 || cren == 9 || cren == 16)) ?
       (var.cren_val = cren) : (var.cren_val = 1));
      gere_thread(&var);
      event(&var);
    }
  return (0);
}
Example #5
0
int try_x_init(int mode)
{
    if(!x_init()) {
        cio_api.mode=CIOLIB_MODE_X;
        cio_api.mouse=1;
        cio_api.puttext=bitmap_puttext;
        cio_api.gettext=bitmap_gettext;
        cio_api.gotoxy=bitmap_gotoxy;
        cio_api.setcursortype=bitmap_setcursortype;
        cio_api.setfont=bitmap_setfont;
        cio_api.getfont=bitmap_getfont;
        cio_api.loadfont=bitmap_loadfont;
        cio_api.beep=x_beep;
        cio_api.movetext=bitmap_movetext;
        cio_api.clreol=bitmap_clreol;
        cio_api.clrscr=bitmap_clrscr;
        cio_api.getcustomcursor=bitmap_getcustomcursor;
        cio_api.setcustomcursor=bitmap_setcustomcursor;
        cio_api.getvideoflags=bitmap_getvideoflags;
        cio_api.setvideoflags=bitmap_setvideoflags;

        cio_api.kbhit=x_kbhit;
        cio_api.getch=x_getch;
        cio_api.textmode=x_textmode;
        cio_api.setname=x_setname;
        cio_api.settitle=x_settitle;
        cio_api.copytext=x_copytext;
        cio_api.getcliptext=x_getcliptext;
        cio_api.get_window_info=x_get_window_info;
        return(1);
    }
    return(0);
}
  void GaussTraceFitter::fit(FeatureFinderAlgorithmPickedHelperStructs::MassTraces& traces)
  {
    LOG_DEBUG << "Traces length: " << traces.size() << "\n";
    setInitialParameters_(traces);

    Eigen::VectorXd x_init(NUM_PARAMS_);
    x_init(0) = height_;
    x_init(1) = x0_;
    x_init(2) = sigma_;

    TraceFitter::ModelData data;
    data.traces_ptr = &traces;
    data.weighted = this->weighted_;
    GaussTraceFunctor functor(NUM_PARAMS_, &data);

    TraceFitter::optimize_(x_init, functor);
  }
  void EGHTraceFitter::fit(FeatureFinderAlgorithmPickedHelperStructs::MassTraces& traces)
  {
    setInitialParameters_(traces);

    Eigen::VectorXd x_init(NUM_PARAMS_);
    x_init(0) = height_;
    x_init(1) = apex_rt_;
    x_init(2) = sigma_;
    x_init(3) = tau_;

    TraceFitter::ModelData data;
    data.traces_ptr = &traces;
    data.weighted = this->weighted_;
    EGHTraceFunctor functor(NUM_PARAMS_, &data);

    TraceFitter::optimize_(x_init, functor);
  }
    GumbelDistributionFitter::GumbelDistributionFitResult GumbelDistributionFitter::fit(vector<DPosition<2> > & input)
    {

      Eigen::VectorXd x_init (2);
      x_init(0) = init_param_.a;
      x_init(1) = init_param_.b;
      GumbelDistributionFunctor functor (2, &input);
      Eigen::LevenbergMarquardt<GumbelDistributionFunctor> lmSolver (functor);
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      //the states are poorly documented. after checking the source, we believe that
      //all states except NotStarted, Running and ImproperInputParameters are good
      //termination states.
      if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-GumbelDistributionFitter", "Could not fit the gumbel distribution to the data");
      }

#ifdef GUMBEL_DISTRIBUTION_FITTER_VERBOSE
      // build a formula with the fitted parameters for gnuplot
      stringstream formula;
      formula << "f(x)=" << "(1/" << x_init(1) << ") * " << "exp(( " << x_init(0) << "- x)/" << x_init(1) << ") * exp(-exp((" << x_init(0) << " - x)/" << x_init(1) << "))";
      cout << formula.str() << endl;
#endif

      return GumbelDistributionFitResult (x_init(0), x_init(1));
    }
Example #9
0
File: terma.c Project: halhen/terma
void
init()
{
    setlocale(LC_CTYPE, "");
    util_init();
    x_init();
    term_init(&callbacks);
    shell_fd = sh_init();
}
Example #10
0
File: os.c Project: Pachot/mlworks
extern void os_init(void)
{
  word x = check_windows();
  if (x)
    error ("This computer requires a Sun kernel "
	   "patch before this application will run.\n"
           "Please contact the application's vendor "
	   "for details of the patch required.");
  pioc_init();
  unix_init();
  x_init();
}
Example #11
0
File: yaftx.c Project: saitoha/yaft
int main()
{
	int size;
	fd_set fds;
	struct timeval tv;
	u8 buf[BUFSIZE];
	XEvent ev;
	xwindow xw;
	terminal term;

	/* init */
	x_init(&xw);
	term_init(&term, xw.res);
	load_ctrl_func(ctrl_func, CTRL_CHARS);
	load_esc_func(esc_func, ESC_CHARS);
	load_csi_func(csi_func, ESC_CHARS);

	/* fork */
	eforkpty(&term.fd, term.lines, term.cols);

	signal(SIGCHLD, sigchld);
	setvbuf(stdout, NULL, _IONBF, 0);

	/* main loop */
	while (loop_flag) {
		check_fds(&fds, &tv, term.fd);

		while(XPending(xw.dsp)) {
			XNextEvent(xw.dsp, &ev);
			if(XFilterEvent(&ev, xw.win))
				continue;
			if (event_func[ev.type])
				event_func[ev.type](&xw, &term, &ev);
		}

		if (FD_ISSET(term.fd, &fds)) {
			size = read(term.fd, buf, BUFSIZE);
			if (size > 0) {
				if (DEBUG)
					ewrite(STDOUT_FILENO, buf, size);
				parse(&term, buf, size);
				if (size == BUFSIZE)
					continue;
				refresh(&xw, &term);
			}
		}
	}

	term_die(&term);
	x_die(&xw);

	return 0;
}
    GammaDistributionFitter::GammaDistributionFitResult GammaDistributionFitter::fit(const std::vector<DPosition<2> >& input)
    {
      Eigen::VectorXd x_init(2);
      x_init << init_param_.b, init_param_.p;
      GammaFunctor functor(2, &input);
      Eigen::LevenbergMarquardt<GammaFunctor> lmSolver(functor);
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      //the states are poorly documented. after checking the source, we believe that
      //all states except NotStarted, Running and ImproperInputParameters are good
      //termination states.
      if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
      {
        throw Exception::UnableToFit(__FILE__, __LINE__, __PRETTY_FUNCTION__, "UnableToFit-GammaDistributionFitter", "Could not fit the gamma distribution to the data");
      }

#ifdef GAMMA_DISTRIBUTION_FITTER_VERBOSE
      std::stringstream formula;
      formula << "f(x)=" << "(" << x_init(0) << " ** " << x_init(1) << ") / gamma(" << x_init(1) << ") * x ** (" << x_init(1) << " - 1) * exp(- " << x_init(0) << " * x)";
      std::cout << formula.str() << std::endl;
#endif

      return GammaDistributionFitResult(x_init(0), x_init(1));
    }
Example #13
0
int main(void)
{
	serial_open(19200, SERIAL_8N1);
	ADMUX |= (1<<REFS0);
	ADCSRA|=(1<<ADEN)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
	
	pinMode(53, OUTPUT);
	pinMode(13, OUTPUT);
	
	x_init();
	x_new(1, dimmer, 1);
	x_new(2, setDelay, 1);
	x_new(3, blinker, 1);
	
	while (1)
	{
		x_delay(500);
	}
}
Example #14
0
struct x_node *x_host_lookup(const char *name, struct x_node *p, int flags)
{
  size_t hash;
  struct hlist_head *head;
  struct x_node *x;
  struct clus_node *c;

  x = x_lookup_hash(X_HOST, name, &hash, &head);
  if (x != NULL)
    return x;

  if (!(flags & L_CREATE))
    return NULL;

  if (p != NULL)
    goto have_p;

  c = clus_lookup_for_host(name);
  if (c == NULL)
    return NULL;

  if (c->c_idle_job == NULL) {
    errno = ESRCH;
    return NULL;
  }

  p = &c->c_idle_job->j_x;

 have_p:
  x = malloc(sizeof(*x) + strlen(name) + 1);
  if (x == NULL)
    return NULL;

  x_init(x, X_HOST, p, hash, head, name);

  return x;
}
Example #15
0
struct x_node *
x_lookup(int type, const char *name, struct x_node *p, int flags)
{
  struct hash_table *t = &x_types[type].x_hash_table;
  size_t hash = str_hash(name, 64); /* XXX */
  struct hlist_head *head = t->t_table + (hash & t->t_mask);
  struct hlist_node *node;
  struct x_node *x;

  hlist_for_each_entry(x, node, head, x_hash_node)
    if (strcmp(name, x->x_name) == 0)
      return x;

  if (!(flags & L_CREATE))
    return NULL;

  x = malloc(sizeof(*x) + strlen(name) + 1);
  if (x == NULL)
    return NULL;

  x_init(x, type, p, hash, head, name);

  return x;
}
Example #16
0
int
main(int argc, char **argv)
{
	const char	*conf_file = NULL;
	char		*conf_path, *display_name = NULL;
	int		 ch;
	struct passwd	*pw;

	if (!setlocale(LC_CTYPE, "") || !XSupportsLocale())
		warnx("no locale support");
	mbtowc(NULL, NULL, MB_CUR_MAX);

	cwm_argv = argv;
	while ((ch = getopt(argc, argv, "c:d:")) != -1) {
		switch (ch) {
		case 'c':
			conf_file = optarg;
			break;
		case 'd':
			display_name = optarg;
			break;
		default:
			usage();
		}
	}
	argc -= optind;
	argv += optind;

	if (signal(SIGCHLD, sigchld_cb) == SIG_ERR)
		err(1, "signal");

	if ((homedir = getenv("HOME")) == NULL || *homedir == '\0') {
		pw = getpwuid(getuid());
		if (pw != NULL && pw->pw_dir != NULL && *pw->pw_dir != '\0')
			homedir = pw->pw_dir;
		else
			homedir = "/";
	}

	if (conf_file == NULL)
		xasprintf(&conf_path, "%s/%s", homedir, CONFFILE);
	else
		conf_path = xstrdup(conf_file);

	if (access(conf_path, R_OK) != 0) {
		if (conf_file != NULL)
			warn("%s", conf_file);
		free(conf_path);
		conf_path = NULL;
	}

	conf_init(&Conf);
	if (conf_path && (parse_config(conf_path, &Conf) == -1))
		warnx("config file %s has errors, not loading", conf_path);
	free(conf_path);

	x_init(display_name);
	xev_loop();
	x_teardown();

	return (0);
}
Example #17
0
int main( ){

    USING_NAMESPACE_ACADO

    // INTRODUCE THE VARIABLES:
    // -------------------------
    DifferentialState     x,y,z,vx,vy,vz,phi,theta,psi,p,q,r,u1,u2,u3,u4;
    // x, y, z : position
    // vx, vy, vz : linear velocity
    // phi, theta, psi : orientation (Yaw-Pitch-Roll = Euler(3,2,1))
    // p, q, r : angular velocity
    // u1, u2, u3, u4 : velocity of the propellers
    Control               vu1,vu2,vu3,vu4;
    // vu1, vu2, vu3, vu4 : derivative of u1, u2, u3, u4
    DifferentialEquation  f;

    // Quad constants
    const double c = 0.00001;
    const double Cf = 0.00065;
    const double d = 0.250;
    const double Jx = 0.018;
    const double Jy = 0.018;
    const double Jz = 0.026;
    const double Im = 0.0001;
    const double m = 0.9;
    const double g = 9.81;
    const double Cx = 0.1;

    // Minimization Weights
    double coeffX = .00001;
    double coeffU = coeffX*0.0001;//0.000000000000001;
    double coeffX2 = coeffX * 100.;
    double coeffX3 = coeffX * 0.00001;
    double coeffO = -coeffX * 0.1;

    // final position (used in the cost function)
    double xf = 0., yf = 0., zf = 20.;

    //
    double T = 8.; //length (in second) of the trajectory predicted in the MPC
    int nb_nodes = 20.; //number of nodes used in the Optimal Control Problem
    // 20 nodes means that the algorithm will discretized the trajectory equally into 20 pieces
    // If you increase the number of node, the solution will be more precise but calculation will take longer (~nb_nodes^2)
    // In ACADO, the commands are piecewise constant functions, constant between each node.
    double tmpc = 0.2; //time (in second) between two activation of the MPC algorithm

    // DEFINE A OPTIMAL CONTROL PROBLEM
    // -------------------------------
    OCP ocp( 0.0, T, nb_nodes );

    // DEFINE THE COST FUNCTION
    // -------------------------------
    Function h, hf;
    h << x;
    h << y;
    h << z;
    h << vu1;
    h << vu2;
    h << vu3;
    h << vu4;
    h << p;
    h << q;
    h << r;

    hf << x;
    hf << y;
    hf << z;

    DMatrix Q(10,10), Qf(3,3);
    Q(0,0) = coeffX;
    Q(1,1) = coeffX;
    Q(2,2) = coeffX;
    Q(3,3) = coeffU;
    Q(4,4) = coeffU;
    Q(5,5) = coeffU;
    Q(6,6) = coeffU;
    Q(7,7) = coeffX2;
    Q(8,8) = coeffX2;
    Q(9,9) = coeffX2;

    Qf(0,0) = 1.;
    Qf(1,1) = 1.;
    Qf(2,2) = 1.;

    DVector reff(3), ref(10);
    ref(0) = xf;
    ref(1) = yf;
    ref(2) = zf;
    ref(3) = 58.;
    ref(4) = 58.;
    ref(5) = 58.;
    ref(6) = 58.;
    ref(7) = 0.;
    ref(8) = 0.;
    ref(9) = 0.;

    reff(0) = xf;
    reff(1) = yf;
    reff(2) = zf;


    // The cost function is define as : integrale from 0 to T { transpose(h(x,u,t)-ref)*Q*(h(x,u,t)-ref) }   +    transpose(hf(x,u,T))*Qf*hf(x,u,T)
    //                                  ==     integrale cost (also called running cost or Lagrange Term)    +        final cost (Mayer Term)
    ocp.minimizeLSQ ( Q, h, ref);
//    ocp.minimizeLSQEndTerm(Qf, hf, reff);

    // When doing MPC, you need terms in the cost function to stabilised the system => p, q, r and vu1, vu2, vu3, vu4. You can check that if you reduce their weights "coeffX2" or "coeffU" too low, the optimization will crashed.

    // DEFINE THE DYNAMIC EQUATION OF THE SYSTEM:
    // ----------------------------------
    f << dot(x) == vx;
    f << dot(y) == vy;
    f << dot(z) == vz;
    f << dot(vx) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(theta)/m;
    f << dot(vy) == -Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*sin(psi)*cos(theta)/m;
    f << dot(vz) == Cf*(u1*u1+u2*u2+u3*u3+u4*u4)*cos(psi)*cos(theta)/m - g;
    f << dot(phi) == -cos(phi)*tan(theta)*p+sin(phi)*tan(theta)*q+r;
    f << dot(theta) == sin(phi)*p+cos(phi)*q;
    f << dot(psi) == cos(phi)/cos(theta)*p-sin(phi)/cos(theta)*q;
    f << dot(p) == (d*Cf*(u1*u1-u2*u2)+(Jy-Jz)*q*r)/Jx;
    f << dot(q) == (d*Cf*(u4*u4-u3*u3)+(Jz-Jx)*p*r)/Jy;
    f << dot(r) == (c*(u1*u1+u2*u2-u3*u3-u4*u4)+(Jx-Jy)*p*q)/Jz;
    f << dot(u1) == vu1;
    f << dot(u2) == vu2;
    f << dot(u3) == vu3;
    f << dot(u4) == vu4;

    // ---------------------------- DEFINE CONTRAINTES --------------------------------- //
    //Dynamic
    ocp.subjectTo( f );

    //State constraints
    //Constraints on the velocity of each propeller
    ocp.subjectTo( 16 <= u1 <= 95 );
    ocp.subjectTo( 16 <= u2 <= 95 );
    ocp.subjectTo( 16 <= u3 <= 95 );
    ocp.subjectTo( 16 <= u4 <= 95 );

    //Command constraints
    //Constraints on the acceleration of each propeller
    ocp.subjectTo( -100 <= vu1 <= 100 );
    ocp.subjectTo( -100 <= vu2 <= 100 );
    ocp.subjectTo( -100 <= vu3 <= 100 );
    ocp.subjectTo( -100 <= vu4 <= 100 );

#if AVOID_SINGULARITIES == 1
    //Constraint to avoid singularity
    // In this example I used Yaw-Pitch-Roll convention to describe orientation of the quadrotor
    // when using Euler Angles representation, you always have a singularity, and we need to
    // avoid it otherwise the algorithm will crashed.
    ocp.subjectTo( -1. <= theta <= 1.);
#endif


    //Example of Eliptic obstacle constraints (here, cylinders with eliptic basis)
    ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-5)*(z-5)) );
    ocp.subjectTo( 16 <= ((x-3)*(x-3)+2*(z-9)*(z-9)) );
    ocp.subjectTo( 16 <= ((x+3)*(x+3)+2*(z-15)*(z-15)) );


    // SETTING UP THE (SIMULATED) PROCESS:
    // -----------------------------------
    OutputFcn identity;
    // The next line define the equation used to simulate the system :
    // here "f" is used (=the same as the one used for the ocp) so the trajectory simulated
    // from the commands will be exactly the same as the one calculated by the MPC.
    // If for instance you want to test robusness, you can define an other dynamic equation "f2"
    // with changed parameters and put it here.
    DynamicSystem dynamicSystem( f,identity );
    Process process( dynamicSystem,INT_RK45 );

    // SETTING UP THE MPC CONTROLLER:
    // ------------------------------
    RealTimeAlgorithm alg( ocp, tmpc );

    //Usually, you do only one step of the optimisation algorithm (~Gauss-Newton here)
    //at each activation of the MPC, that way the delay between getting the state and
    //sending a command is as quick as possible.
    alg.set( MAX_NUM_ITERATIONS, 1 );

    StaticReferenceTrajectory zeroReference;
    Controller controller( alg,zeroReference );


    // SET AN INITIAL GUESS FOR THE FIRST MPC LOOP (NEXT LOOPS WILL USE AS INITIAL GUESS THE SOLUTION FOUND AT THE PREVIOUS MPC LOOP)
    Grid timeGrid(0.0,T,nb_nodes+1);
    VariablesGrid x_init(16, timeGrid);
    // init with static
    for (int i = 0 ; i<nb_nodes+1 ; i++ ) {
            x_init(i,0) = 0.;
            x_init(i,1) = 0.;
            x_init(i,2) = 0.;
            x_init(i,3) = 0.;
            x_init(i,4) = 0.;
            x_init(i,5) = 0.;
            x_init(i,6) = 0.;
            x_init(i,7) = 0.;
            x_init(i,8) = 0.;
            x_init(i,9) = 0.;
            x_init(i,10) = 0.;
            x_init(i,11) = 0.;
            x_init(i,12) = 58.; //58. is the propeller rotation speed so the total thrust balance the weight of the quad
            x_init(i,13) = 58.;
            x_init(i,14) = 58.;
            x_init(i,15) = 58.;
        }
    alg.initializeDifferentialStates(x_init);

    // SET OPTION AND PLOTS WINDOW
    // ---------------------------
    // Linesearch is an algorithm which will try several points along the descent direction to choose a better step length.
    // It looks like activating this option produice more stable trajectories.198
    alg.set( GLOBALIZATION_STRATEGY, GS_LINESEARCH );

    alg.set(INTEGRATOR_TYPE, INT_RK45);

    // You can uncomment those lines to see how the predicted trajectory involve along time
    // (but be carefull because you will have 1 ploting window per MPC loop)
//    GnuplotWindow window1(PLOT_AT_EACH_ITERATION);
//    window1.addSubplot( z,"DifferentialState z" );
//    window1.addSubplot( x,"DifferentialState x" );
//    window1.addSubplot( theta,"DifferentialState theta" );
//    window1.addSubplot( 16./((x+3)*(x+3)+4*(z-5)*(z-5)),"Dist obs1" );
//    window1.addSubplot( 16./((x-3)*(x-3)+4*(z-9)*(z-9)),"Dist obs2" );
//    window1.addSubplot( 16./((x+2)*(x+2)+4*(z-15)*(z-15)),"Dist obs3" );
//    alg<<window1;


    // SETTING UP THE SIMULATION ENVIRONMENT,  RUN THE EXAMPLE...
    // ----------------------------------------------------------
    // The first argument is the starting time, the second the end time.
    SimulationEnvironment sim( 0.0,10.,process,controller );

    //Setting the state of the sytem at the beginning of the simulation.
    DVector x0(16);
    x0.setZero();
    x0(0) = 0.;
    x0(12) = 58.;
    x0(13) = 58.;
    x0(14) = 58.;
    x0(15) = 58.;

    t = clock();
    if (sim.init( x0 ) != SUCCESSFUL_RETURN)
        exit( EXIT_FAILURE );
    if (sim.run( ) != SUCCESSFUL_RETURN)
        exit( EXIT_FAILURE );
    t = clock() - t;
    std::cout << "total time : " << (((float)t)/CLOCKS_PER_SEC)<<std::endl;

    // ...SAVE THE RESULTS IN FILES
    // ----------------------------------------------------------

    std::ofstream file;
    file.open("/tmp/log_state.txt",std::ios::out);
    std::ofstream file2;
    file2.open("/tmp/log_control.txt",std::ios::out);

    VariablesGrid sampledProcessOutput;
    sim.getSampledProcessOutput( sampledProcessOutput );
    sampledProcessOutput.print(file);

    VariablesGrid feedbackControl;
    sim.getFeedbackControl( feedbackControl );
    feedbackControl.print(file2);


    // ...AND PLOT THE RESULTS
    // ----------------------------------------------------------

    GnuplotWindow window;
    window.addSubplot( sampledProcessOutput(0), "x " );
    window.addSubplot( sampledProcessOutput(1), "y " );
    window.addSubplot( sampledProcessOutput(2), "z " );
    window.addSubplot( sampledProcessOutput(6),"phi" );
    window.addSubplot( sampledProcessOutput(7),"theta" );
    window.addSubplot( sampledProcessOutput(8),"psi" );
    window.plot( );

    graphics::corbaServer::ClientCpp client = graphics::corbaServer::ClientCpp();
    client.createWindow("window");

    return 0;
}
Example #18
0
static int
main_init(int argc, const char *argv[], Source **sp, struct block **lp)
{
	int argi, i;
	Source *s = NULL;
	struct block *l;
	unsigned char restricted_shell, errexit, utf_flag;
	char *cp;
	const char *ccp, **wp;
	struct tbl *vp;
	struct stat s_stdin;
#if !defined(_PATH_DEFPATH) && defined(_CS_PATH)
	ssize_t k;
#endif

#if defined(MKSH_EBCDIC) || defined(MKSH_FAUX_EBCDIC)
	ebcdic_init();
#endif
	set_ifs(TC_IFSWS);

#ifdef __OS2__
	os2_init(&argc, &argv);
#endif

	/* do things like getpgrp() et al. */
	chvt_reinit();

	/* make sure argv[] is sane, for weird OSes */
	if (!*argv) {
		argv = empty_argv;
		argc = 1;
	}
	kshname = argv[0];

	/* initialise permanent Area */
	ainit(&aperm);
	/* max. name length: -2147483648 = 11 (+ NUL) */
	vtemp = alloc(offsetof(struct tbl, name[0]) + 12, APERM);

	/* set up base environment */
	env.type = E_NONE;
	ainit(&env.area);
	/* set up global l->vars and l->funs */
	newblock();

	/* Do this first so output routines (eg, errorf, shellf) can work */
	initio();

	/* determine the basename (without '-' or path) of the executable */
	ccp = kshname;
	goto begin_parsing_kshname;
	while ((i = ccp[argi++])) {
		if (mksh_cdirsep(i)) {
			ccp += argi;
 begin_parsing_kshname:
			argi = 0;
			if (*ccp == '-')
				++ccp;
		}
	}
	if (!*ccp)
		ccp = empty_argv[0];

	/*
	 * Turn on nohup by default. (AT&T ksh does not have a nohup
	 * option - it always sends the hup).
	 */
	Flag(FNOHUP) = 1;

	/*
	 * Turn on brace expansion by default. AT&T kshs that have
	 * alternation always have it on.
	 */
	Flag(FBRACEEXPAND) = 1;

	/*
	 * Turn on "set -x" inheritance by default.
	 */
	Flag(FXTRACEREC) = 1;

	/* define built-in commands and see if we were called as one */
	ktinit(APERM, &builtins,
	    /* currently up to 54 builtins: 75% of 128 = 2^7 */
	    7);
	for (i = 0; mkshbuiltins[i].name != NULL; i++)
		if (!strcmp(ccp, builtin(mkshbuiltins[i].name,
		    mkshbuiltins[i].func)))
			Flag(FAS_BUILTIN) = 1;

	if (!Flag(FAS_BUILTIN)) {
		/* check for -T option early */
		argi = parse_args(argv, OF_FIRSTTIME, NULL);
		if (argi < 0)
			return (1);

#if defined(MKSH_BINSHPOSIX) || defined(MKSH_BINSHREDUCED)
		/* are we called as -sh or /bin/sh or so? */
		if (!strcmp(ccp, "sh" MKSH_EXE_EXT)) {
			/* either also turns off braceexpand */
#ifdef MKSH_BINSHPOSIX
			/* enable better POSIX conformance */
			change_flag(FPOSIX, OF_FIRSTTIME, true);
#endif
#ifdef MKSH_BINSHREDUCED
			/* enable kludge/compat mode */
			change_flag(FSH, OF_FIRSTTIME, true);
#endif
		}
#endif
	}

	initvar();

	inittraps();

	coproc_init();

	/* set up variable and command dictionaries */
	ktinit(APERM, &taliases, 0);
	ktinit(APERM, &aliases, 0);
#ifndef MKSH_NOPWNAM
	ktinit(APERM, &homedirs, 0);
#endif

	/* define shell keywords */
	initkeywords();

	init_histvec();

	/* initialise tty size before importing environment */
	change_winsz();

#ifdef _PATH_DEFPATH
	def_path = _PATH_DEFPATH;
#else
#ifdef _CS_PATH
	if ((k = confstr(_CS_PATH, NULL, 0)) > 0 &&
	    confstr(_CS_PATH, cp = alloc(k + 1, APERM), k + 1) == k + 1)
		def_path = cp;
	else
#endif
		/*
		 * this is uniform across all OSes unless it
		 * breaks somewhere hard; don't try to optimise,
		 * e.g. add stuff for Interix or remove /usr
		 * for HURD, because e.g. Debian GNU/HURD is
		 * "keeping a regular /usr"; this is supposed
		 * to be a sane 'basic' default PATH
		 */
		def_path = MKSH_UNIXROOT "/bin" MKSH_PATHSEPS
		    MKSH_UNIXROOT "/usr/bin" MKSH_PATHSEPS
		    MKSH_UNIXROOT "/sbin" MKSH_PATHSEPS
		    MKSH_UNIXROOT "/usr/sbin";
#endif

	/*
	 * Set PATH to def_path (will set the path global variable).
	 * (import of environment below will probably change this setting).
	 */
	vp = global(TPATH);
	/* setstr can't fail here */
	setstr(vp, def_path, KSH_RETURN_ERROR);

#ifndef MKSH_NO_CMDLINE_EDITING
	/*
	 * Set edit mode to emacs by default, may be overridden
	 * by the environment or the user. Also, we want tab completion
	 * on in vi by default.
	 */
	change_flag(FEMACS, OF_SPECIAL, true);
#if !MKSH_S_NOVI
	Flag(FVITABCOMPLETE) = 1;
#endif
#endif

	/* import environment */
	init_environ();

	/* override default PATH regardless of environment */
#ifdef MKSH_DEFPATH_OVERRIDE
	vp = global(TPATH);
	setstr(vp, MKSH_DEFPATH_OVERRIDE, KSH_RETURN_ERROR);
#endif

	/* for security */
	typeset(TinitIFS, 0, 0, 0, 0);

	/* assign default shell variable values */
	typeset("PATHSEP=" MKSH_PATHSEPS, 0, 0, 0, 0);
	substitute(initsubs, 0);

	/* Figure out the current working directory and set $PWD */
	vp = global(TPWD);
	cp = str_val(vp);
	/* Try to use existing $PWD if it is valid */
	set_current_wd((mksh_abspath(cp) && test_eval(NULL, TO_FILEQ, cp,
	    Tdot, true)) ? cp : NULL);
	if (current_wd[0])
		simplify_path(current_wd);
	/* Only set pwd if we know where we are or if it had a bogus value */
	if (current_wd[0] || *cp)
		/* setstr can't fail here */
		setstr(vp, current_wd, KSH_RETURN_ERROR);

	for (wp = initcoms; *wp != NULL; wp++) {
		c_builtin(wp);
		while (*wp != NULL)
			wp++;
	}
	setint_n(global("OPTIND"), 1, 10);

	kshuid = getuid();
	kshgid = getgid();
	kshegid = getegid();

	safe_prompt = ksheuid ? "$ " : "# ";
	vp = global("PS1");
	/* Set PS1 if unset or we are root and prompt doesn't contain a # */
	if (!(vp->flag & ISSET) ||
	    (!ksheuid && !strchr(str_val(vp), '#')))
		/* setstr can't fail here */
		setstr(vp, safe_prompt, KSH_RETURN_ERROR);
	setint_n((vp = global("BASHPID")), 0, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("PGRP")), (mksh_uari_t)kshpgrp, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("PPID")), (mksh_uari_t)kshppid, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("USER_ID")), (mksh_uari_t)ksheuid, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("KSHUID")), (mksh_uari_t)kshuid, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("KSHEGID")), (mksh_uari_t)kshegid, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("KSHGID")), (mksh_uari_t)kshgid, 10);
	vp->flag |= INT_U;
	setint_n((vp = global("RANDOM")), rndsetup(), 10);
	vp->flag |= INT_U;
	setint_n((vp_pipest = global("PIPESTATUS")), 0, 10);

	/* Set this before parsing arguments */
	Flag(FPRIVILEGED) = (kshuid != ksheuid || kshgid != kshegid) ? 2 : 0;

	/* this to note if monitor is set on command line (see below) */
#ifndef MKSH_UNEMPLOYED
	Flag(FMONITOR) = 127;
#endif
	/* this to note if utf-8 mode is set on command line (see below) */
	UTFMODE = 2;

	if (!Flag(FAS_BUILTIN)) {
		argi = parse_args(argv, OF_CMDLINE, NULL);
		if (argi < 0)
			return (1);
	}

	/* process this later only, default to off (hysterical raisins) */
	utf_flag = UTFMODE;
	UTFMODE = 0;

	if (Flag(FAS_BUILTIN)) {
		/* auto-detect from environment variables, always */
		utf_flag = 3;
	} else if (Flag(FCOMMAND)) {
		s = pushs(SSTRINGCMDLINE, ATEMP);
		if (!(s->start = s->str = argv[argi++]))
			errorf(Tf_optfoo, "", "", 'c', Treq_arg);
		while (*s->str) {
			if (ctype(*s->str, C_QUOTE))
				break;
			s->str++;
		}
		if (!*s->str)
			s->flags |= SF_MAYEXEC;
		s->str = s->start;
#ifdef MKSH_MIDNIGHTBSD01ASH_COMPAT
		/* compatibility to MidnightBSD 0.1 /bin/sh (kludge) */
		if (Flag(FSH) && argv[argi] && !strcmp(argv[argi], "--"))
			++argi;
#endif
		if (argv[argi])
			kshname = argv[argi++];
	} else if (argi < argc && !Flag(FSTDIN)) {
		s = pushs(SFILE, ATEMP);
#ifdef __OS2__
		/*
		 * A bug in OS/2 extproc (like shebang) handling makes
		 * it not pass the full pathname of a script, so we need
		 * to search for it. This changes the behaviour of a
		 * simple "mksh foo", but can't be helped.
		 */
		s->file = argv[argi++];
		if (search_access(s->file, X_OK) != 0)
			s->file = search_path(s->file, path, X_OK, NULL);
		if (!s->file || !*s->file)
			s->file = argv[argi - 1];
#else
		s->file = argv[argi++];
#endif
		s->u.shf = shf_open(s->file, O_RDONLY, 0,
		    SHF_MAPHI | SHF_CLEXEC);
		if (s->u.shf == NULL) {
			shl_stdout_ok = false;
			warningf(true, Tf_sD_s, s->file, cstrerror(errno));
			/* mandated by SUSv4 */
			exstat = 127;
			unwind(LERROR);
		}
		kshname = s->file;
	} else {
		Flag(FSTDIN) = 1;
		s = pushs(SSTDIN, ATEMP);
		s->file = "<stdin>";
		s->u.shf = shf_fdopen(0, SHF_RD | can_seek(0),
		    NULL);
		if (isatty(0) && isatty(2)) {
			Flag(FTALKING) = Flag(FTALKING_I) = 1;
			/* The following only if isatty(0) */
			s->flags |= SF_TTY;
			s->u.shf->flags |= SHF_INTERRUPT;
			s->file = NULL;
		}
	}

	/* this bizarreness is mandated by POSIX */
	if (fstat(0, &s_stdin) >= 0 && S_ISCHR(s_stdin.st_mode) &&
	    Flag(FTALKING))
		reset_nonblock(0);

	/* initialise job control */
	j_init();
	/* do this after j_init() which calls tty_init_state() */
	if (Flag(FTALKING)) {
		if (utf_flag == 2) {
#ifndef MKSH_ASSUME_UTF8
			/* auto-detect from locale or environment */
			utf_flag = 4;
#else /* this may not be an #elif */
#if MKSH_ASSUME_UTF8
			utf_flag = 1;
#else
			/* always disable UTF-8 (for interactive) */
			utf_flag = 0;
#endif
#endif
		}
#ifndef MKSH_NO_CMDLINE_EDITING
		x_init();
#endif
	}

#ifdef SIGWINCH
	sigtraps[SIGWINCH].flags |= TF_SHELL_USES;
	setsig(&sigtraps[SIGWINCH], x_sigwinch,
	    SS_RESTORE_ORIG|SS_FORCE|SS_SHTRAP);
#endif

	l = e->loc;
	if (Flag(FAS_BUILTIN)) {
		l->argc = argc;
		l->argv = argv;
		l->argv[0] = ccp;
	} else {
		l->argc = argc - argi;
		/*
		 * allocate a new array because otherwise, when we modify
		 * it in-place, ps(1) output changes; the meaning of argc
		 * here is slightly different as it excludes kshname, and
		 * we add a trailing NULL sentinel as well
		 */
		l->argv = alloc2(l->argc + 2, sizeof(void *), APERM);
		l->argv[0] = kshname;
		memcpy(&l->argv[1], &argv[argi], l->argc * sizeof(void *));
		l->argv[l->argc + 1] = NULL;
		getopts_reset(1);
	}

	/* divine the initial state of the utf8-mode Flag */
	ccp = null;
	switch (utf_flag) {

	/* auto-detect from locale or environment */
	case 4:
#if HAVE_SETLOCALE_CTYPE
		ccp = setlocale(LC_CTYPE, "");
#if HAVE_LANGINFO_CODESET
		if (!isuc(ccp))
			ccp = nl_langinfo(CODESET);
#endif
		if (!isuc(ccp))
			ccp = null;
#endif
		/* FALLTHROUGH */

	/* auto-detect from environment */
	case 3:
		/* these were imported from environ earlier */
		if (ccp == null)
			ccp = str_val(global("LC_ALL"));
		if (ccp == null)
			ccp = str_val(global("LC_CTYPE"));
		if (ccp == null)
			ccp = str_val(global("LANG"));
		UTFMODE = isuc(ccp);
		break;

	/* not set on command line, not FTALKING */
	case 2:
	/* unknown values */
	default:
		utf_flag = 0;
		/* FALLTHROUGH */

	/* known values */
	case 1:
	case 0:
		UTFMODE = utf_flag;
		break;
	}

	/* Disable during .profile/ENV reading */
	restricted_shell = Flag(FRESTRICTED);
	Flag(FRESTRICTED) = 0;
	errexit = Flag(FERREXIT);
	Flag(FERREXIT) = 0;

	/*
	 * Do this before profile/$ENV so that if it causes problems in them,
	 * user will know why things broke.
	 */
	if (!current_wd[0] && Flag(FTALKING))
		warningf(false, "can't determine current directory");

	if (Flag(FLOGIN))
		include(MKSH_SYSTEM_PROFILE, 0, NULL, true);
	if (!Flag(FPRIVILEGED)) {
		if (Flag(FLOGIN))
			include(substitute("$HOME/.profile", 0), 0, NULL, true);
		if (Flag(FTALKING)) {
			cp = substitute("${ENV:-" MKSHRC_PATH "}", DOTILDE);
			if (cp[0] != '\0')
				include(cp, 0, NULL, true);
		}
	} else {
		include(MKSH_SUID_PROFILE, 0, NULL, true);
		/* turn off -p if not set explicitly */
		if (Flag(FPRIVILEGED) != 1)
			change_flag(FPRIVILEGED, OF_INTERNAL, false);
	}

	if (restricted_shell) {
		c_builtin(restr_com);
		/* After typeset command... */
		Flag(FRESTRICTED) = 1;
	}
	Flag(FERREXIT) = errexit;

	if (Flag(FTALKING) && s)
		hist_init(s);
	else
		/* set after ENV */
		Flag(FTRACKALL) = 1;

	alarm_init();

	*sp = s;
	*lp = l;
	return (0);
}
//Resolution of min 1/2 x^t A x + b^t x with x in [-4, +infinity]
//and x_1 in [-1.0, 0.7]
//    x_2 in [-1.3, 1.4]
//    x_3 in [-2.0, 2.1]
int main()
{
    //Definition of the quadratic function
    roboptim::Function::matrix_t A( 3,3 );
    roboptim::Function::vector_t b( 3 );

    A << 1,7,4,7,3,6,4,6,2;
    b << 1,3,4;

    roboptim::NumericQuadraticFunction f( A, b );

    //Testing f([1,1,1])
    roboptim::Function::vector_t x( 3 );
    x << 1,1,1;
    std::cout << "f(x) = " << f( x ) << std::endl;

    //Definition the constraint x_min < x < x_max
    roboptim::Function::vector_t offset( 3 );
    boost::shared_ptr< roboptim::IdentityFunction > c1(
            new roboptim::IdentityFunction( offset ) );

    //Definition of another constraint 0 < Ax +b < +infinity
    roboptim::Function::matrix_t Ac2( 3,3 );
    roboptim::Function::vector_t bc2( 3 );
    Ac2 << 2,3,1,7,1,0,3,5,2;
    bc2 << 4,3,4;
    boost::shared_ptr< roboptim::NumericLinearFunction > c2(
            new roboptim::NumericLinearFunction(Ac2, bc2) );
    
    //Test c([1,1,1])
    roboptim::IdentityFunction c1_el = *c1.get();
    std::cout << "c1(x) = [" << c1_el( x ) << "]" << std::endl;
    
    //Creation of the problem
    solver_t::problem_t pb ( f );
    std::cout << "problem input size : " << pb.function().inputSize() << std::endl;
    std::cout << "problem output size : " << pb.function().outputSize() << std::endl;

    //Set bounds for x in f(x)
    roboptim::Function::intervals_t function_arg_bounds;
    for( unsigned int i=0; i< pb.function().inputSize(); ++i ){
        roboptim::Function::interval_t x_bound = roboptim::Function::makeInterval( -4.0, roboptim::Function::infinity() );
        function_arg_bounds.push_back( x_bound );
    }

    pb.argumentBounds() = function_arg_bounds;

    //Set bounds for constraints: x_min and x_max
    roboptim::Function::intervals_t constraint_bounds;
    roboptim::Function::interval_t c1_0 = roboptim::Function::makeInterval( -1.0, 0.7 );
    roboptim::Function::interval_t c1_1 = roboptim::Function::makeInterval( -1.3, 1.4 );
    roboptim::Function::interval_t c1_2 = roboptim::Function::makeInterval( -2.0, 2.1 );
    constraint_bounds.push_back( c1_0 );
    constraint_bounds.push_back( c1_1 );
    constraint_bounds.push_back( c1_2 );

    //Set bounds for c2
    roboptim::Function::intervals_t c2_bounds;
    for( unsigned int i=0; i< pb.function().inputSize(); ++i ){
        roboptim::Function::interval_t c2_interval = roboptim::Function::makeLowerInterval( 0 );
        c2_bounds.push_back(c2_interval);
    }

    //Set scales for the problem
    solver_t::problem_t::scales_t scales(pb.function().inputSize(), 1.0);
    
    //Add constraint
    pb.addConstraint(
            boost::static_pointer_cast<
      roboptim::GenericLinearFunction<roboptim::EigenMatrixDense>  >
            (c1), constraint_bounds, scales );

    
    pb.addConstraint(
            boost::static_pointer_cast<
      roboptim::GenericLinearFunction<roboptim::EigenMatrixDense> >
            (c2), c2_bounds, scales);
      
    //Set initial guess
    roboptim::NumericQuadraticFunction::argument_t x_init( 3 );
    x_init << 0.4,0.0,0.2;

    pb.startingPoint() = x_init;

    //Set path of the roboptim-core-plugin-ipopt.so so that libltdl finds the lib.
    //The path is detected in the CMakeLists.txt which will substitute the variable PLUGIN_PATH during the compilation
    //Those two lines can be omitted if the environment variable LD_LIBRARY_PATH contains the path of the plugin
    lt_dlinit();
    lt_dlsetsearchpath (PLUGIN_PATH);

    roboptim::SolverFactory<solver_t> factory ("ipopt", pb);
    solver_t& solver = factory ();

    solver_t::result_t res = solver.minimum ();

    std::cout << solver << std::endl;

    // Check if the minimization has succeed.
    if (res.which () != solver_t::SOLVER_VALUE)
    {
        std::cout << "A solution should have been found. Failing..."
            << std::endl
            << boost::get<roboptim::SolverError> (res).what ()
            << std::endl;
        return 0;
    } 

    // Get the result.
    roboptim::Result& result = boost::get<roboptim::Result> (res);
    std::cout << "A solution has been found: " << std::endl
              << result << std::endl;
    return 1;
}
Example #20
0
  void OptimizePick::optimize(std::vector<PeakShape> & peaks, Data & data)
  {
    if (peaks.empty())
      return;

    size_t global_peak_number = 0;
    data.peaks.assign(peaks.begin(), peaks.end());

    size_t num_dimensions = 4 * data.peaks.size();
    Eigen::VectorXd x_init (num_dimensions);
    x_init.setZero();
    // We have to initialize the parameters for the optimization
    for (size_t i = 0; i < data.peaks.size(); i++)
    {
      PeakShape current_peak = data.peaks[i];
      double h  = current_peak.height;
      double wl = current_peak.left_width;
      double wr = current_peak.right_width;
      double p  = current_peak.mz_position;
      if (boost::math::isnan(wl))
      {
        data.peaks[i].left_width = 1;
        wl = 1.;
      }
      if (boost::math::isnan(wr))
      {
        data.peaks[i].right_width = 1;
        wr = 1.;
      }
      x_init(4 * i) = h;
      x_init(4 * i + 1) = wl;
      x_init(4 * i + 2) = wr;
      x_init(4 * i + 3) = p;
    }

    data.penalties = penalties_;

    unsigned num_data_points = std::max(data.positions.size() + 1, num_dimensions);
    OptPeakFunctor functor (num_dimensions, num_data_points, &data);
    Eigen::LevenbergMarquardt<OptPeakFunctor> lmSolver (functor);
    lmSolver.parameters.maxfev = max_iteration_;
    Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);
    //the states are poorly documented. after checking the source, we believe that
    //all states except NotStarted, Running and ImproperInputParameters are good
    //termination states.
    if (status <= Eigen::LevenbergMarquardtSpace::ImproperInputParameters)
    {
        throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-OptimizePeak:", "Could not fit the data: Error " + String(status));
    }

    // iterate over all peaks and store the optimized values in peaks
    for (size_t current_peak = 0; current_peak < data.peaks.size(); current_peak++)
    {
      // Store the current parameters for this peak
      peaks[global_peak_number + current_peak].height =  x_init(4 * current_peak);
      peaks[global_peak_number + current_peak].mz_position = x_init(4 * current_peak + 3);
      peaks[global_peak_number + current_peak].left_width = x_init(4 * current_peak + 1);
      peaks[global_peak_number + current_peak].right_width = x_init(4 * current_peak + 2);

      // compute the area
      // is it a Lorentz or a Sech - Peak?
      if (peaks[global_peak_number + current_peak].type == PeakShape::LORENTZ_PEAK)
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * sqrt(p.height / 1 - 1);
        double x_right_endpoint = p.mz_position + 1 / p.right_width * sqrt(p.height / 1 - 1);
        double area_left = -p.height / p.left_width * atan(p.left_width * (x_left_endpoint - p.mz_position));
        double area_right = -p.height / p.right_width * atan(p.right_width * (p.mz_position - x_right_endpoint));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Lorentz " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
#endif
      }
      else  //It's a Sech - Peak
      {
        PeakShape p = peaks[global_peak_number + current_peak];
        double x_left_endpoint = p.mz_position - 1 / p.left_width * boost::math::acosh(sqrt(p.height / 0.001));
        double x_right_endpoint = p.mz_position + 1 / p.right_width * boost::math::acosh(sqrt(p.height / 0.001));
        double area_left = p.height / p.left_width * (sinh(p.left_width * (p.mz_position - x_left_endpoint)) / cosh(p.left_width * (p.mz_position - x_left_endpoint)));
        double area_right = -p.height / p.right_width * (sinh(p.right_width * (p.mz_position - x_right_endpoint)) / cosh(p.right_width * (p.mz_position - x_right_endpoint)));
        peaks[global_peak_number + current_peak].area = area_left + area_right;
#ifdef DEBUG_PEAK_PICKING
        std::cout << "Sech " << area_left << " " << area_right
                  << " " << peaks[global_peak_number + current_peak].area << std::endl;
        std::cout << p.mz_position << " " << x_left_endpoint << " " << x_right_endpoint << std::endl;
#endif
      }
    }
    //global_peak_number += data.peaks.size();

  }
Example #21
0
int main(int argc,char *argv[])
{
     int er=1,i;
     signed char *s=NULL;
     char *tmpp;

     int mifiles = 5;
     int nifiles = 0;
     int verbose = 0;
     int oldfile = 0;
     int no_link = 0;

     char **ifiles;
     char *ofile;
     char *efile;
     char *lfile;
     char *ifile;
     
     char old_e[MAXLINE];
     char old_l[MAXLINE];
     char old_o[MAXLINE];

     tim1=time(NULL);
     
     ncmos=0;
     n65816=0;
     cmosfl=1;
     w65816=0;	/* default: 6502 only */

     altppchar = '#' ; /* i.e., NO alternate char */

     if((tmpp = strrchr(argv[0],'/'))) {
	tmpp++;
     } else {
	tmpp = argv[0];
     }
     if( (!strcmp(tmpp,"xa65816"))
	|| (!strcmp(tmpp,"XA65816"))
	|| (!strcmp(tmpp,"xa816"))
	|| (!strcmp(tmpp,"XA816"))
	) {
	w65816 = 1;	/* allow 65816 per default */
     }

     /* default output charset for strings in quotes */
     set_charset("ASCII");

     ifiles = malloc(mifiles*sizeof(char*));

     afile = alloc_file();

     if (argc <= 1) {
          usage(w65816, stderr);
          exit(1);
     }

     if (strstr(argv[1], "--help")) {
          usage(w65816, stdout);
	  exit(0);
     }

     if (strstr(argv[1], "--version")) {
          version(programname, progversion, authors, copyright);
	  exit(0);
     }

     ofile="a.o65";
     efile=NULL;
     lfile=NULL;

     if(pp_init()) {
       logout("fatal: pp: no memory!");
       return 1;
     }
     if(b_init()) {
       logout("fatal: b: no memory!");
       return 1;
     }
     if(l_init()) {
       logout("fatal: l: no memory!");
       return 1;
     }

     i=1;
     while(i<argc) {
	if(argv[i][0]=='-') {
	  switch(argv[i][1]) {
	  case 'p':
		/* intentionally not allowing an argument to follow with a
			space to avoid - being seen as the alternate
			preprocessor char! */
		if (argv[i][2] == '\0') {
			fprintf(stderr, "-p requires a character argument\n");
			exit(1);
		}
		if (argv[i][2] == '#')
			fprintf(stderr,
				"using -p# is evidence of stupidity\n");
		altppchar = argv[i][2];
		if (argv[i][3] != '\0')
			fprintf(stderr,
				"warning: extra characters to -p ignored\n");
		break;
	  case 'M':
		masm = 1;	/* MASM compatibility mode */
		break;
	  case 'O':		/* output charset */
		{
		  char *name = NULL;
		  if (argv[i][2] == 0) { 
		    name = argv[++i]; 
		  } else {
		    name = argv[i]+2;
		  }
		  if (set_charset(name) < 0) {
		    fprintf(stderr, "Output charset name '%s' unknown - ignoring! (check case?)\n", name);
		  }
		}
		break;
	  case 'A':		/* make text segment start so that text relocation
				   is not necessary when _file_ starts at adr */
		romable = 2;
		if(argv[i][2]==0) romaddr = atoi(argv[++i]);
		else romaddr = atoi(argv[i]+2);
		break;
	  case 'G':
		noglob = 1;
		break;
	  case 'L':		/* define global label */
		if(argv[i][2]) lg_set(argv[i]+2);
		break;
	  case 'r':
		crossref = 1;
		break;
	  case 'R':
		relmode = 1;
		break;
	  case 'D':
		s = (signed char*)strstr(argv[i]+2,"=");
		if(s) *s = ' ';
		pp_define(argv[i]+2);
		break;
	  case 'c':
		no_link = 1;
		fmode |= FM_OBJ;
		break;
	  case 'v':
		verbose = 1;
		break;
	  case 'C':
		cmosfl = 0;
		break;
          case 'W':
                w65816 = 0;
                break;
          case 'w':
                w65816 = 1;
                break;
	  case 'B':
		showblk = 1;
		break;
	  case 'x':		/* old filename behaviour */
		oldfile = 1;
		fprintf(stderr, "Warning: -x is now deprecated and may disappear in future versions!\n");
		break;
	  case 'I':
		if(argv[i][2]==0) {
		  reg_include(argv[++i]);
		} else {
		  reg_include(argv[i]+2);
		}
		break;
	  case 'o':
		if(argv[i][2]==0) {
		  ofile=argv[++i];
		} else {
		  ofile=argv[i]+2;
		}
		break;
	  case 'l':
		if(argv[i][2]==0) {
		  lfile=argv[++i];
		} else {
		  lfile=argv[i]+2;
		}
		break;
	  case 'e':
		if(argv[i][2]==0) {
		  efile=argv[++i];
		} else {
		  efile=argv[i]+2;
		}
		break;
	  case 'b':			/* set segment base addresses */
		switch(argv[i][2]) {
		case 't':
			if(argv[i][3]==0) tbase = atoi(argv[++i]);
			else tbase = atoi(argv[i]+3);
			break;
		case 'd':
			if(argv[i][3]==0) dbase = atoi(argv[++i]);
			else dbase = atoi(argv[i]+3);
			break;
		case 'b':
			if(argv[i][3]==0) bbase = atoi(argv[++i]);
			else bbase = atoi(argv[i]+3);
			break;
		case 'z':
			if(argv[i][3]==0) zbase = atoi(argv[++i]);
			else zbase = atoi(argv[i]+3);
			break;
		default:
			fprintf(stderr,"unknown segment type '%c' - ignoring!\n",
								argv[i][2]);
			break;
		}
		break;
	  case 0:
		fprintf(stderr, "Single dash '-' on command line - ignoring!\n");
		break;
	  default:
		fprintf(stderr, "Unknown option '%c' - ignoring!\n",argv[i][1]);
		break;
	  }
	} else {		/* no option -> filename */
	  ifiles[nifiles++] = argv[i];
	  if(nifiles>=mifiles) {
	    mifiles += 5;
	    ifiles=realloc(ifiles, mifiles*sizeof(char*));
	    if(!ifiles) {
		fprintf(stderr, "Oops: couldn't alloc enough mem for filelist table..!\n");
		exit(1);
	    }
	  }
	}
	i++;
     }
     if(!nifiles) {
	fprintf(stderr, "No input files given!\n");
	exit(0);
     }

     if(oldfile) {
	strcpy(old_e, ifiles[0]);
	strcpy(old_o, ifiles[0]);
	strcpy(old_l, ifiles[0]);

	if(setfext(old_e,".err")==0) efile = old_e;
	if(setfext(old_o,".obj")==0) ofile = old_o;
	if(setfext(old_l,".lab")==0) lfile = old_l;
     }

     fplab= lfile ? xfopen(lfile,"w") : NULL;
     fperr= efile ? xfopen(efile,"w") : NULL;
     if(!strcmp(ofile,"-")) {
	ofile=NULL;
        fpout = stdout;
     } else {
        fpout= xfopen(ofile,"wb");
     }
     if(!fpout) {
	fprintf(stderr, "Couldn't open output file!\n");
	exit(1);
     }

     if(verbose) fprintf(stderr, "%s\n",copyright);

     if(1 /*!m_init()*/)
     {
       if(1 /*!b_init()*/)
       {
         if(1 /*!l_init()*/)
         {
           /*if(!pp_init())*/
           {
             if(!x_init())
             {
	       if(fperr) fprintf(fperr,"%s\n",copyright);
	       if(verbose) logout(ctime(&tim1));

	       /* Pass 1 */

               pc[SEG_ABS]= 0;		/* abs addressing */
	       seg_start(fmode, tbase, dbase, bbase, zbase, 0, relmode);

	       if(relmode) {
		 r_mode(RMODE_RELOC);
		 segment = SEG_TEXT;
	       } else {
		 r_mode(RMODE_ABS);
	       }

	       nolink = no_link;

               for (i=0; i<nifiles; i++)
               {
		 ifile = ifiles[i];

                 sprintf(out,"xAss65: Pass 1: %s\n",ifile);
                 if(verbose) logout(out);

                 er=pp_open(ifile);
                    puttmp(0);
                    puttmp(T_FILE);
                    puttmp(0);
                    puttmp(0);
                    puttmps((signed char*)&ifile, sizeof(filep->fname));

                 if(!er) {
                   er=pass1();
                   pp_close();
                 } else {
		   sprintf(out, "Couldn't open source file '%s'!\n", ifile);
		   logout(out);
                 }
               }           

	       if((er=b_depth())) {
		 sprintf(out,"Still %d blocks open at end of file!\n",er);
		 logout(out);
	       }

	       if(tbase & (align-1)) {
		   sprintf(out,"Warning: text segment ($%04x) start address doesn't align to %d!\n", tbase, align);
		   logout(out);
	       }
	       if(dbase & (align-1)) {
		   sprintf(out,"Warning: data segment ($%04x) start address doesn't align to %d!\n", dbase, align);
		   logout(out);
	       }
	       if(bbase & (align-1)) {
		   sprintf(out,"Warning: bss segment ($%04x) start address doesn't align to %d!\n", bbase, align);
		   logout(out);
	       }
	       if(zbase & (align-1)) {
		   sprintf(out,"Warning: zero segment ($%04x) start address doesn't align to %d!\n", zbase, align);
		   logout(out);
	       }
               if (n65816>0)
                   fmode |= 0x8000;
	       switch(align) {
		case 1: break;
		case 2: fmode |= 1; break;
		case 4: fmode |= 2; break;
		case 256: fmode |=3; break;
	       }
	       
	       if((!er) && relmode) 
			h_write(fpout, fmode, tlen, dlen, blen, zlen, 0);


               if(!er)
               {
                    if(verbose) logout("xAss65: Pass 2:\n");

		    seg_pass2();

	            if(!relmode) {
	              r_mode(RMODE_ABS);
	            } else {
	              r_mode(RMODE_RELOC);
		      segment = SEG_TEXT;
	            }
                    er=pass2();
               } 

               if(fplab) printllist(fplab);
               tim2=time(NULL);
	       if(verbose) printstat();

	       if((!er) && relmode) seg_end(fpout);	/* write reloc/label info */
			                              
               if(fperr) fclose(fperr);
               if(fplab) fclose(fplab);
               if(fpout) fclose(fpout);

             } else {
               logout("fatal: x: no memory!\n");
             }
             pp_end();
/*           } else {
             logout("fatal: pp: no memory!");*/
           }
         } else {
          logout("fatal: l: no memory!\n");
         }
       } else {
          logout("fatal: b: no memory!\n");
       }
       /*m_exit();*/
     } else { 
          logout("Not enough memory available!\n");
     }

     if(ner || er)
     {
          fprintf(stderr, "Break after %d error%c\n",ner,ner?'s':0);
	  /*unlink();*/
	  if(ofile) {
	    unlink(ofile);
	  }
     }

     free(ifiles);

     return( (er || ner) ? 1 : 0 );
}
Example #22
0
int main(void)
{
	int error;

#if _MINT_				/* HR 151102 */
	have_ssystem = Ssystem(-1, 0, 0) == 0;		/* HR 151102: use Ssystem where possible */

	mint   = (find_cookie('MiNT') == -1) ? FALSE : TRUE;
	magx   = (find_cookie('MagX') == -1) ? FALSE : TRUE;	/* HR 151102 */
	geneva = (find_cookie('Gnva') == -1) ? FALSE : TRUE;    /* DjV 035 080203 */
	mint  |= magx;			/* Quick & dirty */

	if (mint)
	{
		Psigsetmask(0x7FFFE14EL);
		Pdomain(1);
	}
#endif

	x_init();

	if ((ap_id = appl_init()) < 0)
		return -1;

	if  (_GemParBlk.glob.version >= 0x400)
	{
		shel_write(9, 1, 0, NULL, NULL);
		menu_register(ap_id, "  Tera Desktop");
	}

	if (rsrc_load(RSRCNAME) == 0)
		form_alert(1, msg_resnfnd);
	else
	{
		if ((error = init_xdialog(&vdi_handle, malloc, free,
								  "Tera Desktop", 1, &nfonts)) < 0)
			xform_error(error);
		else
		{
			init_vdi();
			rsc_init();

			if (((max_w / screen_info.fnt_w) < 40) || ((max_h / screen_info.fnt_h) < 25))
				alert_printf(1, MRESTLOW);
			else
			{
				if ((error = alloc_global_memory()) == 0)
				{
					if (exec_deskbat() == FALSE)
					{
						if (load_icons() == FALSE)
						{
							if (init() == FALSE)
							{
								graf_mouse(ARROW, NULL);
								evntloop();

								wd_del_all();
								menu_bar(menu, 0);
								xw_close_desk();
							}

							free_icons();		/* HR 151102 */

							wind_set(0, WF_NEWDESK, NULL, 0);
							dsk_draw();
						}
					}

					Mfree(global_memory);
				}
				else
					xform_error(error);
			}

			if (vq_gdos() != 0)
				vst_unload_fonts(vdi_handle, 0);
			exit_xdialog();
		}

		rsrc_free();
	}

	/* DjV 013 030103 100203 ---vvv--- */
	/*
	 * The following section handles system shutdown and resolution change
	 * If a resolution change is required, shutdown is performed first
	 * If only shutdown s required, system will reset at the end.
	 */ 

	/* appl_exit(); */

	if ( chrez || shutdown ) /* If change resolution or shutdown ... */
	{

		/* Tell all applications which would understand it to end */

		quit = shel_write ( 4, 2, 0, NULL, NULL ); 	/* complete shutdown */
		evnt_timer( 3000, 0 );						/* Wait a bit? */

		/*

		/* 
		 * In Mint, must tell all proceseses to terminate nicely ?
		 * but this is only in this group ? What to do?
		 */

		Pkill(0, SIGTERM); 
		evnt_timer(3000, 0); /* Wait a bit? */
		*/

		/* 
		 * After all applications have hopefully been closed,
		 * change the screen resolution if needed;
		 * else- reset the computer
		 */
		if ( chrez )
			get_set_video(2);

#if 1
		else
	#if _MINT_
		if (!mint)			/* HR 230203: Dont reset under MiNT or MagiC !!!!! */
	#endif
		{
			/* Perform a reset here */

	#if 0		/* with warnings */
			long *m;					/* to memory locations */
			long rv;					/* reset vector */

			Super ( 0L );				/* Supervisor; old stack won't be needed again */
			*(m = 0x420L) = 0L;			/* memctrl  */
			*(m = 0x43aL) = 0L;			/* memval2  */
			*(m = 0x426L) = 0L;			/* resvalid */	
			m = *( m = 0x4f2 );			/* to start of OS */
			rv = *(m + 4);				/* to routine that  handles the reset */
			Supexec(rv);				/* execute it */
	#else			/* HR: without warnings */
			long (*rv)();					/* reset vector */

			Super ( 0L );				/* Supervisor; old stack won't be needed again */
			memctrl = 0L;
			memval2 = 0L;
			resvalid = 0L;
			(long)rv = *((long *)os_start + 4);	/* routine that handles the reset */
			Supexec(rv);				/* execute it */
	#endif
		}
#endif
	}