Add setWrench function for SequencePlayer. Checked with 315.1.10 comp…

…atibility.

idl/SequencePlayerService.idl

@@ -198,5 +198,14 @@ module OpenHRP
      * @return true if set successfully, false otherwise
      */
     boolean setWrenches(in dSequence wrenches, in double tm);
+
+    /**
+     * @brief Interpolate a Wrench. Function returns without waiting for interpolation to finish
+     * @param name Name of interpolated wrench
+     * @param wrench Wrench
+     * @param tm duration
+     * @return true if set successfully, false otherwise
+     */
+    boolean setWrench(in string name, in dSequence wrench, in double tm);
   };
 };

rtc/SequencePlayer/SequencePlayer.cpp

@@ -436,6 +436,24 @@ bool SequencePlayer::setWrenches(const double *wrenches, double tm)
     return true;
 }
 
+bool SequencePlayer::setWrench(const char* name, const double *wrench, double tm)
+{
+    Guard guard(m_mutex);
+    int idx = -1;
+    for (int i = 0; i < m_wrenches.size(); i++) {
+      hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i);
+      if (std::string(s->name) == std::string(name)) {
+        idx = i;
+      }
+    }
+    if (idx == -1) {
+      std::cerr << "[setWrench] " << name << " does not exist" << std::endl;
+      return false;
+    }
+    m_seq->setWrench(idx, wrench, tm);
+    return true;
+}
+
 bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name)
 {
     if ( m_debugLevel > 0 ) {

rtc/SequencePlayer/SequencePlayer.h

@@ -104,6 +104,7 @@ class SequencePlayer
   bool setZmp(const double *zmp, double tm);
   bool setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name);
   bool setWrenches(const double *wrenches, double tm);
+  bool setWrench(const char* name, const double *wrench, double tm);
   void loadPattern(const char *basename, double time); 
   void playPattern(const OpenHRP::dSequenceSequence& pos, const OpenHRP::dSequenceSequence& rpy, const OpenHRP::dSequenceSequence& zmp, const OpenHRP::dSequence& tm);
   bool setInterpolationMode(OpenHRP::SequencePlayerService::interpolationMode i_mode_);

rtc/SequencePlayer/SequencePlayerService_impl.cpp

@@ -81,6 +81,11 @@ CORBA::Boolean SequencePlayerService_impl::setWrenches(const dSequence& wrenches
     return m_player->setWrenches(wrenches.get_buffer(), tm);
 }
 
+CORBA::Boolean SequencePlayerService_impl::setWrench(const char* name, const dSequence& wrench, CORBA::Double tm)
+{
+    return m_player->setWrench(name, wrench.get_buffer(), tm);
+}
+
 CORBA::Boolean SequencePlayerService_impl::setTargetPose(const char* gname, const dSequence& xyz, const dSequence& rpy, CORBA::Double tm){
     char* frame_name = (char *)strrchr(gname, ':');
     if ( frame_name ) {

rtc/SequencePlayer/SequencePlayerService_impl.h

@@ -25,6 +25,7 @@ class SequencePlayerService_impl
   CORBA::Boolean setBaseRpy(const dSequence& rpy, CORBA::Double tm);
   CORBA::Boolean setZmp(const dSequence& zmp, CORBA::Double tm);
   CORBA::Boolean setWrenches(const dSequence& wrenches, CORBA::Double tm);
+  CORBA::Boolean setWrench(const char* name, const dSequence& wrench, CORBA::Double tm);
   CORBA::Boolean setTargetPose(const char* gname, const dSequence& xyz, const dSequence& rpy, CORBA::Double tm);
   CORBA::Boolean isEmpty();
   void loadPattern(const char* basename, CORBA::Double tm);

rtc/SequencePlayer/seqplay.cpp

@@ -8,14 +8,19 @@
 
 seqplay::seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum, unsigned int optional_data_dim) : m_dof(i_dof)
 {
+	NINTERPOLATOR = NINTERPOLATOR_WITHOUT_WRENCHES+i_fnum-1;
+	force_sensor_num = i_fnum;
+	interpolators.resize(NINTERPOLATOR);
     interpolators[Q] = new interpolator(i_dof, i_dt);
     interpolators[ZMP] = new interpolator(3, i_dt);
     interpolators[ACC] = new interpolator(3, i_dt);
     interpolators[P] = new interpolator(3, i_dt);
     interpolators[RPY] = new interpolator(3, i_dt);
     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);
+	for (size_t i = 0; i < i_fnum; i++) {
+		interpolators[WRENCHES+i] = new interpolator(6 , i_dt, interpolator::HOFFARBIB, 100); // wrenches = 6 * [number of force sensors]
+	}
     //
 
 #ifdef WAIST_HEIGHT
@@ -28,9 +33,11 @@ seqplay::seqplay(unsigned int i_dof, double i_dt, unsigned int i_fnum, unsigned
     double initial_zmp[] = {0,0,0};
 #endif
     interpolators[ZMP]->set(initial_zmp);
-    double initial_wrenches[6 * i_fnum];
-    for (size_t i = 0; i < 6 * i_fnum; i++) initial_wrenches[i] = 0;
-    interpolators[WRENCHES]->set(initial_wrenches);
+	for (size_t i = 0; i < i_fnum; i++) {
+		double initial_wrench[6];
+		for (size_t ii = 0; ii < 6; ii++) initial_wrench[ii] = 0;
+		interpolators[WRENCHES+i]->set(initial_wrench);
+	}
 	double initial_optional_data[optional_data_dim];
 	for (size_t i = 0; i < optional_data_dim; i++) initial_optional_data[i] = 0;
 	interpolators[OPTIONAL_DATA]->set(initial_optional_data);
@@ -195,9 +202,22 @@ void seqplay::setBaseAcc(const double *i_acc, double i_tm)
 void seqplay::setWrenches(const double *i_wrenches, double i_tm)
 {
 	if (i_tm == 0){
-		interpolators[WRENCHES]->set(i_wrenches);
+		for (size_t i = 0; i < force_sensor_num; i++) {
+			interpolators[WRENCHES+i]->set(i_wrenches+i*6);
+		}
+	}else{
+		for (size_t i = 0; i < force_sensor_num; i++) {
+			interpolators[WRENCHES+i]->setGoal(i_wrenches+i*6, i_tm);
+		}
+	}
+}
+
+void seqplay::setWrench(const int idx, const double *i_wrench, double i_tm)
+{
+	if (i_tm == 0){
+		interpolators[WRENCHES+idx]->set(i_wrench);
 	}else{
-		interpolators[WRENCHES]->setGoal(i_wrenches, i_tm);
+		interpolators[WRENCHES+idx]->setGoal(i_wrench, i_tm);
 	}
 }
 
@@ -312,7 +332,9 @@ void seqplay::loadPattern(const char *basename, double tm)
     string wrenches = basename; wrenches.append(".wrenches");
     if (access(wrenches.c_str(),0)==0){
         found = true;
-        interpolators[WRENCHES]->load(wrenches, tm, scale, false);
+		for (size_t i = 0; i < force_sensor_num; i++) {
+			interpolators[WRENCHES+i]->load(wrenches, tm, scale, false, i*6, (i+1)*6);
+		}
         if (debug_level > 0) cout << wrenches;
     }
     if (debug_level > 0) cout << endl << "optional_data   = ";
@@ -365,7 +387,9 @@ void seqplay::get(double *o_q, double *o_zmp, double *o_accel,
 	interpolators[P]->get(o_basePos);
 	interpolators[RPY]->get(o_baseRpy);
 	interpolators[TQ]->get(o_tq);
-	interpolators[WRENCHES]->get(o_wrenches);
+	for (size_t i = 0; i < force_sensor_num; i++) {
+		interpolators[WRENCHES+i]->get(o_wrenches+i*6);
+	}
 	interpolators[OPTIONAL_DATA]->get(o_optional_data);
 }
 
@@ -390,7 +414,11 @@ void seqplay::go(const double *i_q, const double *i_zmp, const double *i_acc,
 	if (i_p) interpolators[P]->go(i_p, ii_p, i_time, false);
 	if (i_rpy) interpolators[RPY]->go(i_rpy, ii_rpy, i_time, false);
 	if (i_tq) interpolators[TQ]->go(i_tq, ii_tq, i_time, false);
-	if (i_wrenches) interpolators[WRENCHES]->go(i_wrenches, ii_wrenches, i_time, false);
+	if (i_wrenches) {
+		for (size_t i = 0; i < force_sensor_num; i++) {
+			interpolators[WRENCHES+i]->go(i_wrenches+i*6, ii_wrenches+i*6, i_time, false);
+		}
+	}
 	if (i_optional_data) interpolators[OPTIONAL_DATA]->go(i_optional_data, ii_optional_data, i_time, false);
 	if (immediate) sync();
 }

rtc/SequencePlayer/seqplay.h

@@ -26,6 +26,7 @@ class seqplay
     void setBaseRpy(const double *i_rpy, double i_tm=0.0);
     void setBaseAcc(const double *i_acc, double i_tm=0.0);
     void setWrenches(const double *i_wrenches, double i_tm=0.0);
+    void setWrench(const int idx, const double *i_wrench, double i_tm);
     void 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);
     //
     bool addJointGroup(const char *gname, const std::vector<int>& indices);
@@ -136,8 +137,9 @@ class seqplay
         double time2remove;
     };
     void pop_back();
-    enum {Q, ZMP, ACC, P, RPY, TQ, WRENCHES, OPTIONAL_DATA, NINTERPOLATOR};
-    interpolator *interpolators[NINTERPOLATOR];
+    enum {Q, ZMP, ACC, P, RPY, TQ, OPTIONAL_DATA, WRENCHES, NINTERPOLATOR_WITHOUT_WRENCHES};
+    size_t NINTERPOLATOR, force_sensor_num;
+    std::vector<interpolator *> interpolators; 
     std::map<std::string, groupInterpolator *> groupInterpolators; 
     int debug_level, m_dof;
 };