SequencePlayerService_impl.cpp
Go to the documentation of this file.
00001 #include "SequencePlayerService_impl.h"
00002 #include "SequencePlayer.h"
00003 #include <hrpModel/Body.h>
00004 #include <hrpModel/Link.h>
00005 
00006 SequencePlayerService_impl::SequencePlayerService_impl() : m_player(NULL)
00007 {
00008 }
00009 
00010 SequencePlayerService_impl::~SequencePlayerService_impl()
00011 {
00012 }
00013 
00014 void SequencePlayerService_impl::waitInterpolation()
00015 {
00016   m_player->waitInterpolation();
00017 }
00018 
00019 CORBA::Boolean SequencePlayerService_impl::waitInterpolationOfGroup(const char *gname)
00020 {
00021     return m_player->waitInterpolationOfGroup(gname);
00022 }
00023 
00024 CORBA::Boolean SequencePlayerService_impl::setJointAngles(const dSequence& jvs, CORBA::Double tm)
00025 {
00026   if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
00027       std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00028       return false;
00029   }
00030   return m_player->setJointAngles(jvs.get_buffer(), tm);
00031 }
00032 
00033 CORBA::Boolean SequencePlayerService_impl::setJointAnglesWithMask(const dSequence& jvs, const bSequence& mask, CORBA::Double tm)
00034 {
00035     if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())
00036         || mask.length() != (unsigned int)(m_player->robot()->numJoints())) {
00037         std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", mask:" << mask.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00038         return false;
00039     }
00040     return m_player->setJointAngles(jvs.get_buffer(), mask.get_buffer(), tm);
00041 }
00042 
00043 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequence(const dSequenceSequence& jvss, const dSequence& tms)
00044 {
00045   const OpenHRP::bSequence mask;
00046   return setJointAnglesSequenceWithMask(jvss, mask, tms);
00047 }
00048 
00049 CORBA::Boolean SequencePlayerService_impl::clearJointAngles()
00050 {
00051   return m_player->clearJointAngles();
00052 }
00053 
00054 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceWithMask(const dSequenceSequence& jvss, const bSequence& mask, const dSequence& tms)
00055 {
00056   if (jvss.length() <= 0) {
00057       std::cerr << __PRETTY_FUNCTION__ << " num of joint angles sequence is invalid:" << jvss.length() << " > 0" << std::endl;
00058       return false;
00059   }
00060   if (jvss.length() != tms.length()) {
00061       std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
00062       return false;
00063   }
00064   const dSequence& jvs = jvss[0];
00065   if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
00066       std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00067       return false;
00068   }
00069 
00070   if (mask.length() > 0 && mask.length() != (unsigned int)(m_player->robot()->numJoints())) {
00071       std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", mask:" << mask.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00072       return false;
00073   }
00074   
00075   return m_player->setJointAnglesSequence(jvss, mask, tms);
00076 }
00077 
00078 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceFull(const dSequenceSequence& jvss, const dSequenceSequence& vels, const dSequenceSequence& torques, const dSequenceSequence& poss, const dSequenceSequence& rpys, const dSequenceSequence& accs, const dSequenceSequence& zmps, const dSequenceSequence& wrenches, const dSequenceSequence& optionals, const dSequence &tms)
00079 {
00080   if (jvss.length() <= 0) {
00081       std::cerr << __PRETTY_FUNCTION__ << " num of joint angles sequence is invalid:" << jvss.length() << " > 0" << std::endl;
00082       return false;
00083   }
00084   if (jvss.length() != tms.length()) {
00085       std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
00086       return false;
00087   }
00088   const dSequence& jvs = jvss[0];
00089   if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
00090       std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00091       return false;
00092   }
00093   if (vels.length() > 0 ) {
00094       const dSequence& vel = vels[0];
00095       if (vels.length() != tms.length()) {
00096           std::cerr << __PRETTY_FUNCTION__ << " length of joint velocitys sequence and time sequence differ, joint velocity:" << vels.length() << ", time:" << tms.length() << std::endl;
00097           return false;
00098       }
00099       if ( vel.length() != (unsigned int)(m_player->robot()->numJoints())) {
00100           std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", vel:" << vel.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00101           return false;
00102       }
00103   }
00104   if (torques.length() > 0 ) {
00105       const dSequence& torque = torques[0];
00106       if (torques.length() != tms.length()) {
00107           std::cerr << __PRETTY_FUNCTION__ << " length of joint torque sequence and time sequence differ, joint torque:" << torques.length() << ", time:" << tms.length() << std::endl;
00108           return false;
00109       }
00110       if ( torque.length() != (unsigned int)(m_player->robot()->numJoints())) {
00111           std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", torque:" << torque.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00112           return false;
00113       }
00114   }
00115 
00116   if (poss.length() > 0) {
00117       const dSequence& pos = poss[0];
00118       if (poss.length() != tms.length()) {
00119           std::cerr << __PRETTY_FUNCTION__ << " length of base pos sequence and time sequence differ, pos:" << poss.length() << ", time:" << tms.length() << std::endl;
00120           return false;
00121       }
00122       if ( pos.length() != 3) {
00123           std::cerr << __PRETTY_FUNCTION__ << " num of base pos is differ, pos:" << pos.length() << std::endl;
00124           return false;
00125       }
00126   }
00127 
00128   if (rpys.length() > 0) {
00129       const dSequence& rpy = rpys[0];
00130       if (rpys.length() != tms.length()) {
00131           std::cerr << __PRETTY_FUNCTION__ << " length of base rpy sequence and time sequence differ, rpy:" << rpys.length() << ", time:" << tms.length() << std::endl;
00132           return false;
00133       }
00134       if ( rpy.length() != 3) {
00135           std::cerr << __PRETTY_FUNCTION__ << " num of base rpy is differ, rpy:" << rpy.length() << std::endl;
00136           return false;
00137       }
00138   }
00139 
00140   if (accs.length() > 0 ) {
00141       const dSequence& acc = accs[0];
00142       if (accs.length() != tms.length()) {
00143           std::cerr << __PRETTY_FUNCTION__ << " length of joint accocitys sequence and time sequence differ, joint accocity:" << accs.length() << ", time:" << tms.length() << std::endl;
00144           return false;
00145       }
00146       if ( acc.length() != 3) {
00147           std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", acc:" << acc.length() << ", robot" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
00148           return false;
00149       }
00150   }
00151 
00152   if (zmps.length() > 0) {
00153       const dSequence& zmp = zmps[0];
00154       if (zmps.length() != tms.length()) {
00155           std::cerr << __PRETTY_FUNCTION__ << " length of zmp sequence and time sequence differ, zmp:" << zmps.length() << ", time:" << tms.length() << std::endl;
00156           return false;
00157       }
00158       if ( zmp.length() != 3) {
00159           std::cerr << __PRETTY_FUNCTION__ << " num of zmp is differ, zmp:" << zmp.length() <<  std::endl;
00160           return false;
00161       }
00162   }
00163 
00164   if (wrenches.length() > 0) {
00165       if (wrenches.length() != tms.length()) {
00166           std::cerr << __PRETTY_FUNCTION__ << " length of wrench sequence and time sequence differ, wrench:" << wrenches.length() << ", time:" << tms.length() << std::endl;
00167           return false;
00168       }
00169       // need to check size of wrench
00170       //const dSequence& wrench = wrenches[0];
00171   }
00172 
00173   if (optionals.length() > 0) {
00174       if (optionals.length() != tms.length()) {
00175           std::cerr << __PRETTY_FUNCTION__ << " length of optional sequence and time sequence differ, optional:" << optionals.length() << ", time:" << tms.length() << std::endl;
00176           return false;
00177       }
00178       // need to check size of optional
00179       //const dSequence& optional = optionasl[0];
00180   }
00181 
00182   return m_player->setJointAnglesSequenceFull(jvss, vels, torques, poss, rpys, accs, zmps, wrenches, optionals, tms);
00183 }
00184 
00185 CORBA::Boolean SequencePlayerService_impl::setJointAngle(const char *jname, CORBA::Double jv, CORBA::Double tm)
00186 {
00187     BodyPtr r = m_player->robot();
00188     Link *l = r->link(jname);
00189     if (!l){
00190         std::cerr << "can't find(" << jname << ")" << std::endl;
00191         return false;
00192     }
00193     int id = l->jointId;
00194     return m_player->setJointAngle(id, jv, tm);
00195 }
00196 
00197 CORBA::Boolean SequencePlayerService_impl::setBasePos(const dSequence& pos, CORBA::Double tm)
00198 {
00199     if (pos.length() != 3) return false;
00200 
00201     return m_player->setBasePos(pos.get_buffer(), tm);
00202 }
00203 
00204 CORBA::Boolean SequencePlayerService_impl::setBaseRpy(const dSequence& rpy, CORBA::Double tm)
00205 {
00206     if (rpy.length() != 3) return false;
00207 
00208     return m_player->setBaseRpy(rpy.get_buffer(), tm);
00209 }
00210 
00211 CORBA::Boolean SequencePlayerService_impl::setZmp(const dSequence& zmp, CORBA::Double tm)
00212 {
00213     if (zmp.length() != 3) return false;
00214 
00215     return m_player->setZmp(zmp.get_buffer(), tm);
00216 }
00217 
00218 CORBA::Boolean SequencePlayerService_impl::setWrenches(const dSequence& wrenches, CORBA::Double tm)
00219 {
00220   //if (wrenches.length() != ) return false;
00221 
00222     return m_player->setWrenches(wrenches.get_buffer(), tm);
00223 }
00224 
00225 CORBA::Boolean SequencePlayerService_impl::setTargetPose(const char* gname, const dSequence& xyz, const dSequence& rpy, CORBA::Double tm){
00226     char* frame_name = (char *)strrchr(gname, ':');
00227     if ( frame_name ) {
00228         ((char *)gname)[frame_name - gname] = '\0'; // cut frame_name, gname[strpos(':')] = 0x00
00229         frame_name++; // skip ":"
00230     }
00231     return m_player->setTargetPose(gname, xyz.get_buffer(), rpy.get_buffer(), tm, frame_name);
00232 }
00233 
00234 CORBA::Boolean SequencePlayerService_impl::isEmpty()
00235 {
00236   return m_player->player()->isEmpty();
00237 }
00238 
00239 void SequencePlayerService_impl::loadPattern(const char* basename, CORBA::Double tm)
00240 {
00241   if (!m_player->player()){
00242     std::cerr << "player is not set"<< std::endl;
00243     return;
00244   }
00245   m_player->loadPattern(basename, tm);
00246 }
00247 
00248 void SequencePlayerService_impl::clear()
00249 {
00250   m_player->player()->clear();
00251 }
00252 
00253 CORBA::Boolean  SequencePlayerService_impl::clearOfGroup(const char *gname, CORBA::Double i_limitation)
00254 {
00255     m_player->player()->clearOfGroup(gname, i_limitation);
00256     return true;
00257 }
00258 
00259 void SequencePlayerService_impl::clearNoWait()
00260 {
00261   m_player->setClearFlag();
00262 }
00263 
00264 CORBA::Boolean SequencePlayerService_impl::setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_)
00265 {
00266   return m_player->setInterpolationMode(i_mode_);
00267 }
00268 
00269 CORBA::Boolean SequencePlayerService_impl::setInitialState()
00270 {
00271   m_player->setInitialState();
00272   return m_player->setInitialState(m_player->dt);
00273 }
00274 
00275 void SequencePlayerService_impl::player(SequencePlayer *i_player)
00276 {
00277   m_player = i_player;
00278 } 
00279 
00280 void SequencePlayerService_impl::playPattern(const dSequenceSequence& pos, const dSequenceSequence& rpy, const dSequenceSequence& zmp, const dSequence& tm)
00281 {
00282     m_player->playPattern(pos, rpy, zmp, tm);
00283 }
00284 
00285 CORBA::Boolean SequencePlayerService_impl::addJointGroup(const char* gname, const OpenHRP::SequencePlayerService::StrSequence& jnames)
00286 {
00287     return m_player->addJointGroup(gname, jnames);
00288 }
00289 
00290 CORBA::Boolean SequencePlayerService_impl::removeJointGroup(const char* gname)
00291 {
00292     return m_player->removeJointGroup(gname);
00293 }
00294 
00295 CORBA::Boolean SequencePlayerService_impl::setJointAnglesOfGroup(const char *gname, const dSequence& jvs, CORBA::Double tm)
00296 {
00297     return m_player->setJointAnglesOfGroup(gname, jvs, tm);
00298 }
00299 
00300 CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceOfGroup(const char *gname, const dSequenceSequence& jvss, const dSequence& tms)
00301 {
00302     if (jvss.length() != tms.length()) {
00303         std::cerr << __PRETTY_FUNCTION__ << " length of joint angles sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
00304         return false;
00305     }
00306     return m_player->setJointAnglesSequenceOfGroup(gname, jvss, tms);
00307 }
00308 
00309 CORBA::Boolean SequencePlayerService_impl::clearJointAnglesOfGroup(const char *gname)
00310 {
00311     return m_player->clearJointAnglesOfGroup(gname);
00312 }
00313 
00314 CORBA::Boolean SequencePlayerService_impl::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm)
00315 {
00316     return m_player->playPatternOfGroup(gname, pos, tm);
00317 }
00318 
00319 void SequencePlayerService_impl::setMaxIKError(CORBA::Double pos, CORBA::Double rot)
00320 {
00321     return m_player->setMaxIKError(pos, rot);
00322 }
00323 
00324 void SequencePlayerService_impl::setMaxIKIteration(CORBA::Short iter)
00325 {
00326     return m_player->setMaxIKIteration(iter);
00327 }


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed May 15 2019 05:02:19