seqplay.cpp
Go to the documentation of this file.
00001 // -*- mode: c++; indent-tabs-mode: t; tab-width: 4; c-basic-offset: 4; -*-
00002 
00003 #include <iostream>
00004 #include <unistd.h>
00005 #include "seqplay.h"
00006 
00007 #define deg2rad(x)      ((x)*M_PI/180)
00008 
00009 seqplay::seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum, unsigned int optional_data_dim) : m_dof(i_dof)
00010 {
00011     interpolators[Q] = new interpolator(i_dof, i_dt);
00012     interpolators[ZMP] = new interpolator(3, i_dt);
00013     interpolators[ACC] = new interpolator(3, i_dt);
00014     interpolators[P] = new interpolator(3, i_dt);
00015     interpolators[RPY] = new interpolator(3, i_dt);
00016     interpolators[TQ] = new interpolator(i_dof, i_dt);
00017     interpolators[WRENCHES] = new interpolator(6 * i_fnum, i_dt, interpolator::HOFFARBIB, 100); // wrenches = 6 * [number of force sensors]
00018         interpolators[OPTIONAL_DATA] = new interpolator(optional_data_dim, i_dt);
00019     // Set interpolator name
00020     interpolators[Q]->setName("Q");
00021     interpolators[ZMP]->setName("ZMP");
00022     interpolators[ACC]->setName("ACC");
00023     interpolators[P]->setName("P");
00024     interpolators[RPY]->setName("RPY");
00025     interpolators[TQ]->setName("TQ");
00026     interpolators[WRENCHES]->setName("WRENCHES");
00027     interpolators[OPTIONAL_DATA]->setName("OPTIONAL_DATA");
00028     //
00029 
00030 #ifdef WAIST_HEIGHT
00031     double initial_zmp[3] = {0,0,-WAIST_HEIGHT};
00032     double initial_waist[3] = {0,0,WAIST_HEIGHT};
00033     interpolators[P]->set(initial_waist);
00034 #elif defined(INITIAL_ZMP_REF_X)
00035     double initial_zmp[3] = {INITIAL_ZMP_REF_X, 0, INITIAL_ZMP_REF_Z};
00036 #else
00037     double initial_zmp[] = {0,0,0};
00038 #endif
00039     interpolators[ZMP]->set(initial_zmp);
00040     double initial_wrenches[6 * i_fnum];
00041     for (size_t i = 0; i < 6 * i_fnum; i++) initial_wrenches[i] = 0;
00042     interpolators[WRENCHES]->set(initial_wrenches);
00043         double initial_optional_data[optional_data_dim];
00044         for (size_t i = 0; i < optional_data_dim; i++) initial_optional_data[i] = 0;
00045         interpolators[OPTIONAL_DATA]->set(initial_optional_data);
00046 }
00047 
00048 seqplay::~seqplay()
00049 {
00050         for (unsigned int i=0; i<NINTERPOLATOR; i++){
00051                 delete interpolators[i];
00052         }
00053 }
00054 
00055 #if 0 // TODO
00056 void seqplay::goHalfSitting(double tm)
00057 {
00058     if (tm == 0){
00059         tm = (double)angle_interpolator->calc_interpolation_time(get_half_sitting_posture());
00060     }
00061     angle_interpolator->setGoal(get_half_sitting_posture(), tm);
00062 #ifdef INITIAL_ZMP_REF_X
00063     double zmp[]={INITIAL_ZMP_REF_X, 0, INITIAL_ZMP_REF_Z};
00064 #else
00065     double zmp[] = {0,0,0};
00066 #endif
00067     zmp_interpolator->setGoal(zmp, tm);
00068 #ifdef INITIAL_ZMP_REF_Z
00069     double waist_pos[]={ref_state.basePosAtt.px, 
00070                         ref_state.basePosAtt.py,
00071                         -INITIAL_ZMP_REF_Z};
00072 #else
00073     double waist_pos[] = {0,0,0};
00074 #endif
00075     waist_pos_interpolator->setGoal(waist_pos, tm);
00076     double p[3], rpy[3];
00077     OpenHRP::convTransformToPosRpy(ref_state.basePosAtt, p, rpy);
00078     double initial_rpy[3]={0,0,rpy[2]};
00079     waist_rpy_interpolator->setGoal(initial_rpy, tm);
00080 }
00081 
00082 void seqplay::goInitial(double tm)
00083 {
00084     if (tm == 0){
00085         tm = (double)angle_interpolator->calc_interpolation_time(get_initial_posture());
00086     }
00087     angle_interpolator->setGoal(get_initial_posture(), tm);
00088 #ifdef WAIST_HEIGHT
00089     double zmp[] = {0,0,-WAIST_HEIGHT};
00090     double pos[] = {ref_state.basePosAtt.px,
00091                     ref_state.basePosAtt.py,
00092                     WAIST_HEIGHT};
00093     waist_pos_interpolator->setGoal(pos, tm);
00094 #elif defined(INITIAL_ZMP_REF_Z)
00095     double zmp[] = {0,0, INITIAL_ZMP_REF_Z};
00096 #else
00097     double zmp[] = {0,0,0};
00098 #endif
00099     zmp_interpolator->setGoal(zmp, tm);
00100 }
00101 #endif
00102 
00103 bool seqplay::isEmpty() const
00104 {
00105         for (unsigned int i=0; i<NINTERPOLATOR; i++){
00106                 if (!interpolators[i]->isEmpty()) return false;
00107         }
00108         std::map<std::string, groupInterpolator *>::const_iterator it;
00109         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00110                 groupInterpolator *gi = it->second;
00111                 if (gi && !gi->isEmpty()) return false;
00112         }
00113 
00114         return true;
00115 }
00116 
00117 bool seqplay::isEmpty(const char *gname)
00118 {
00119         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00120         groupInterpolator *i = groupInterpolators[gname];
00121         if (!i) return true;
00122         return i->isEmpty();
00123 }
00124 
00125 #if 0
00126 void seqplay::setReferenceState(const ::CharacterState& ref, double tm)
00127 {
00128     if (tm == 0){
00129         tm = (double)angle_interpolator->calc_interpolation_time(ref.angle);
00130     }
00131     if (ref.angle.length()>0) angle_interpolator->setGoal(ref.angle, tm, false);
00132     if (ref.velocity.length()>0)
00133         velocity_interpolator->setGoal(ref.velocity, tm, false);
00134     if (ref.zmp.length()>0) zmp_interpolator->setGoal(ref.zmp, tm, false);
00135     if (ref.accel.length()>0 && ref.accel[0].length() == 3)
00136         acc_interpolator->setGoal(ref.accel[0], tm, false);
00137     double p[3], rpy[3];
00138     OpenHRP::convTransformToPosRpy(ref.basePosAtt, p, rpy);
00139     waist_pos_interpolator->setGoal(p, tm, false);
00140     waist_rpy_interpolator->setGoal(rpy, tm, false);
00141 
00142     sync();
00143 }
00144 
00145 void seqplay::getReferenceState(::CharacterState_out ref)
00146 {
00147     ref = new CharacterState;
00148     *ref = ref_state;
00149 }
00150 #endif
00151 
00152 
00153 void seqplay::setJointAngles(const double *jvs, double tm)
00154 {
00155         if (tm == 0){
00156                 interpolators[Q]->set(jvs);
00157         }else{
00158                 interpolators[Q]->setGoal(jvs, tm);
00159         }
00160 }
00161 
00162 void seqplay::getJointAngles(double *jvs)
00163 {
00164     interpolators[Q]->get(jvs, false);
00165 }
00166 
00167 
00168 void seqplay::setZmp(const double *i_zmp, double i_tm)
00169 {
00170         if (i_tm == 0){
00171                 interpolators[ZMP]->set(i_zmp);
00172         }else{
00173                 interpolators[ZMP]->setGoal(i_zmp, i_tm);
00174         }
00175 }
00176 
00177 void seqplay::setBasePos(const double *i_pos, double i_tm)
00178 {
00179         if (i_tm == 0){
00180                 interpolators[P]->set(i_pos);
00181         }else{
00182                 interpolators[P]->setGoal(i_pos, i_tm);
00183         }
00184 }
00185 
00186 void seqplay::setBaseRpy(const double *i_rpy, double i_tm)
00187 {
00188         if (i_tm == 0){
00189                 interpolators[RPY]->set(i_rpy);
00190         }else{
00191                 interpolators[RPY]->setGoal(i_rpy, i_tm);
00192         }
00193 }
00194 
00195 void seqplay::setBaseAcc(const double *i_acc, double i_tm)
00196 {
00197         if (i_tm == 0){
00198                 interpolators[ACC]->set(i_acc);
00199         }else{
00200                 interpolators[ACC]->setGoal(i_acc, i_tm);
00201         }
00202 }
00203 
00204 void seqplay::setWrenches(const double *i_wrenches, double i_tm)
00205 {
00206         if (i_tm == 0){
00207                 interpolators[WRENCHES]->set(i_wrenches);
00208         }else{
00209                 interpolators[WRENCHES]->setGoal(i_wrenches, i_tm);
00210         }
00211 }
00212 
00213 void seqplay::setJointAngle(unsigned int i_rank, double jv, double tm)
00214 {
00215     double pos[m_dof];
00216         getJointAngles(pos);
00217     pos[i_rank] = jv;
00218     interpolators[Q]->setGoal(pos, tm);
00219 }
00220 
00221 void seqplay::playPattern(std::vector<const double*> pos, std::vector<const double*> zmp, std::vector<const double*> rpy, std::vector<double> tm, const double *qInit, unsigned int len)
00222 {
00223     const double *q=NULL, *z=NULL, *a=NULL, *p=NULL, *e=NULL, *tq=NULL, *wr=NULL, *od=NULL; double t=0;
00224     double *v = new double[len];
00225     for (unsigned int i=0; i<pos.size(); i++){
00226         q = pos[i];
00227         if (i < pos.size() - 1 ) {
00228           double t0, t1;
00229           if (tm.size() == pos.size()) {
00230             t0 = tm[i]; t1 = tm[i+1];
00231           } else {
00232             t0 = t1 = tm[0];
00233           }
00234           const double *q_next = pos[i+1];
00235           const double *q_prev 
00236               = i==0 ? qInit : pos[i-1];
00237           for (unsigned int j = 0; j < len; j++) {
00238             double d0, d1, v0, v1;
00239             d0 = (q[j] - q_prev[j]);
00240             d1 = (q_next[j] - q[j]);
00241             v0 = d0/t0;
00242             v1 = d1/t1;
00243             if ( v0 * v1 >= 0 ) {
00244               v[j] = 0.5 * (v0 + v1);
00245             } else {
00246               v[j] = 0;
00247             }
00248           }
00249         } else {
00250           for (unsigned int j = 0; j < len; j++) { v[j] = 0.0; }
00251         }
00252         if (i < zmp.size()) z = zmp[i];
00253         if (i < rpy.size()) e = rpy[i];
00254         if (i < tm.size()) t = tm[i];
00255         go(q, z, a, p, e, tq, wr, od,
00256                    v, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
00257                   t, false);
00258     }
00259     sync();
00260     delete [] v;
00261 }
00262 
00263 void seqplay::clear(double i_timeLimit)
00264 {
00265         tick_t t1 = get_tick();
00266         while (!isEmpty()){
00267                 if (i_timeLimit > 0 
00268                         && tick2sec(get_tick()-t1)>=i_timeLimit) break;
00269                 pop_back();
00270         }
00271 }
00272 
00273 void seqplay::loadPattern(const char *basename, double tm)
00274 {
00275     double scale = 1.0;
00276     bool found = false;
00277     if (debug_level > 0) cout << "pos   = ";
00278     string pos = basename; pos.append(".pos");
00279     if (access(pos.c_str(),0)==0){
00280         found = true;
00281         interpolators[Q]->load(pos, tm, scale, false);
00282         if (debug_level > 0) cout << pos;
00283     }
00284     if (debug_level > 0) cout << endl << "zmp   = ";
00285     string zmp = basename; zmp.append(".zmp");
00286     if (access(zmp.c_str(),0)==0){
00287         found = true;
00288         interpolators[ZMP]->load(zmp, tm, scale, false);
00289         if (debug_level > 0) cout << zmp;
00290     }
00291     if (debug_level > 0) cout << endl << "gsens = ";
00292     string acc = basename; acc.append(".gsens");
00293     if (access(acc.c_str(),0)==0){
00294         found = true;
00295         interpolators[ACC]->load(acc, tm, scale, false);
00296         if (debug_level > 0) cout << acc;
00297     }
00298     if (debug_level > 0) cout << endl << "hip   = ";
00299     string hip = basename; hip.append(".hip");
00300     if (access(hip.c_str(),0)==0){
00301         found = true;
00302         interpolators[RPY]->load(hip, tm, scale, false);
00303         if (debug_level > 0) cout << hip;
00304     }else{
00305         hip = basename; hip.append(".waist");
00306         if (access(hip.c_str(),0)==0){
00307             found = true;
00308             interpolators[P]->load(hip, tm, scale, false, 0, 3);
00309             interpolators[RPY]->load(hip, tm, scale, false, 3, 0);
00310             if (debug_level > 0) cout << hip;
00311         }
00312     }
00313     if (debug_level > 0) cout << endl << "torque = ";
00314     string torque = basename; torque.append(".torque");
00315     if (access(torque.c_str(),0)==0){
00316         found = true;
00317         interpolators[TQ]->load(torque, tm, scale, false);
00318         if (debug_level > 0) cout << torque;
00319     }
00320     if (debug_level > 0) cout << endl << "wrenches   = ";
00321     string wrenches = basename; wrenches.append(".wrenches");
00322     if (access(wrenches.c_str(),0)==0){
00323         found = true;
00324         interpolators[WRENCHES]->load(wrenches, tm, scale, false);
00325         if (debug_level > 0) cout << wrenches;
00326     }
00327     if (debug_level > 0) cout << endl << "optional_data   = ";
00328     string optional_data = basename; optional_data.append(".optionaldata");
00329     if (access(optional_data.c_str(),0)==0){
00330         found = true;
00331         interpolators[OPTIONAL_DATA]->load(optional_data, tm, scale, false);
00332         if (debug_level > 0) cout << optional_data;
00333     }
00334     if (debug_level > 0) cout << endl;
00335     if (!found) cerr << "pattern not found(" << basename << ")" << endl;
00336     //
00337     sync();
00338 }
00339 
00340 void seqplay::sync()
00341 {
00342         for (unsigned int i=0; i<NINTERPOLATOR; i++){
00343                 interpolators[i]->sync();
00344         }
00345 }
00346 
00347 void seqplay::pop_back()
00348 {
00349         for (unsigned int i=0; i<NINTERPOLATOR; i++){
00350                 interpolators[i]->pop_back();
00351         }
00352 }
00353 
00354 void seqplay::get(double *o_q, double *o_zmp, double *o_accel,
00355                                   double *o_basePos, double *o_baseRpy, double *o_tq, double *o_wrenches, double *o_optional_data)
00356 {
00357         double v[m_dof];
00358         interpolators[Q]->get(o_q, v);
00359         std::map<std::string, groupInterpolator *>::iterator it;
00360         for (it=groupInterpolators.begin(); it!=groupInterpolators.end();){
00361                 groupInterpolator *gi = it->second;
00362                 if (gi){
00363                         gi->get(o_q, v);
00364                         if (gi->state == groupInterpolator::removed){
00365                                 groupInterpolators.erase(it++);
00366                                 delete gi;
00367                                 continue;
00368                         }
00369                 }
00370                 ++it;
00371         }
00372         interpolators[ZMP]->get(o_zmp);
00373         interpolators[ACC]->get(o_accel);
00374         interpolators[P]->get(o_basePos);
00375         interpolators[RPY]->get(o_baseRpy);
00376         interpolators[TQ]->get(o_tq);
00377         interpolators[WRENCHES]->get(o_wrenches);
00378         interpolators[OPTIONAL_DATA]->get(o_optional_data);
00379 }
00380 
00381 void seqplay::go(const double *i_q, const double *i_zmp, const double *i_acc,
00382                                  const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data, double i_time, 
00383                                  bool immediate)
00384 {
00385         go(i_q, i_zmp, i_acc, i_p, i_rpy, i_tq, i_wrenches, i_optional_data,
00386            NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
00387            i_time, immediate);
00388 }
00389 
00390 void seqplay::go(const double *i_q, const double *i_zmp, const double *i_acc,
00391                                  const double *i_p, const double *i_rpy, const double *i_tq, const double *i_wrenches, const double *i_optional_data,
00392                                  const double *ii_q, const double *ii_zmp, const double *ii_acc,
00393                                  const double *ii_p, const double *ii_rpy, const double *ii_tq, const double *ii_wrenches, const double *ii_optional_data,
00394                                  double i_time,  bool immediate)
00395 {
00396         if (i_q) interpolators[Q]->go(i_q, ii_q, i_time, false);
00397         if (i_zmp) interpolators[ZMP]->go(i_zmp, ii_zmp, i_time, false);
00398         if (i_acc) interpolators[ACC]->go(i_acc, ii_acc, i_time, false);
00399         if (i_p) interpolators[P]->go(i_p, ii_p, i_time, false);
00400         if (i_rpy) interpolators[RPY]->go(i_rpy, ii_rpy, i_time, false);
00401         if (i_tq) interpolators[TQ]->go(i_tq, ii_tq, i_time, false);
00402         if (i_wrenches) interpolators[WRENCHES]->go(i_wrenches, ii_wrenches, i_time, false);
00403         if (i_optional_data) interpolators[OPTIONAL_DATA]->go(i_optional_data, ii_optional_data, i_time, false);
00404         if (immediate) sync();
00405 }
00406 
00407 bool seqplay::setInterpolationMode (interpolator::interpolation_mode i_mode_)
00408 {
00409     if (i_mode_ != interpolator::LINEAR && i_mode_ != interpolator::HOFFARBIB &&
00410                 i_mode_ != interpolator::QUINTICSPLINE && i_mode_ != interpolator::CUBICSPLINE) return false;
00411 
00412         bool ret=true; 
00413         for (unsigned int i=0; i<NINTERPOLATOR; i++){
00414                 ret &= interpolators[i]->setInterpolationMode(i_mode_);
00415         }
00416         std::map<std::string, groupInterpolator *>::const_iterator it;
00417         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00418                 groupInterpolator *gi = it->second;
00419                 ret &= gi->inter->setInterpolationMode(i_mode_);
00420         }
00421         return ret;
00422 }
00423 
00424 bool seqplay::addJointGroup(const char *gname, const std::vector<int>& indices)
00425 {
00426         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00427         groupInterpolator *i = groupInterpolators[gname];
00428         if (i) {
00429                 std::cerr << "[addJointGroup] group name " << gname << " is already installed" << std::endl;
00430                 return false;
00431         }
00432         i = new groupInterpolator(indices, interpolators[Q]->deltaT());
00433         groupInterpolators[gname] = i;
00434         return true;
00435 }
00436 
00437 bool seqplay::getJointGroup(const char *gname, std::vector<int>& indices)
00438 {
00439         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00440         groupInterpolator *i = groupInterpolators[gname];
00441         if (i) {
00442                 for(unsigned j = 0; j < i->indices.size(); j++) {
00443                         indices.push_back(i->indices[j]);
00444                 }
00445                 return true;
00446         }else{
00447                 std::cerr << "[getJointGroup] group name " << gname << " is not installed" << std::endl;
00448                 return false;
00449         }
00450 }
00451 
00452 bool seqplay::removeJointGroup(const char *gname, double time)
00453 {
00454         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00455         groupInterpolator *i = groupInterpolators[gname];
00456         if (i){
00457                 i->remove(time);
00458                 return true;
00459         }else{
00460                 std::cerr << "[removeJointGroup] group name " << gname << " is not installed" << std::endl;
00461                 return false;
00462         }
00463 }
00464 
00465 bool seqplay::resetJointGroup(const char *gname, const double *full)
00466 {
00467         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00468         groupInterpolator *i = groupInterpolators[gname];
00469         if (i){
00470                 i->set(full);
00471                 std::map<std::string, groupInterpolator *>::iterator it;
00472         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00473                         if ( it->first != std::string(gname) ) { // other 
00474                                 groupInterpolator *gi = it->second;
00475                                 if (gi && (gi->state == groupInterpolator::created || gi->state == groupInterpolator::working) && gi->inter->isEmpty()) {
00476                                         gi->set(full);
00477                                 }
00478                         }
00479                 }
00480         // update for non working interpolators
00481                 
00482                 return true;
00483         }else{
00484                 std::cerr << "[resetJointGroup] group name " << gname << " is not installed" << std::endl;
00485                 return false;
00486         }
00487 }
00488 
00489 bool seqplay::setJointAnglesOfGroup(const char *gname, const double* i_qRef, const size_t i_qsize, double i_tm)
00490 {
00491         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00492         groupInterpolator *i = groupInterpolators[gname];
00493         if (i){
00494                 if (i_qsize != i->indices.size() ) {
00495                         std::cerr << "[setJointAnglesOfGroup] group name " << gname << " : size of manipulater is not equal to input. " << i_qsize << " /= " << i->indices.size() << std::endl;
00496                         return false;
00497                 }
00498                 if (i->state == groupInterpolator::created){
00499                         double q[m_dof], dq[m_dof];
00500                         interpolators[Q]->get(q, dq, false);
00501                         std::map<std::string, groupInterpolator *>::iterator it;
00502                         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00503                                 groupInterpolator *gi = it->second;
00504                                 if (gi) gi->get(q, dq, false);
00505                         }
00506                         double x[i->indices.size()], v[i->indices.size()];
00507                         i->extract(x, q);
00508                         i->extract(v, dq);
00509                         i->inter->go(x,v,interpolators[Q]->deltaT());
00510                 }
00511                 double x[i->indices.size()], v[i->indices.size()];
00512                 i->inter->get(x, v, false);
00513                 i->setGoal(i_qRef, i_tm);
00514                 return true;
00515         }else{
00516                 std::cerr << "[setJointAnglesOfGroup] group name " << gname << " is not installed" << std::endl;
00517                 return false;
00518         }
00519 }
00520 
00521 void seqplay::clearOfGroup(const char *gname, double i_timeLimit)
00522 {
00523         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00524         groupInterpolator *i = groupInterpolators[gname];
00525         if (i){
00526                 i->clear(i_timeLimit);
00527         }
00528 }
00529 
00530 bool seqplay::playPatternOfGroup(const char *gname, std::vector<const double *> pos, std::vector<double> tm, const double *qInit, unsigned int len)
00531 {
00532         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00533         groupInterpolator *i = groupInterpolators[gname];
00534         if (i){
00535                 if (len != i->indices.size() ) {
00536                         std::cerr << "[playPatternOfGroup] group name " << gname << " : size of manipulater is not equal to input. " << len << " /= " << i->indices.size() << std::endl;
00537                         return false;
00538                 }
00539                 if (i->state == groupInterpolator::created){
00540                         double q[m_dof], dq[m_dof];
00541                         interpolators[Q]->get(q, dq, false);
00542                         std::map<std::string, groupInterpolator *>::iterator it;
00543                         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00544                                 groupInterpolator *gi = it->second;
00545                                 if (gi) gi->get(q, dq, false);
00546                         }
00547                         double x[i->indices.size()], v[i->indices.size()];
00548                         i->extract(x, q);
00549                         i->extract(v, dq);
00550                         i->inter->go(x,v,interpolators[Q]->deltaT());
00551                 }
00552                 const double *q=NULL; double t=0;
00553                 double *v = new double[len];
00554                 double *qi = new double[len];
00555                 for (unsigned int j=0; j<len; j++){
00556                         qi[j] = qInit[i->indices[j]];
00557                 }
00558                 for (unsigned int l=0; l<pos.size(); l++){
00559                         q = pos[l];
00560                         if (l < pos.size() - 1 ) {
00561                                 double t0, t1;
00562                                 if (tm.size() == pos.size()) {
00563                                         t0 = tm[l]; t1 = tm[l+1];
00564                                 } else {
00565                                         t0 = t1 = tm[0];
00566                                 }
00567                                 const double *q_next = pos[l+1];
00568                                 const double *q_prev = l==0 ? qi : pos[l-1];
00569                                 for (unsigned int j = 0; j < len; j++) {
00570                                         double d0, d1, v0, v1;
00571                                         d0 = (q[j] - q_prev[j]);
00572                                         d1 = (q_next[j] - q[j]);
00573                                         v0 = d0/t0;
00574                                         v1 = d1/t1;
00575                                         if ( v0 * v1 >= 0 ) {
00576                                                 v[j] = 0.5 * (v0 + v1);
00577                                         } else {
00578                                                 v[j] = 0;
00579                                         }
00580                                 }
00581                         } else {
00582                                 for (unsigned int j = 0; j < len; j++) { v[j] = 0.0; }
00583                         }
00584                         if (l < tm.size()) t = tm[l];
00585                         i->go(q, v, t);
00586                 }
00587                 sync();
00588                 delete [] v;
00589                 delete [] qi;
00590 
00591                 return true;
00592         }else{
00593                 std::cerr << "[playPatternOfGroup] group name " << gname << " is not installed" << std::endl;
00594                 return false;
00595         }
00596 }
00597 
00598 bool seqplay::setJointAnglesSequence(std::vector<const double*> pos, std::vector<double> tm)
00599 {
00600         // setJointAngles to override curren tgoal
00601         double x[m_dof], v[m_dof], a[m_dof];
00602         interpolators[Q]->get(x, v, a, false);
00603         interpolators[Q]->set(x, v);
00604         interpolators[Q]->clear();
00605         interpolators[Q]->push(x, v, a, true);
00606 
00607     const double *q=NULL;
00608     for (unsigned int i=0; i<pos.size(); i++){
00609         q = pos[i];
00610                 if (i < pos.size() - 1 ) {
00611                         double t0, t1;
00612                         if (tm.size() == pos.size()) {
00613                                 t0 = tm[i]; t1 = tm[i+1];
00614                         } else {
00615                                 t0 = t1 = tm[0];
00616                         }
00617                         const double *q_next = pos[i+1];
00618                         const double *q_prev = i==0?x:pos[i-1];
00619                         for (int j = 0; j < m_dof; j++) {
00620                                 double d0, d1, v0, v1;
00621                                 d0 = (q[j] - q_prev[j]);
00622                                 d1 = (q_next[j] - q[j]);
00623                                 v0 = d0/t0;
00624                                 v1 = d1/t1;
00625                                 if ( v0 * v1 >= 0 ) {
00626                                         v[j] = 0.5 * (v0 + v1);
00627                                 } else {
00628                                         v[j] = 0;
00629                                 }
00630                         }
00631                 } else {
00632                         for (int j = 0; j < m_dof; j++) { v[j] = 0.0; }
00633                 }
00634 
00635                 interpolators[Q]->setGoal(pos[i], v, tm[i], false);
00636                 do{
00637                         interpolators[Q]->interpolate(tm[i]);
00638                 }while(tm[i]>0);
00639                 sync();
00640         }
00641         return true;
00642 }
00643 
00644 bool seqplay::clearJointAngles()
00645 {
00646         // setJointAngles to override curren tgoal
00647         double x[m_dof], v[m_dof], a[m_dof];
00648         interpolators[Q]->get(x, v, a, false);
00649         interpolators[Q]->set(x, v);
00650         interpolators[Q]->clear();
00651         double tm = interpolators[Q]->deltaT();
00652         interpolators[Q]->setGoal(x, v, tm, false);
00653         do{
00654                 interpolators[Q]->interpolate(tm);
00655         }while(tm>0);
00656         sync();
00657         return true;
00658 }
00659 
00660 bool seqplay::setJointAnglesSequenceFull(std::vector<const double*> i_pos, std::vector<const double*> i_vel, std::vector<const double*> i_torques, std::vector<const double*> i_bpos, std::vector<const double*> i_brpy, std::vector<const double*> i_bacc,  std::vector<const double*> i_zmps, std::vector<const double*> i_wrenches, std::vector<const double*> i_optionals, std::vector<double> i_tm)
00661 {
00662         // setJointAngles to override curren tgoal
00663         double x[m_dof], v[m_dof], a[m_dof];
00664         interpolators[Q]->get(x, v, a, false);
00665         interpolators[Q]->set(x, v);
00666         interpolators[Q]->clear();
00667         interpolators[Q]->push(x, v, a, true);
00668         double torque[m_dof], dummy_dof[m_dof];
00669         for (int j = 0; j < m_dof; j++) { dummy_dof[j] = 0.0; }
00670         interpolators[TQ]->get(torque, false);
00671         interpolators[TQ]->set(torque);
00672         interpolators[TQ]->clear();
00673         interpolators[TQ]->push(torque, dummy_dof, dummy_dof, true);
00674         double bpos[3], brpy[3], bacc[3], dummy_3[3]={0,0,0};
00675         interpolators[P]->get(bpos, false);
00676         interpolators[P]->set(bpos);
00677         interpolators[P]->clear();
00678         interpolators[P]->push(bpos, dummy_3, dummy_3, true);
00679         interpolators[RPY]->get(brpy, false);
00680         interpolators[RPY]->set(brpy);
00681         interpolators[RPY]->clear();
00682         interpolators[RPY]->push(brpy, dummy_3, dummy_3, true);
00683         interpolators[ACC]->get(bacc, false);
00684         interpolators[ACC]->set(bacc);
00685         interpolators[ACC]->clear();
00686         interpolators[ACC]->push(bacc, dummy_3, dummy_3, true);
00687         int fnum = interpolators[WRENCHES]->dimension()/6, optional_data_dim = interpolators[OPTIONAL_DATA]->dimension();
00688         double zmp[3], wrench[6*fnum], dummy_fnum[6*fnum], optional[optional_data_dim], dummy_optional[optional_data_dim];
00689         for (int j = 0; j < 6*fnum; j++) { dummy_dof[j] = 0.0; }
00690         for (int j = 0; j < optional_data_dim; j++) { dummy_optional[j] = 0.0; }
00691         interpolators[ZMP]->get(zmp, false);
00692         interpolators[ZMP]->set(zmp);
00693         interpolators[ZMP]->clear();
00694         interpolators[ZMP]->push(zmp, dummy_3, dummy_3, true);
00695         interpolators[WRENCHES]->get(wrench, false);
00696         interpolators[WRENCHES]->set(wrench);
00697         interpolators[WRENCHES]->clear();
00698         interpolators[WRENCHES]->push(wrench, dummy_fnum, dummy_fnum, true);
00699         interpolators[OPTIONAL_DATA]->get(optional, false);
00700         interpolators[OPTIONAL_DATA]->set(optional);
00701         interpolators[OPTIONAL_DATA]->clear();
00702         interpolators[OPTIONAL_DATA]->push(optional, dummy_optional, dummy_optional, true);
00703 
00704     const double *q=NULL;
00705     for (unsigned int i=0; i<i_pos.size(); i++){
00706                 if (i_vel.size() > 0 ) {
00707                         for (int j = 0; j < m_dof; j++) {
00708                                 v[j] = i_vel[i][j];
00709                         }
00710                 }else{
00711                         q = i_pos[i];
00712                         if (i < i_pos.size() - 1 ) {
00713                                 double t0, t1;
00714                                 if (i_tm.size() == i_pos.size()) {
00715                                         t0 = i_tm[i]; t1 = i_tm[i+1];
00716                                 } else {
00717                                         t0 = t1 = i_tm[0];
00718                                 }
00719                                 const double *q_next = i_pos[i+1];
00720                                 const double *q_prev = i==0?x:i_pos[i-1];
00721                                 for (int j = 0; j < m_dof; j++) {
00722                                         double d0, d1, v0, v1;
00723                                         d0 = (q[j] - q_prev[j]);
00724                                         d1 = (q_next[j] - q[j]);
00725                                         v0 = d0/t0;
00726                                         v1 = d1/t1;
00727                                         if ( v0 * v1 >= 0 ) {
00728                                                 v[j] = 0.5 * (v0 + v1);
00729                                         } else {
00730                                                 v[j] = 0;
00731                                         }
00732                                 }
00733                         } else {
00734                                 for (int j = 0; j < m_dof; j++) { v[j] = 0.0; }
00735                         }
00736                 }
00737 
00738                 interpolators[Q]->setGoal(i_pos[i], v, i_tm[i], false);
00739                 interpolators[TQ]->setGoal(i_torques[i], i_tm[i], false);
00740                 interpolators[P]->setGoal(i_bpos[i], i_tm[i], false);
00741                 interpolators[RPY]->setGoal(i_brpy[i], i_tm[i], false);
00742                 interpolators[ACC]->setGoal(i_bacc[i], i_tm[i], false);
00743                 interpolators[ZMP]->setGoal(i_zmps[i], i_tm[i], false);
00744                 interpolators[WRENCHES]->setGoal(i_wrenches[i], i_tm[i], false);
00745                 interpolators[OPTIONAL_DATA]->setGoal(i_optionals[i], i_tm[i], false);
00746                 do{
00747                         double tm = i_tm[i], tm_tmp;
00748                         interpolators[Q]->interpolate(i_tm[i]);
00749                         tm_tmp = tm; interpolators[TQ]->interpolate(tm_tmp);
00750                         tm_tmp = tm; interpolators[P]->interpolate(tm_tmp);
00751                         tm_tmp = tm; interpolators[RPY]->interpolate(tm_tmp);
00752                         tm_tmp = tm; interpolators[ACC]->interpolate(tm_tmp);
00753                         tm_tmp = tm; interpolators[ZMP]->interpolate(tm_tmp);
00754                         tm_tmp = tm; interpolators[WRENCHES]->interpolate(tm_tmp);
00755                         tm_tmp = tm; interpolators[OPTIONAL_DATA]->interpolate(tm_tmp);
00756                 }while(i_tm[i]>0);
00757                 sync();
00758         }
00759         return true;
00760 }
00761 
00762 bool seqplay::setJointAnglesSequenceOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm, const size_t pos_size)
00763 {
00764         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00765         groupInterpolator *i = groupInterpolators[gname];
00766 
00767         if (! i){
00768                 std::cerr << "[setJointAnglesSequenceOfGroup] group name " << gname << " is not installed" << std::endl;
00769                 return false;
00770         }
00771         if (pos_size != i->indices.size() ) {
00772                 std::cerr << "[setJointAnglesSequenceOfGroup] group name " << gname << " : size of manipulater is not equal to input. " << pos_size << " /= " << i->indices.size() << std::endl;
00773                 return false;
00774         }
00775         int len = i->indices.size();
00776         double x[len], v[len];
00777         double q[m_dof], dq[m_dof];
00778         i->inter->get(q, dq, false);
00779         i->inter->set(q, dq);
00780         i->extract(x, q);
00781         i->extract(v, dq);
00782         i->inter->clear();
00783     const double *q_curr=NULL;
00784     for (unsigned int j=0; j<pos.size(); j++){
00785         q_curr = pos[j];
00786                 if ( j < pos.size() - 1 ) {
00787                         double t0, t1;
00788                         if (tm.size() == pos.size()) {
00789                                 t0 = tm[j]; t1 = tm[j+1];
00790                         } else {
00791                                 t0 = t1 = tm[0];
00792                         }
00793                         const double *q_next = pos[j+1];
00794                         const double *q_prev = j==0?x:pos[j-1];
00795                         for (int k = 0; k < len; k++) {
00796                                 double d0, d1, v0, v1;
00797                                 d0 = (q_curr[k] - q_prev[k]);
00798                                 d1 = (q_next[k] - q_curr[k]);
00799                                 v0 = d0/t0;
00800                                 v1 = d1/t1;
00801                                 if ( v0 * v1 >= 0 ) {
00802                                         v[k] = 0.5 * (v0 + v1);
00803                                 } else {
00804                                         v[k] = 0;
00805                                 }
00806                         }
00807                 } else {
00808                         for (int k = 0; k < len; k++) { v[k] = 0.0; }
00809                 }
00810                 if (i->state == groupInterpolator::created){
00811                         interpolators[Q]->get(q, dq, false);
00812                         std::map<std::string, groupInterpolator *>::iterator it;
00813                         for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){
00814                                 groupInterpolator *gi = it->second;
00815                                 if (gi) gi->get(q, dq, false);
00816                         }
00817                         i->extract(x, q);
00818                         i->extract(v, dq);
00819                         i->inter->go(x,v,interpolators[Q]->deltaT());
00820                 }
00821                 i->inter->setGoal(pos[j], v, tm[j], false);
00822                 do{
00823                         i->inter->interpolate(tm[j]);
00824                 }while(tm[j]>0);
00825                 i->inter->sync();
00826                 i->state = groupInterpolator::working;
00827         }
00828         return true;
00829 }
00830 
00831 bool seqplay::clearJointAnglesOfGroup(const char *gname)
00832 {
00833         char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
00834         groupInterpolator *i = groupInterpolators[gname];
00835 
00836         if (! i){
00837                 std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is not installed" << std::endl;
00838                 return false;
00839         }
00840 
00841         if (i->state == groupInterpolator::created){
00842                 std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is not created" << std::endl;
00843                 return false;
00844         }
00845 
00846         if (i->state == groupInterpolator::removing || i->state == groupInterpolator::removed){
00847                 std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is removing" << std::endl;
00848                 return false;
00849         }
00850 
00851         int len = i->indices.size();
00852         double x[len], v[len], a[len];
00853         i->inter->get(x, v, a, false);
00854         i->inter->set(x, v);
00855         while(i->inter->remain_time() > 0){
00856                 i->inter->pop();
00857         }
00858         double tm = interpolators[Q]->deltaT();
00859         i->inter->setGoal(x, v, tm, true);// true: update remian_t
00860         do{
00861                 i->inter->interpolate(tm);
00862         }while(tm>0);
00863         i->inter->sync();
00864 
00865         return true;
00866 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56