Example #1
0
 void SubscribeLcmChannels(StateMachineControl *fsm_control) {
     pose_sub_ = lcm_->subscribe(pose_channel_, &StateMachineControl::ProcessImuMsg, fsm_control);
     stereo_sub_ = lcm_->subscribe(stereo_channel_, &StateMachineControl::ProcessStereoMsg, fsm_control);
     rc_traj_sub_ = lcm_->subscribe(rc_trajectory_commands_channel_, &StateMachineControl::ProcessRcTrajectoryMsg, fsm_control);
     go_auto_sub_ = lcm_->subscribe(state_machine_go_autonomous_channel_, &StateMachineControl::ProcessGoAutonomousMsg, fsm_control);
 }
Example #2
0
int main() {

	if(!lcm1.good())
        return 1;


    lcm1.subscribe("vicon_state", &Handler::handleMessage, &handlerObject);

	initialize_Matrices();

	// initial conditions
	mat tmp0;
	tmp0 = {0, 0, 0, 1.2, 0, 0, 1.3, 0, 0, 0, 0, 0};
	tmp0 = trans(tmp0);

	mat z0(12*HORIZON, 1);

	mat beq(9*HORIZON,1);

	x0 = {1, 0, 0, 1, 0, 0, 0, 0, 0};
	x0 = trans(x0);

	//for testing purpose
	float noise_mu = 0.0001;
	mat noise = noise_mu * x0;
	mat beqFirstHorizon = A_d_all*x0 + noise;
	
	for(int i = 0; i < 9; i++) {

		beq(i,0) = beqFirstHorizon(i, 0);

	}

	for(int i = 0; i < 12; i ++) {
		
		z0(i,0) = tmp0(i,0);
	
	}

	// set the values of entrices of beq for other horizons
	for(int i = 9; i < HORIZON*9; i ++) {

		beq(i, 0) = noise(i%9,0);

	}

	for (int i = 12; i < HORIZON*12; i++) {

		z0(i,0) = tmp0(i%12,0);

	}

	mat v0 =  0.05 * randi<mat>(9*HORIZON, 1, distr_param(0, 20));

	mat z = z0;
	mat v = v0;

 	vec r(21);
 	r.ones();
	vec r_d(12*HORIZON);
	vec r_p(9*HORIZON);

	float w_r_p = 1e-4;
	float w_d_p = 1e-4;

	int loop_num = 0;

	float fval;
	float x, y, z_h;
	mat HESSIAN, GRADIENT;

	float u1, u2, u3, h;
	float tmp_denum_grad_x_1, tmp_denum_grad_x_2, tmp_denum_grad_y_1, tmp_denum_grad_y_2;
	float d_phi_dx, d_phi_dy, d_phi_dz;
	float d_phi_du1, d_phi_du2, d_phi_du3;

	float tmp_denum_x_1, tmp_denum_x_2, tmp_denum_y_1, tmp_denum_y_2;
	float dd_phi_dxdx, dd_phi_dxdz, dd_phi_dydy, dd_phi_dydz, dd_phi_dzdz;
	float dd_phi_du1du1, dd_phi_du2du2, dd_phi_du3du3;
	
	// infeasible start newton method params 
	vec d_v, d_z, beta;
	mat inv_PHI, Y;

	//linear search parameters
	float beta_line_search; //parameter
	float d; //step size
	vec line_search_tmp;

	clock_t start;
	double duration;
	start = clock();

	thread first(updateStateThread);


	while(1) {
		start = clock();
		beqFirstHorizon = A_d_all*x0 + noise;
	
		for(int i = 0; i < 9; i++) {

			beq(i,0) = beqFirstHorizon(i, 0);

		}

		while(norm(r) > 1e-4) {
			duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;

			if(duration > 0.2) {
				continue;
			}
			
			fval = dot(trans(z), (H * z));


			HESSIAN.zeros(12*HORIZON, 12*HORIZON);
			GRADIENT.zeros(12*HORIZON, 1);


			for (int i = 0; i < HORIZON; i++) {

				x = z.at(3 + i * 12);

				y = z.at(6 + i * 12);

				z_h = z.at(9 + i * 12);

				u1 = z.at(0 + 12 * i);


				u2 = z.at(1 + 12 * i);

				u3 = z.at(2 + 12 * i);


				h = -z_h + better_height;

				tmp_denum_grad_x_1 = h*tan_half_phi - x;
				tmp_denum_grad_x_2 = h*tan_half_phi + x;
				tmp_denum_grad_y_1 = h*tan_half_psi - y;
				tmp_denum_grad_y_2 = h*tan_half_psi + y;

				d_phi_dx = 1/tmp_denum_grad_x_1 - 1/tmp_denum_grad_x_2;
	         	d_phi_dy = 1/tmp_denum_grad_y_1 - 1/tmp_denum_grad_y_2;
	         	d_phi_dz = 1/(z_max - z_h) - 1/(z_h - z_min) + tan_half_phi/tmp_denum_grad_x_1 + tan_half_phi/tmp_denum_grad_x_2
	             	+ tan_half_psi/tmp_denum_grad_y_1 + tan_half_psi/tmp_denum_grad_y_2;

	         	d_phi_du1 = 1/(u1_max - u1) - 1/(u1 - u1_min);
	        	d_phi_du2 = 1/(u2_max - u2) - 1/(u2 - u2_min);
	         	d_phi_du3 = 1/(u3_max - u3) - 1/(u3 - u3_min);

	         	GRADIENT.at(i*12) = mu_u*d_phi_du1;
	         	GRADIENT.at(i*12 + 1) = mu_u*d_phi_du2;
	         	GRADIENT.at(i*12 + 2) = mu_u*d_phi_du3;
	         	GRADIENT.at(i*12 + 3) = mu_state*d_phi_dx;
	         	GRADIENT.at(i*12 + 6) = mu_state*d_phi_dy;
	         	GRADIENT.at(i*12 + 9) = mu_state*d_phi_dz;

	         	tmp_denum_x_1 = (h*tan_half_phi - x)*(h*tan_half_phi - x);
	         	tmp_denum_x_2 = (h*tan_half_phi + x)*(h*tan_half_phi + x);

	         	tmp_denum_y_1 = (h*tan_half_psi - y)*(h*tan_half_psi - y);
	         	tmp_denum_y_2 = (h*tan_half_psi + y)*(h*tan_half_psi + y);

	         	dd_phi_dxdx = 1/tmp_denum_x_1 + 1/tmp_denum_x_2;
	         	dd_phi_dxdz = tan_half_phi/tmp_denum_x_1 - tan_half_phi/tmp_denum_x_2;

	         	dd_phi_dydy = 1/tmp_denum_y_1 + 1/tmp_denum_y_2;
	         	dd_phi_dydz = tan_half_psi/tmp_denum_y_1 - tan_half_psi/tmp_denum_y_2;

	         	dd_phi_dzdz = 1/(z_max - z_h)/(z_max - z_h) + 1/(z_h - z_min)/(z_h - z_min) + tan_half_phi*tan_half_phi/tmp_denum_x_1
	            	 + tan_half_phi*tan_half_phi/tmp_denum_x_2 + tan_half_psi*tan_half_psi/tmp_denum_y_1 + tan_half_psi*tan_half_psi/tmp_denum_y_2;

	         	dd_phi_du1du1 = 1/(u1_max - u1)/(u1_max - u1) + 1/(u1 - u1_min)/(u1 - u1_min);
	         	dd_phi_du2du2 = 1/(u2_max - u2)/(u2_max - u2) + 1/(u2 - u2_min)/(u2 - u2_min);
	         	dd_phi_du3du3 = 1/(u3_max - u3)/(u3_max - u3) + 1/(u3 - u3_min)/(u3 - u3_min);

	         	HESSIAN.at(12*i, 12*i) = mu_u*dd_phi_du1du1;
	         	HESSIAN.at(12*i + 1, 12*i + 1) = mu_u*dd_phi_du2du2;
	         	HESSIAN.at(12*i + 2, 12*i + 2) = mu_u*dd_phi_du3du3;

	         	HESSIAN.at(12*i + 3, 12*i + 3) = mu_state*dd_phi_dxdx;
	         	HESSIAN.at(12*i + 3, 12*i + 9) = mu_state*dd_phi_dxdz;
	         	HESSIAN.at(12*i + 6, 12*i + 6) = mu_state*dd_phi_dydy;
	         	HESSIAN.at(12*i + 6, 12*i + 9) = mu_state*dd_phi_dydz;
	         	HESSIAN.at(12*i + 9, 12*i + 9) = mu_state*dd_phi_dzdz;
	         	HESSIAN.at(12*i + 9, 12*i + 3) = mu_state*dd_phi_dxdz;
	         	HESSIAN.at(12*i + 9, 12*i + 6) = mu_state*dd_phi_dydz;

			}

			r_d = 2*H*z + GRADIENT + trans(C)*v;
			r_p = C*z - beq;

			r = r_d;
			r.insert_rows(12*HORIZON, r_p);
			// start = clock();

			inv_PHI = inv(2*H + HESSIAN);
			// duration = ( clock() - start ) / (double) CLOCKS_PER_SEC;
			// cout<<"duration: " <<duration <<endl;
			// return 0;
			// inv_PHI.print("inv_PHI");
			// C.print("C");
			Y = C*inv_PHI*C.t();

			// Y.print("Y");
			beta = -r_p + C*inv_PHI*r_d;

			d_v = -inv_sympd(Y)*beta;
			d_z = inv_PHI*(-r_d - C.t()*d_v);

			// line search
			beta_line_search = 0.5;
			d = 1;
			line_search_tmp = 2*H*(z + d*d_z) + GRADIENT + C.t()*(v + d*d_v);
			line_search_tmp.insert_rows(line_search_tmp.n_rows,  (C*(z + d*d_z)-beq));
			
			while(norm(line_search_tmp)  > (1 - 0.4*d)*norm(r) ) {

				d = beta_line_search*d;

			}

			// update z and v
			z = z + d*d_z;
			v = v + d*d_v;
			// r.print("r");

			// cout<<__LINE__<<endl;

		}

		accel_msg.timestamp = utime_now();
		accel_msg.accel[0] = z(0);
		accel_msg.accel[1] = z(1);
		accel_msg.accel[2] = z(2);

		lcm1.publish("acceleration", &accel_msg);


		
		cout<<"duration: " <<duration <<endl;
		
		r.ones();

		usleep(5e4);

	}

	first.join();


	cout<<"control:u1 u2 u3 " <<z(0,0)<<", " <<z(1,0) <<", " <<z(2,0)<<endl;


}