void LoginDialog::profileChanged(int)
{
if (m_client)
return;
int n = cmbProfile->currentItem();
if (n < 0){
clearInputs();
buttonOk->setEnabled(false);
btnDelete->setEnabled(false);
return;
}
buttonOk->setEnabled(true);
if (n >= (int)cmbProfile->count() - 1){
lblPasswd->hide();
clearInputs();
btnDelete->setEnabled(false);
}else{
clearInputs();
CorePlugin::m_plugin->setProfile(CorePlugin::m_plugin->m_profiles[n].c_str());
ClientList clients;
CorePlugin::m_plugin->loadClients(clients);
unsigned nClients = 0;
unsigned i;
for (i = 0; i < clients.size(); i++){
if (clients[i]->protocol()->description()->flags & PROTOCOL_NO_AUTH)
continue;
nClients++;
}
if (nClients > 1){
lblPasswd->show();
}else{
lblPasswd->hide();
}
unsigned row = 2;
if (nClients == 1){
makeInputs(row, clients[0], true);
}else{
for (unsigned i = 0; i < clients.size(); i++){
if (clients[i]->protocol()->description()->flags & PROTOCOL_NO_AUTH)
continue;
makeInputs(row, clients[i], false);
}
}
if (passwords.size())
passwords[0]->setFocus();
btnDelete->setEnabled(m_loginProfile == CorePlugin::m_plugin->m_profiles[n].c_str());
buttonOk->setEnabled(false);
pswdChanged("");
}
QTimer::singleShot(0, this, SLOT(adjust()));
}
void LoginDialog::profileChanged(int)
{
if (m_client)
return;
int n = cmbProfile->currentItem();
if (n < 0){
clearInputs();
buttonOk->setEnabled(false);
btnDelete->setEnabled(false);
return;
}
buttonOk->setEnabled(true);
if (n >= (int)cmbProfile->count() - 1){
lblPasswd->hide();
clearInputs();
btnDelete->setEnabled(false);
}else{
clearInputs();
CorePlugin::m_plugin->setProfile(CorePlugin::m_plugin->m_profiles[n].c_str());
ClientList clients;
CorePlugin::m_plugin->loadClients(clients);
if (clients.size() > 1){
lblPasswd->show();
}else{
lblPasswd->hide();
}
unsigned row = 2;
if (clients.size() == 1){
makeInputs(row, clients[0], true);
}else{
for (unsigned i = 0; i < clients.size(); i++)
makeInputs(row, clients[i], false);
}
if (passwords.size())
passwords[0]->setFocus();
btnDelete->setEnabled(m_loginProfile == CorePlugin::m_plugin->m_profiles[n].c_str());
buttonOk->setEnabled(false);
pswdChanged("");
}
}
void LoginDialog::fill()
{
if (m_client){
lblPasswd->hide();
unsigned row = 2;
makeInputs(row, m_client, true);
return;
}
cmbProfile->clear();
int newCur = -1;
string save_profile = CorePlugin::m_plugin->getProfile();
CorePlugin::m_plugin->m_profiles.clear();
CorePlugin::m_plugin->loadDir();
for (unsigned i = 0; i < CorePlugin::m_plugin->m_profiles.size(); i++){
string curProfile = CorePlugin::m_plugin->m_profiles[i];
if (!strcmp(curProfile.c_str(), save_profile.c_str()))
newCur = i;
CorePlugin::m_plugin->setProfile(curProfile.c_str());
ClientList clients;
CorePlugin::m_plugin->loadClients(clients);
if (clients.size()){
Client *client = clients[0];
cmbProfile->insertItem(
Pict(client->protocol()->description()->icon),
QString::fromLocal8Bit(client->name().c_str()));
}
}
cmbProfile->insertItem(i18n("New profile"));
if (newCur != - 1){
cmbProfile->setCurrentItem(newCur);
CorePlugin::m_plugin->setProfile(save_profile.c_str());
}else{
cmbProfile->setCurrentItem(cmbProfile->count() - 1);
CorePlugin::m_plugin->setProfile(NULL);
}
}
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;
}
