From 0634feb79b6b3de109e44481efe751be78bf8745 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sun, 24 May 2015 23:42:59 +0900 Subject: [PATCH] add clearJointAngles and clearJointAnglesOfGroup() --- idl/SequencePlayerService.idl | 12 ++++ rtc/SequencePlayer/SequencePlayer.cpp | 25 ++++++++ rtc/SequencePlayer/SequencePlayer.h | 2 + .../SequencePlayerService_impl.cpp | 10 ++++ .../SequencePlayerService_impl.h | 2 + rtc/SequencePlayer/seqplay.cpp | 58 +++++++++++++++++++ rtc/SequencePlayer/seqplay.h | 2 + 7 files changed, 111 insertions(+) diff --git a/idl/SequencePlayerService.idl b/idl/SequencePlayerService.idl index 82bcc5948b4..b6a67355e52 100644 --- a/idl/SequencePlayerService.idl +++ b/idl/SequencePlayerService.idl @@ -240,5 +240,17 @@ module OpenHRP * @return true joint angles are set successfully, false otherwise */ boolean setJointAnglesSequenceFull(in dSequenceSequence jvss, in dSequenceSequence vels, in dSequenceSequence torques, in dSequenceSequence poss, in dSequenceSequence rpys, in dSequenceSequence accs, in dSequenceSequence zmps, in dSequenceSequence wrenchs, in dSequenceSequence optionals, in dSequence tms); + + /** + * @brief clear current JointAngles + * @return true joint angles are set successfully, false otherwise + */ + boolean clearJointAngles(); + /** + * @brief clear current JointAnglesOfGroup + * @param gname name of the joint group + * @return true joint angles are set successfully, false otherwise + */ + boolean clearJointAnglesOfGroup(in string gname); }; }; diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp index 2b5fe21ae00..809f854844d 100644 --- a/rtc/SequencePlayer/SequencePlayer.cpp +++ b/rtc/SequencePlayer/SequencePlayer.cpp @@ -439,6 +439,18 @@ bool SequencePlayer::setJointAnglesSequence(const OpenHRP::dSequenceSequence ang return m_seq->setJointAnglesSequence(v_poss, v_tms); } +bool SequencePlayer::clearJointAngles() +{ + if ( m_debugLevel > 0 ) { + std::cerr << __PRETTY_FUNCTION__ << std::endl; + } + Guard guard(m_mutex); + + if (!setInitialState()) return false; + + return m_seq->clearJointAngles(); +} + bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times) { if ( m_debugLevel > 0 ) { @@ -456,6 +468,19 @@ bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const Open return m_seq->setJointAnglesSequenceOfGroup(gname, v_poss, v_tms); } +bool SequencePlayer::clearJointAnglesOfGroup(const char *gname) +{ + if ( m_debugLevel > 0 ) { + std::cerr << __PRETTY_FUNCTION__ << std::endl; + } + Guard guard(m_mutex); + if (!setInitialState()) return false; + + if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false; + + return m_seq->clearJointAnglesOfGroup(gname); +} + bool SequencePlayer::setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms) { if ( m_debugLevel > 0 ) { diff --git a/rtc/SequencePlayer/SequencePlayer.h b/rtc/SequencePlayer/SequencePlayer.h index 62389f617bb..69cf55b6340 100644 --- a/rtc/SequencePlayer/SequencePlayer.h +++ b/rtc/SequencePlayer/SequencePlayer.h @@ -101,6 +101,7 @@ class SequencePlayer bool setJointAngles(const double *angles, const bool *mask, double tm); bool setJointAnglesSequence(const OpenHRP::dSequenceSequence angless, const OpenHRP::bSequence& mask, const OpenHRP::dSequence& times); bool setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence i_jvss, const OpenHRP::dSequenceSequence i_vels, const OpenHRP::dSequenceSequence i_torques, const OpenHRP::dSequenceSequence i_poss, const OpenHRP::dSequenceSequence i_rpys, const OpenHRP::dSequenceSequence i_accs, const OpenHRP::dSequenceSequence i_zmps, const OpenHRP::dSequenceSequence i_wrenches, const OpenHRP::dSequenceSequence i_optionals, const dSequence i_tms); + bool clearJointAngles(); bool setBasePos(const double *pos, double tm); bool setBaseRpy(const double *rpy, double tm); bool setZmp(const double *zmp, double tm); @@ -114,6 +115,7 @@ class SequencePlayer bool removeJointGroup(const char *gname); bool setJointAnglesOfGroup(const char *gname, const double *angles, double tm); bool setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times); + bool clearJointAnglesOfGroup(const char *gname); bool playPatternOfGroup(const char *gname, const OpenHRP::dSequenceSequence& pos, const OpenHRP::dSequence& tm); void setMaxIKError(double pos, double rot); diff --git a/rtc/SequencePlayer/SequencePlayerService_impl.cpp b/rtc/SequencePlayer/SequencePlayerService_impl.cpp index c083738e1f5..3e2fd6c54b0 100644 --- a/rtc/SequencePlayer/SequencePlayerService_impl.cpp +++ b/rtc/SequencePlayer/SequencePlayerService_impl.cpp @@ -46,6 +46,11 @@ CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequence(const dSequenc return setJointAnglesSequenceWithMask(jvss, mask, tms); } +CORBA::Boolean SequencePlayerService_impl::clearJointAngles() +{ + return m_player->clearJointAngles(); +} + CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceWithMask(const dSequenceSequence& jvss, const bSequence& mask, const dSequence& tms) { if (jvss.length() <= 0) { @@ -300,6 +305,11 @@ CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceOfGroup(const c return m_player->setJointAnglesSequenceOfGroup(gname, jvss, tms); } +CORBA::Boolean SequencePlayerService_impl::clearJointAnglesOfGroup(const char *gname) +{ + return m_player->clearJointAnglesOfGroup(gname); +} + CORBA::Boolean SequencePlayerService_impl::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm) { return m_player->playPatternOfGroup(gname, pos, tm); diff --git a/rtc/SequencePlayer/SequencePlayerService_impl.h b/rtc/SequencePlayer/SequencePlayerService_impl.h index bb107529765..5576ba8d54e 100644 --- a/rtc/SequencePlayer/SequencePlayerService_impl.h +++ b/rtc/SequencePlayer/SequencePlayerService_impl.h @@ -21,6 +21,7 @@ class SequencePlayerService_impl CORBA::Boolean setJointAnglesSequence(const dSequenceSequence& jvs, const dSequence &tms); CORBA::Boolean setJointAnglesSequenceWithMask(const dSequenceSequence& jvs, const bSequence& mask, const dSequence &tms); CORBA::Boolean 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); + CORBA::Boolean clearJointAngles(); CORBA::Boolean setJointAngles(const dSequence& jvs, CORBA::Double tm); CORBA::Boolean setJointAnglesWithMask(const dSequence& jvs, const bSequence& mask, CORBA::Double tm); CORBA::Boolean setJointAngle(const char *jname, CORBA::Double jv, CORBA::Double tm); @@ -40,6 +41,7 @@ class SequencePlayerService_impl CORBA::Boolean removeJointGroup(const char* gname); CORBA::Boolean setJointAnglesOfGroup(const char *gname, const dSequence& jvs, CORBA::Double tm); CORBA::Boolean setJointAnglesSequenceOfGroup(const char *gname, const dSequenceSequence& jvs, const dSequence &tms); + CORBA::Boolean clearJointAnglesOfGroup(const char *gname); CORBA::Boolean clearOfGroup(const char *gname, CORBA::Double i_timelimit); CORBA::Boolean playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm); void setMaxIKError(CORBA::Double pos, CORBA::Double rot); diff --git a/rtc/SequencePlayer/seqplay.cpp b/rtc/SequencePlayer/seqplay.cpp index dd1b4cd73d1..3a3a064b60c 100644 --- a/rtc/SequencePlayer/seqplay.cpp +++ b/rtc/SequencePlayer/seqplay.cpp @@ -628,6 +628,22 @@ bool seqplay::setJointAnglesSequence(std::vector pos, std::vector return true; } +bool seqplay::clearJointAngles() +{ + // setJointAngles to override curren tgoal + double x[m_dof], v[m_dof], a[m_dof]; + interpolators[Q]->get(x, v, a, false); + interpolators[Q]->set(x, v); + interpolators[Q]->clear(); + double tm = interpolators[Q]->deltaT(); + interpolators[Q]->setGoal(x, v, tm, false); + do{ + interpolators[Q]->interpolate(tm); + }while(tm>0); + sync(); + return true; +} + bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std::vector i_vel, std::vector i_torques, std::vector i_bpos, std::vector i_brpy, std::vector i_bacc, std::vector i_zmps, std::vector i_wrenches, std::vector i_optionals, std::vector i_tm) { // setJointAngles to override curren tgoal @@ -794,3 +810,45 @@ bool seqplay::setJointAnglesSequenceOfGroup(const char *gname, std::vectorstate == groupInterpolator::created){ + std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is not created" << std::endl; + return false; + } + + if (i->state == groupInterpolator::removing || i->state == groupInterpolator::removed){ + std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is removing" << std::endl; + return false; + } + + int len = i->indices.size(); + double x[len], v[len], a[len]; + i->inter->get(x, v, a, false); + i->inter->set(x, v); + //i->inter->clear(); // somehow isEmpty did not true if length is 0, remain_t has soeme value + while(i->inter->remain_time() > 0){ + i->inter->pop(); + } + std::cerr << __PRETTY_FUNCTION__ << " isEmpty() " << i->inter->isEmpty() << std::endl; + for(int i = 0; i < len; i++ ){ + std::cerr << x[i]*180/M_PI << " "; + } + double tm = interpolators[Q]->deltaT(); + i->inter->setGoal(x, v, tm, false); + do{ + i->inter->interpolate(tm); + }while(tm>0); + i->inter->sync(); + + return true; +} diff --git a/rtc/SequencePlayer/seqplay.h b/rtc/SequencePlayer/seqplay.h index 4cbb0e3122a..ebee37228b6 100644 --- a/rtc/SequencePlayer/seqplay.h +++ b/rtc/SequencePlayer/seqplay.h @@ -40,6 +40,8 @@ class seqplay bool setJointAnglesSequence(std::vector pos, std::vector tm); bool setJointAnglesSequenceOfGroup(const char *gname, std::vector pos, std::vector tm); bool setJointAnglesSequenceFull(std::vector pos, std::vector vel, std::vector torques, std::vector bpos, std::vector brpy, std::vector bacc, std::vector zmps, std::vector wrenches, std::vector optionals, std::vector tm); + bool clearJointAngles(); + bool clearJointAnglesOfGroup(const char *gname); // void setJointAngle(unsigned int i_rank, double jv, double tm); void loadPattern(const char *i_basename, double i_tm);