void rice::pastry::socket::nat::rendezvous::RendezvousSocketPastryNodeFactory::init_(bool firewalled)
{
random = npc(environment)->getRandomSource();
if(firewalled)
setContactState(RendezvousSocketNodeHandle::CONTACT_FIREWALLED);
}
int StateEstimatorKinematic::run_state_est_lin_task() {
if (first_time) {
//initKF(Eigen::Vector3d(base_state.x[1],base_state.x[2],base_state.x[3]), Eigen::Vector3d(base_state.xd[1],base_state.xd[2],base_state.xd[3]));
//initKF(Eigen::Vector3d(0,0,0), Eigen::Vector3d(0,0,0));
first_time = false;
buffer_first_time = true;
contact_change_flag = true;
sek.initKF(Eigen::Vector3d(0,0,0), Eigen::Vector3d(0,0,0));
computeAnkleRelated();
contact_change_flag = true;
set_hack_footpos();
}
if (initializing_q > 0) {
initializing_q--;
Eigen::Matrix<double,3,1> current_imu_linear_acceleration;
current_imu_linear_acceleration[0] = misc_sensor[B_XACC_IMU];
current_imu_linear_acceleration[1] = misc_sensor[B_YACC_IMU];
current_imu_linear_acceleration[2] = misc_sensor[B_ZACC_IMU];
init_gravity_vector = 0.995 * init_gravity_vector + 0.005 *( _imu_quat_offset.conjugate()*current_imu_linear_acceleration);
if(initializing_q == 1) {
_q.setFromTwoVectors(init_gravity_vector,Eigen::Vector3d(0, 0, 1));
std::cout << "Done Initializing Q" <<std::endl;
std::cout << "Final vector = " <<std::endl;
std::cout << _imu_quat_offset*init_gravity_vector;
std::cout<<std::endl<<std::endl;
_gravity = -(_q.matrix()*(init_gravity_vector));
std::cout << "Final gravity = " <<std::endl;
std::cout << _gravity;
std::cout<<std::endl<<std::endl;
}
if(initializing_q ==0) {
computeAnkleRelated();
contact_change_flag = true;
set_hack_footpos();
}
_x[0] = 0;
_x[1] = 0;
_x[2] = 0;
_x[3] = 0;
_x[4] = 0;
_x[5] = 0;
return TRUE;
}
// static int printcounter = 1000;
// printcounter++;
// if (printcounter > 1000){
// printcounter = 0;
//
// std::cout<<"innov === "<<_innov[0] <<"\t"<<_innov[1]<<"\t"<<_innov[2] <<"\t" <<_innov[3] <<"\t"<<_innov[4]<<"\t"<<_innov[5];
// std::cout<<std::endl;
// std::cout<<"z === "<<_z[0] <<"\t"<<_z[1]<<"\t"<<_z[2] <<"\t" <<_z[3] <<"\t"<<_z[4]<<"\t"<<_z[5];
// std::cout<<std::endl;
// std::cout<<"y === "<<_y[0] <<" "<<_y[1]<<" "<<_y[2] <<"\t" <<_y[3] <<" "<<_y[4]<<" "<<_y[5];
// std::cout<<std::endl;
//// std::cout<<"err: "<<base_state.x[1] - _x[0] <<" "<<base_state.x[2] - _x[1]<<" "<<base_state.x[3] - _x[2] <<
//// "\t" <<base_state.xd[1] - _x[3] <<" "<<base_state.xd[2] - _x[4]<<" "<<base_state.xd[3] - _x[5];
// std::cout<<std::endl;
// std::cout<<std::endl;
// }
Eigen::Matrix<double,3,1> imu_angular_velocity;
imu_angular_velocity[0] = misc_sensor[B_AD_A_IMU];
imu_angular_velocity[1] = misc_sensor[B_AD_B_IMU];
imu_angular_velocity[2] = misc_sensor[B_AD_G_IMU];
Eigen::Matrix<double,3,1> imu_linear_acceleration;
imu_linear_acceleration[0] = misc_sensor[B_XACC_IMU];
imu_linear_acceleration[1] = misc_sensor[B_YACC_IMU];
imu_linear_acceleration[2] = misc_sensor[B_ZACC_IMU];
Eigen::Matrix<double, 50, 1> joints;
for(int x =0; x < 50; x++) joints[x] = joint_state[x+1].th;
Eigen::Matrix<double, 50, 1> jointsd;
for(int x =0; x < 50; x++) jointsd[x] = joint_state[x+1].thd;
imu_angular_velocity = _imu_quat_offset.normalized().conjugate().toRotationMatrix()*imu_angular_velocity;
integrate_angular_velocity(imu_angular_velocity);
makeInputs(_q, imu_angular_velocity, imu_linear_acceleration, joints, jointsd);
setContactState(2, -1);
makeMeasurement(misc_sensor[L_CFx], misc_sensor[R_CFx]);
filterOneTimeStep_ss();
return TRUE;
}
