void PlotWindow::update(Memory* mem) { FrameInfoBlock *frame_info(NULL); mem->getBlockByName(frame_info,"frame_info",false); if (frame_info == NULL) return; plot_marker_.setXValue(frame_info->seconds_since_start); plot_.replot(); }
void PlotWindow::setMemoryLog(Log* memory_log) { memory_log_ = memory_log; for (unsigned int i = 0; i < memory_log_->size(); i++) { Memory memory = memory_log_->getFrame(i); populateGraphableBlock(memory); } // set up the time FrameInfoBlock *frame_info(NULL); // clear times and data for new log times_.clear(); for (unsigned i = 0; i < plot_curves_.size(); i++){ plot_curves_[i]->detach(); delete plot_curves_[i]; } plot_curves_.clear(); for (unsigned int i = 0; i < memory_log_->size(); i++) { (*memory_log_)[i].getBlockByName(frame_info,"frame_info",false); if (frame_info == NULL) (*memory_log_)[i].getBlockByName(frame_info,"vision_frame_info",false); if (frame_info == NULL) continue; times_.push_back(frame_info->seconds_since_start); } std::string filename = std::string(getenv("NAO_HOME")) + "/tools/UTNaoTool/graph.csv"; std::ifstream in(filename.c_str()); std::string name; std::string color; if (!in.good()) { std::cout << "ERROR opening: " << filename << std::endl; return; } std::cout << "Graphing from: " << filename << std::endl; while (!in.eof()) { getline(in,name,','); if (in.eof()) break; getline(in,color,'\n'); if (in.eof()) break; if (name[0] == '#') continue; addLine(name,color); } //addLine("com.x","green"); //addLine("commanded com.x","blue"); plot_.replot(); }
int search_frames(s_audinfo_t *inf) { int ret = 0; int idx = 0; do { if (is_frame_valid(inf)) inf->parser.idx++; idx++; } while (idx < inf->filebytes); frame_info(inf); return ret; }
void debug_backtrace_dump(const struct debug_stack_frame *backtrace, unsigned nr_frames) { unsigned i, offset; const char *filename; for (i = 0; i < nr_frames; ++i) { if (!backtrace[i].start_ip) break; filename = frame_info(&backtrace[i], &offset); debug_printf("\t%s(+0x%x) (%s+0x%x) [%p]\n", filename, offset, backtrace[i].procname, backtrace[i].off, frame_ip(&backtrace[i])); } }
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)); } }