add clearJointAngles and clearJointAnglesOfGroup()

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);
   };
 };

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 ) {

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);

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);

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);

rtc/SequencePlayer/seqplay.cpp

@@ -628,6 +628,22 @@ bool seqplay::setJointAnglesSequence(std::vector<const double*> 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<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)
 {
 	// setJointAngles to override curren tgoal
@@ -794,3 +810,45 @@ bool seqplay::setJointAnglesSequenceOfGroup(const char *gname, std::vector<const
 	}
 	return true;
 }
+
+bool seqplay::clearJointAnglesOfGroup(const char *gname)
+{
+	char *s = (char *)gname; while(*s) {*s=toupper(*s);s++;}
+	groupInterpolator *i = groupInterpolators[gname];
+
+	if (! i){
+		std::cerr << "[clearJointAnglesOfGroup] group name " << gname << " is not installed" << std::endl;
+		return false;
+	}
+
+	if (i->state == 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;
+}

rtc/SequencePlayer/seqplay.h

@@ -40,6 +40,8 @@ class seqplay
     bool setJointAnglesSequence(std::vector<const double*> pos, std::vector<double> tm);
     bool setJointAnglesSequenceOfGroup(const char *gname, std::vector<const double*> pos, std::vector<double> tm);
     bool setJointAnglesSequenceFull(std::vector<const double*> pos, std::vector<const double*> vel, std::vector<const double*> torques, std::vector<const double*> bpos, std::vector<const double*> brpy, std::vector<const double*> bacc, std::vector<const double*> zmps, std::vector<const double*> wrenches, std::vector<const double*> optionals, std::vector<double> 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);