From f0c3a061cfc32610746cebe0a822dcd947726774 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 8 Oct 2015 19:23:03 +0900 Subject: [PATCH] [rtc/SequencePlayer/interpolator.*,seqplay.cpp,rtc/AutoBalancer/AutoBalancer.cpp,rtc/AutoBalancer/GaitGenerator.h,rtc/CollisionDetector/CollisionDetector.cpp,rtc/EmergencyStopper/EmergencyStopper.cpp] Add name for interpolator and set name for RTCs using interpolator --- rtc/AutoBalancer/AutoBalancer.cpp | 4 ++++ rtc/AutoBalancer/GaitGenerator.h | 4 ++++ rtc/CollisionDetector/CollisionDetector.cpp | 1 + rtc/EmergencyStopper/EmergencyStopper.cpp | 1 + rtc/SequencePlayer/interpolator.cpp | 8 ++++---- rtc/SequencePlayer/interpolator.h | 3 +++ rtc/SequencePlayer/seqplay.cpp | 9 +++++++++ 7 files changed, 26 insertions(+), 4 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 6f7b4018ccd..9988147ece4 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -222,13 +222,17 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() } zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt); + zmp_offset_interpolator->setName(std::string(m_profile.instance_name)+" zmp_offset_interpolator"); zmp_transition_time = 1.0; transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator"); transition_interpolator_ratio = 1.0; adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator"); transition_time = 2.0; adjust_footstep_transition_time = 2.0; leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator"); leg_names_interpolator_ratio = 1.0; // setting stride limitations from conf file diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 50d7992e415..675a9ed413c 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -238,6 +238,7 @@ namespace rats zmp_weight_map = boost::assign::map_list_of(RLEG, zmp_weight_initial_value[0])(LLEG, zmp_weight_initial_value[1])(RARM, zmp_weight_initial_value[2])(LARM, zmp_weight_initial_value[3]); zmp_weight_interpolator = boost::shared_ptr(new interpolator(4, dt)); zmp_weight_interpolator->set(zmp_weight_initial_value); /* set initial value */ + zmp_weight_interpolator->setName("GaitGenerator zmp_weight_interpolator"); }; ~refzmp_generator() { @@ -623,6 +624,9 @@ namespace rats if (swing_foot_rot_ratio_interpolator == NULL) swing_foot_rot_ratio_interpolator = new interpolator(1, dt); //if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, dt, interpolator::LINEAR); if (toe_heel_interpolator == NULL) toe_heel_interpolator = new interpolator(1, dt); + foot_ratio_interpolator->setName("GaitGenerator foot_ratio_interpolator"); + swing_foot_rot_ratio_interpolator->setName("GaitGenerator swing_foot_rot_ratio_interpolator"); + toe_heel_interpolator->setName("GaitGenerator toe_heel_interpolator"); }; ~leg_coords_generator() { diff --git a/rtc/CollisionDetector/CollisionDetector.cpp b/rtc/CollisionDetector/CollisionDetector.cpp index aa8b40ca5d5..ab41bc32742 100644 --- a/rtc/CollisionDetector/CollisionDetector.cpp +++ b/rtc/CollisionDetector/CollisionDetector.cpp @@ -255,6 +255,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() m_recover_jointdata = new double[m_robot->numJoints()]; m_lastsafe_jointdata = new double[m_robot->numJoints()]; m_interpolator = new interpolator(m_robot->numJoints(), i_dt); + m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator"); m_link_collision = new bool[m_robot->numLinks()]; for(int i=0; inumJoints(); i++){ diff --git a/rtc/EmergencyStopper/EmergencyStopper.cpp b/rtc/EmergencyStopper/EmergencyStopper.cpp index c3efb8a1048..63a54b0c2f1 100644 --- a/rtc/EmergencyStopper/EmergencyStopper.cpp +++ b/rtc/EmergencyStopper/EmergencyStopper.cpp @@ -125,6 +125,7 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize() //default_retrieve_time = 1.0/m_dt; m_stop_posture = new double[m_robot->numJoints()]; m_interpolator = new interpolator(m_robot->numJoints(), recover_time_dt); + m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator"); m_q.data.length(m_robot->numJoints()); for(int i=0; inumJoints(); i++){ diff --git a/rtc/SequencePlayer/interpolator.cpp b/rtc/SequencePlayer/interpolator.cpp index 72debd606c3..55eeb627378 100644 --- a/rtc/SequencePlayer/interpolator.cpp +++ b/rtc/SequencePlayer/interpolator.cpp @@ -216,7 +216,7 @@ void interpolator::load(const char *fname, double time_to_start, double scale, { ifstream strm(fname); if (!strm.is_open()) { - cerr << "file not found(" << fname << ")" << endl; + cerr << "[interpolator " << name << "] file not found(" << fname << ")" << endl; return; } double *vs, ptime=-1,time, tmp; @@ -353,17 +353,17 @@ void interpolator::get(double *x_, double *v_, double *a_, bool popp) if (length!=0){ double *&vs = q.front(); if (vs == NULL) { - cerr << "interpolator::get vs = NULL, q.size() = " << q.size() + cerr << "[interpolator " << name << "] interpolator::get vs = NULL, q.size() = " << q.size() << ", length = " << length << endl; } double *&dvs = dq.front(); if (dvs == NULL) { - cerr << "interpolator::get dvs = NULL, dq.size() = " << dq.size() + cerr << "[interpolator " << name << "] interpolator::get dvs = NULL, dq.size() = " << dq.size() << ", length = " << length << endl; } double *&ddvs = ddq.front(); if (ddvs == NULL) { - cerr << "interpolator::get ddvs = NULL, ddq.size() = " << ddq.size() + cerr << "[interpolator " << name << "] interpolator::get ddvs = NULL, ddq.size() = " << ddq.size() << ", length = " << length << endl; } memcpy(x_, vs, sizeof(double)*dim); diff --git a/rtc/SequencePlayer/interpolator.h b/rtc/SequencePlayer/interpolator.h index 9f64ff5126f..c4689a79e63 100644 --- a/rtc/SequencePlayer/interpolator.h +++ b/rtc/SequencePlayer/interpolator.h @@ -59,6 +59,7 @@ class interpolator void interpolate(double& remain_t_); double deltaT() const { return dt; } double dimension() const { return dim; } + void setName (const std::string& _name) { name = _name; }; private: // Current interpolation mode interpolation_mode imode; @@ -81,6 +82,8 @@ class interpolator double *a0, *a1, *a2, *a3, *a4, *a5; // Default average velocity for calc_interpolation_time double default_avg_vel; + // Interpolator name + std::string name; void hoffarbib(double &remain_t_, double a0, double a1, double a2, diff --git a/rtc/SequencePlayer/seqplay.cpp b/rtc/SequencePlayer/seqplay.cpp index d521d5e15a8..49b717b8be7 100644 --- a/rtc/SequencePlayer/seqplay.cpp +++ b/rtc/SequencePlayer/seqplay.cpp @@ -16,6 +16,15 @@ seqplay::seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum, unsigned interpolators[TQ] = new interpolator(i_dof, i_dt); interpolators[WRENCHES] = new interpolator(6 * i_fnum, i_dt, interpolator::HOFFARBIB, 100); // wrenches = 6 * [number of force sensors] interpolators[OPTIONAL_DATA] = new interpolator(optional_data_dim, i_dt); + // Set interpolator name + interpolators[Q]->setName("Q"); + interpolators[ZMP]->setName("ZMP"); + interpolators[ACC]->setName("ACC"); + interpolators[P]->setName("P"); + interpolators[RPY]->setName("RPY"); + interpolators[TQ]->setName("TQ"); + interpolators[WRENCHES]->setName("WRENCHES"); + interpolators[OPTIONAL_DATA]->setName("OPTIONAL_DATA"); // #ifdef WAIST_HEIGHT