parameters::parameters(datafile dat, model nv_mod, model ref_mod,parameters ref_param,int compo,int iter){
const MatrixXi & omega=nv_mod.Get_model(),ref_omega=ref_mod.Get_model(),mat=dat.Get_mat_datafile();
const int g=omega.rows(),unique=mat.rows();
m_proba=ref_param.m_proba;
m_proba_block.resize(g);
m_param.resize(g);
for (int k=0;k<g;k++){
if (k!=compo){
m_param[k].resize(omega.rowwise().maxCoeff()(k)+1);
m_param[k]=ref_param.m_param[k];
m_proba_block[k].resize(unique,omega.rowwise().maxCoeff()(k)+1);
m_proba_block[k]=ref_param.m_proba_block[k];
for (int b=0;b<(omega.rowwise().maxCoeff()(k)+1) ;b++){
if ((omega.row(k).array()==b).any()){
m_param[k][b]=ref_param.m_param[k][b];
}
}
}else{
m_param[k].resize(omega.rowwise().maxCoeff()(k)+1);
m_proba_block[k].resize(unique,omega.rowwise().maxCoeff()(k)+1);
for (int b=0;b<(omega.rowwise().maxCoeff()(k)+1) ;b++){
if ((omega.row(k).array()==b).any()){
if ((((omega.row(k).array()==b)==(ref_omega.row(k).array()==b)).prod())==1){
m_param[k][b]=ref_param.m_param[k][b];
}else{
m_param[k][b]=param_block(k,b,dat,nv_mod,m_proba.col(k).array()/m_proba.rowwise().sum().array(),1);
if ((omega.row(k).array()==b).count()>1){
int prem=0;
while(omega(k,prem)!=b){prem++;}
if (mat.col(prem).maxCoeff()>5){
m_param[k][b]=m_param[k][b].Optimise_gamma(k,b,dat,nv_mod,5,m_proba.col(k).array()/m_proba.rowwise().sum().array(),dat.Get_eff_datafile());
}
}
}
}
}
}
}
m_propor=uniforme(g);
Probapost( nv_mod , mat );
Compte_nbparam(dat,nv_mod);
Likelihood(dat.Get_eff_datafile());
Estimation(1,0,iter,dat,nv_mod);
}
parameters::parameters(datafile dat, model mod){
// TODO Auto-generated constructor stub
const MatrixXi & omega=mod.Get_model(),mat=dat.Get_mat_datafile();
const int g=omega.rows(),unique=mat.rows();
m_proba=MatrixXd::Ones(unique,g);
m_proba_block.resize(g);
m_param.resize(g);
for (int k=0;k<g;k++){
m_param[k].resize(omega.rowwise().maxCoeff()(k)+1);
m_proba_block[k].resize(unique,omega.rowwise().maxCoeff()(k)+1);
for (int b=0;b<(omega.rowwise().maxCoeff()(k)+1) ;b++){
if ((omega.row(k).array()==b).any()){
m_param[k][b]=param_block(k,b,dat,mod,VectorXd::Ones(mat.rows()),1);
}
}
}
m_propor=uniforme(g);
Probapost( mod , mat );
Compte_nbparam(dat,mod);
Likelihood(dat.Get_eff_datafile());
Estimation(1,0,6,dat,mod);
}
void PlotWindow::populateGraphableBlock(Memory &memory) {
GraphableBlock *graphable = NULL;
memory.getOrAddBlockByName(graphable,"graphable");
WalkEngineBlock *walk_engine;
memory.getBlockByName(walk_engine,"walk_engine",false);
if (walk_engine != NULL) {
graphable->addData("abs_left_foot.x",walk_engine->abs_left_foot_.translation.x);
graphable->addData("abs_left_foot.y",walk_engine->abs_left_foot_.translation.y);
graphable->addData("abs_left_foot.z",walk_engine->abs_left_foot_.translation.z);
graphable->addData("abs_left_foot.rot",walk_engine->abs_left_foot_.rotation.getZAngle());
graphable->addData("abs_right_foot.x",walk_engine->abs_right_foot_.translation.x);
graphable->addData("abs_right_foot.y",walk_engine->abs_right_foot_.translation.y);
graphable->addData("abs_right_foot.z",walk_engine->abs_right_foot_.translation.z);
graphable->addData("abs_right_foot.rot",walk_engine->abs_right_foot_.rotation.getZAngle());
graphable->addData("sensor_zmp.x",walk_engine->sensor_zmp_.x);
graphable->addData("sensor_zmp.y",walk_engine->sensor_zmp_.y);
graphable->addData("current_state_zmp.x",walk_engine->current_state_.zmp_.x);
graphable->addData("current_state_zmp.y",walk_engine->current_state_.zmp_.y);
graphable->addData("current_state_pen_pos.x",walk_engine->current_state_.pen_pos_.x);
graphable->addData("current_state_pen_pos.y",walk_engine->current_state_.pen_pos_.y);
graphable->addData("desired_state_zmp.x",walk_engine->desired_next_state_.zmp_.x);
graphable->addData("desired_state_zmp.y",walk_engine->desired_next_state_.zmp_.y);
graphable->addData("desired_state_pen_pos.x",walk_engine->desired_next_state_.pen_pos_.x);
graphable->addData("desired_state_pen_pos.y",walk_engine->desired_next_state_.pen_pos_.y);
graphable->addData("desired_state_pen_vel.x",walk_engine->desired_next_state_.pen_vel_.x);
graphable->addData("desired_state_pen_vel.y",walk_engine->desired_next_state_.pen_vel_.y);
graphable->addData("desired_state_open_zmp.x",walk_engine->desired_next_without_closed_loop_.zmp_.x);
graphable->addData("desired_state_open_zmp.y",walk_engine->desired_next_without_closed_loop_.zmp_.y);
graphable->addData("desired_state_open_pen_pos.x",walk_engine->desired_next_without_closed_loop_.pen_pos_.x);
graphable->addData("desired_state_open_pen_pos.y",walk_engine->desired_next_without_closed_loop_.pen_pos_.y);
graphable->addData("desired_state_open_pen_vel.x",walk_engine->desired_next_without_closed_loop_.pen_vel_.x);
graphable->addData("desired_state_open_pen_vel.y",walk_engine->desired_next_without_closed_loop_.pen_vel_.y);
graphable->addData("ref_zmp.x",walk_engine->zmp_ref_[0].x);
graphable->addData("ref_zmp.y",walk_engine->zmp_ref_[0].y);
graphable->addData("delayed_zmp.x",walk_engine->delayed_zmp_state_.x);
graphable->addData("delayed_zmp.y",walk_engine->delayed_zmp_state_.y);
graphable->addData("delayed_com.x",walk_engine->delayed_pen_state_.x);
graphable->addData("delayed_com.y",walk_engine->delayed_pen_state_.y);
graphable->addData("sensor_com.x",walk_engine->sensor_pen_.x);
graphable->addData("sensor_com.y",walk_engine->sensor_pen_.y);
graphable->addData("swing_target.x",walk_engine->swing_foot_.translation.x);
graphable->addData("swing_target.y",walk_engine->swing_foot_.translation.y);
graphable->addData("swing_target.z",walk_engine->swing_foot_.translation.z);
graphable->addData("swing_target.rot",walk_engine->swing_foot_.rotation.getZAngle());
graphable->addData("step_current.x",walk_engine->step_current_.position_.translation.x);
graphable->addData("step_current.y",walk_engine->step_current_.position_.translation.y);
graphable->addData("step_current.rot",RAD_T_DEG * walk_engine->step_current_.position_.rotation);
float support_foot;
if (walk_engine->step_current_.is_left_foot_)
support_foot = -10;
else
support_foot = 10;
WalkParamBlock *param_block(NULL);
memory.getBlockByName(param_block,"walk_param",false);
FrameInfoBlock *frame_info(NULL);
memory.getBlockByName(frame_info,"frame_info",false);
if (param_block != NULL && frame_info != NULL) {
if (frame_info->frame_id < walk_engine->step_current_.frame_ + walk_engine->num_double_support_frames_)
support_foot = 0;
float phase_frac = (frame_info->frame_id - walk_engine->step_current_.frame_) / (float)(walk_engine->step_next_.frame_ - walk_engine->step_current_.frame_);
//std::cout << phase_frac << std::endl;
phase_frac = crop(phase_frac,-10,10);
//float single_support_frac = (phase_frac - param_block->params_.double_support_frac_ - 0.01 / param_block->params_.phase_length_) / (1.0 - param_block->params_.double_support_frac_);
float denom = walk_engine->step_next_.frame_ - walk_engine->step_current_.frame_;
if (denom <= 0)
denom = 1;
float single_support_frac = ((int)frame_info->frame_id - (int)walk_engine->step_current_.frame_ - (int)walk_engine->num_double_support_frames_) / denom;
graphable->addData("phase_frac",phase_frac);
graphable->addData("single_support_frac",single_support_frac);
}
graphable->addData("support_foot",support_foot);
}
BodyModelBlock *body_model;
memory.getBlockByName(body_model,"body_model",false);
if (body_model != NULL) {
graphable->addData("com.x",body_model->center_of_mass_.x);
graphable->addData("com.y",body_model->center_of_mass_.y);
graphable->addData("com.z",body_model->center_of_mass_.z);
if (walk_engine != NULL) {
Vector3<float> abs_to_stance_offset;
Vector3<float> abs_to_swing_offset;
if (walk_engine->step_current_.is_left_foot_) {
abs_to_stance_offset = -body_model->abs_parts_[BodyPart::left_foot].translation;
abs_to_swing_offset = -body_model->abs_parts_[BodyPart::right_foot].translation;
} else {
abs_to_stance_offset = -body_model->abs_parts_[BodyPart::right_foot].translation;
abs_to_swing_offset = -body_model->abs_parts_[BodyPart::left_foot].translation;
}
graphable->addData("abs_to_stance_offset.x",abs_to_stance_offset.x);
graphable->addData("abs_to_stance_offset.y",abs_to_stance_offset.y);
graphable->addData("abs_to_swing_offset.x",abs_to_swing_offset.x);
graphable->addData("abs_to_swing_offset.y",abs_to_swing_offset.y);
// convert from abs to stance
Pose3D com(0,0,0);
com.translation = body_model->center_of_mass_ + abs_to_stance_offset;
// convert from stance back to global
com = com.relativeToGlobal(walk_engine->global_frame_offset_);
com.translation.z += robot_dimensions_.footHeight;
graphable->addData("global_com.x",com.translation.x);
graphable->addData("global_com.y",com.translation.y);
graphable->addData("global_com.z",com.translation.z);
}
graphable->addData("sensor_tilt", RAD_T_DEG*body_model->sensors_tilt_roll_.tilt_);
graphable->addData("sensor_roll", RAD_T_DEG*body_model->sensors_tilt_roll_.roll_);
graphable->addData("left_foot_tilt", RAD_T_DEG*body_model->left_foot_body_tilt_roll_.tilt_);
graphable->addData("left_foot_roll",RAD_T_DEG* body_model->left_foot_body_tilt_roll_.roll_);
graphable->addData("right_foot_tilt", RAD_T_DEG*body_model->right_foot_body_tilt_roll_.tilt_);
graphable->addData("right_foot_roll", RAD_T_DEG*body_model->right_foot_body_tilt_roll_.roll_);
}
SensorBlock* sensors;
memory.getBlockByName(sensors,"processed_sensors",false);
if (sensors != NULL){
graphable->addData("sensor_accel.x",sensors->values_[accelX]);
graphable->addData("sensor_accel.y",sensors->values_[accelY]);
graphable->addData("sensor_accel.z",sensors->values_[accelZ]);
graphable->addData("sensor_tilt",sensors->values_[angleY]);
graphable->addData("sensor_roll",sensors->values_[angleX]);
}
SensorBlock* raw_sensors;
memory.getBlockByName(raw_sensors,"raw_sensors",false);
if (raw_sensors != NULL) {
graphable->addData("raw_sensor_tilt",raw_sensors->values_[angleY]);
graphable->addData("raw_sensor_roll",raw_sensors->values_[angleX]);
}
JointBlock *joint_angles;
memory.getBlockByName(joint_angles,"processed_joint_angles",false);
if (joint_angles != NULL) {
graphable->addData("sensed_lhiproll",RAD_T_DEG * joint_angles->values_[LHipRoll]);
graphable->addData("sensed_rhiproll",RAD_T_DEG * joint_angles->values_[RHipRoll]);
graphable->addData("sensed_lhippitch",RAD_T_DEG * joint_angles->values_[LHipPitch]);
graphable->addData("sensed_rhippitch",RAD_T_DEG * joint_angles->values_[RHipPitch]);
graphable->addData("sensed_lanklepitch",RAD_T_DEG * joint_angles->values_[LAnklePitch]);
graphable->addData("sensed_ranklepitch",RAD_T_DEG * joint_angles->values_[RAnklePitch]);
graphable->addData("sensed_lankleroll",RAD_T_DEG * joint_angles->values_[LAnkleRoll]);
graphable->addData("sensed_rankleroll",RAD_T_DEG * joint_angles->values_[RAnkleRoll]);
}
JointCommandBlock *joint_commands;
memory.getBlockByName(joint_commands,"processed_joint_commands",false);
if (joint_commands != NULL) {
graphable->addData("commanded_lhiproll",RAD_T_DEG * joint_commands ->angles_[LHipRoll]);
graphable->addData("commanded_rhiproll",RAD_T_DEG * joint_commands ->angles_[RHipRoll]);
graphable->addData("commanded_lhippitch",RAD_T_DEG * joint_commands ->angles_[LHipPitch]);
graphable->addData("commanded_rhippitch",RAD_T_DEG * joint_commands ->angles_[RHipPitch]);
graphable->addData("commanded_lkneepitch",RAD_T_DEG * joint_commands ->angles_[LKneePitch]);
graphable->addData("commanded_rkneepitch",RAD_T_DEG * joint_commands ->angles_[RKneePitch]);
graphable->addData("commanded_lanklepitch",RAD_T_DEG * joint_commands->angles_[LAnklePitch]);
graphable->addData("commanded_ranklepitch",RAD_T_DEG * joint_commands->angles_[RAnklePitch]);
graphable->addData("commanded_lankleroll",RAD_T_DEG * joint_commands->angles_[LAnkleRoll]);
graphable->addData("commanded_rankleroll",RAD_T_DEG * joint_commands->angles_[RAnkleRoll]);
}
OdometryBlock *odometry;
memory.getBlockByName(odometry,"vision_odometry",false);
if (odometry != NULL) {
graphable->addData("odom.y",odometry->displacement.translation.y);
}
WorldObjectBlock *world_object;
memory.getBlockByName(world_object,"world_objects",false);
if (world_object != NULL) {
WorldObject &ball = world_object->objects_[WO_BALL];
graphable->addData("rel_ball.y",ball.relPos.y);
graphable->addData("vision_rel_ball.y",ball.visionDistance * sin(ball.visionBearing));
}
}
