[SequencePlayer] add setJointVelocities, setJointVelocitiesSequence, setJointTorques, setJointTorquesSequence

idl/SequencePlayerService.idl

@@ -55,6 +55,38 @@ module OpenHRP
      */
     boolean setJointAngle(in string jname, in double jv, in double tm);
 
+    /**
+     * @brief Interpolate all joint velocities on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot.
+     * @param jvs sequence of joint velocitys
+     * @param tm duration [s]
+     * @return true joint velocitys are set successfully, false otherwise
+     */
+    boolean setJointVelocities(in dSequence jvs, in double tm);
+
+    /**
+     * @brief Interpolate all joint velocitys on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0]
+     * @param jvs sequence of sequence of joint velocitys [rad]
+     * @param tm sequence of duration [s]
+     * @return true joint velocitys are set successfully, false otherwise
+     */
+    boolean setJointVelocitiesSequence(in dSequenceSequence jvss, in dSequence tms);
+
+    /**
+     * @brief Interpolate all joint torques on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot.
+     * @param jvs sequence of joint torques
+     * @param tm duration [s]
+     * @return true joint torques are set successfully, false otherwise
+     */
+    boolean setJointTorques(in dSequence jvs, in double tm);
+
+    /**
+     * @brief Interpolate all joint torques on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0]
+     * @param jvs sequence of sequence of joint torques [rad]
+     * @param tm sequence of duration [s]
+     * @return true joint torques are set successfully, false otherwise
+     */
+    boolean setJointTorquesSequence(in dSequenceSequence jvss, in dSequence tms);
+
     /**
      * @brief Interpolate position of the base link. Function returns without waiting for interpolation to finish
      * @param pos position of the base link [m]
@@ -161,6 +193,24 @@ module OpenHRP
      */
     boolean setJointAnglesOfGroup(in string gname, in dSequence jvs, in double tm);
 
+    /**
+     * @brief Interpolate joint velocities in a group  using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot.
+     * @param gname name of the joint group
+     * @param jvs sequence of joint velocities
+     * @param tm duration [s]
+     * @return true joint velocities are set successfully, false otherwise
+     */
+    boolean setJointVelocitiesOfGroup(in string gname, in dSequence jvs, in double tm);
+
+    /**
+     * @brief Interpolate joint torques in a group  using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot.
+     * @param gname name of the joint group
+     * @param jvs sequence of joint torques
+     * @param tm duration [s]
+     * @return true joint torques are set successfully, false otherwise
+     */
+    boolean setJointTorquesOfGroup(in string gname, in dSequence jvs, in double tm);
+
     /**
      * @brief clear pattern data with joint group
      * @param gname name of the joint group
@@ -225,6 +275,24 @@ module OpenHRP
      */
     boolean setJointAnglesSequenceOfGroup(in string gname, in dSequenceSequence jvss, in dSequence tms);
 
+    /**
+     * @brief Interpolate all joint velocities in groups, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0]
+     * @param gname name of the joint group
+     * @param jvs sequence of sequence of joint velocities [rad]
+     * @param tm sequence of duration [s]
+     * @return true joint velocities are set successfully, false otherwise
+     */
+    boolean setJointVelocitiesSequenceOfGroup(in string gname, in dSequenceSequence jvss, in dSequence tms);
+
+    /**
+     * @brief Interpolate all joint torques in groups, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0]
+     * @param gname name of the joint group
+     * @param jvs sequence of sequence of joint torques [rad]
+     * @param tm sequence of duration [s]
+     * @return true joint torques are set successfully, false otherwise
+     */
+    boolean setJointTorquesSequenceOfGroup(in string gname, in dSequenceSequence jvss, in dSequence tms);
+
     /**
      * @brief Interpolate all joint angles, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal.  [>= 315.5.0]
      * @param jvss sequence of sequence of joint angles [rad]

rtc/SequencePlayer/SequencePlayer.cpp

@@ -49,6 +49,7 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager)
       m_zmpRefInitIn("zmpRefInit", m_zmpRefInit),
       m_qRefOut("qRef", m_qRef),
       m_tqRefOut("tqRef", m_tqRef),
+      m_dqRefOut("dqRef", m_dqRef),
       m_zmpRefOut("zmpRef", m_zmpRef),
       m_accRefOut("accRef", m_accRef),
       m_basePosOut("basePos", m_basePos),
@@ -88,6 +89,7 @@ RTC::ReturnCode_t SequencePlayer::onInitialize()
     // Set OutPort buffer
     addOutPort("qRef", m_qRefOut);
     addOutPort("tqRef", m_tqRefOut);
+    addOutPort("dqRef", m_dqRefOut);
     addOutPort("zmpRef", m_zmpRefOut);
     addOutPort("accRef", m_accRefOut);
     addOutPort("basePos", m_basePosOut);
@@ -175,6 +177,7 @@ RTC::ReturnCode_t SequencePlayer::onInitialize()
     // allocate memory for outPorts
     m_qRef.data.length(dof);
     m_tqRef.data.length(dof);
+    m_dqRef.data.length(dof);
     m_optionalData.data.length(optional_data_dim);
 
     return RTC::RTC_OK;
@@ -247,7 +250,7 @@ RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id)
 	Guard guard(m_mutex);
 
         double zmp[3], acc[3], pos[3], rpy[3], wrenches[6*m_wrenches.size()];
-        m_seq->get(m_qRef.data.get_buffer(), zmp, acc, pos, rpy, m_tqRef.data.get_buffer(), wrenches, m_optionalData.data.get_buffer());
+        m_seq->get(m_qRef.data.get_buffer(), zmp, acc, pos, rpy, m_tqRef.data.get_buffer(), wrenches, m_optionalData.data.get_buffer(), m_dqRef.data.get_buffer());
         m_zmpRef.data.x = zmp[0];
         m_zmpRef.data.y = zmp[1];
         m_zmpRef.data.z = zmp[2];
@@ -304,6 +307,7 @@ RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id)
         m_qRef.tm = m_qInit.tm;
         m_qRefOut.write();
         m_tqRefOut.write();
+        m_dqRefOut.write();
         m_zmpRefOut.write();
         m_accRefOut.write();
         m_basePosOut.write();
@@ -507,6 +511,40 @@ bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const Open
     return m_seq->setJointAnglesSequenceOfGroup(gname, v_poss, v_tms, angless.length()>0?angless[0].length():0);
 }
 
+bool SequencePlayer::setJointVelocitiesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence velocitiess, const OpenHRP::dSequence& times)
+{
+    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;
+
+    std::vector<const double*> v_vels;
+    std::vector<double> v_tms;
+    for ( unsigned int i = 0; i < velocitiess.length(); i++ ) v_vels.push_back(velocitiess[i].get_buffer());
+    for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
+    return m_seq->setJointVelocitiesSequenceOfGroup(gname, v_vels, v_tms, velocitiess.length()>0?velocitiess[0].length():0);
+}
+
+bool SequencePlayer::setJointTorquesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence torquess, const OpenHRP::dSequence& times)
+{
+    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;
+
+    std::vector<const double*> v_torques;
+    std::vector<double> v_tms;
+    for ( unsigned int i = 0; i < torquess.length(); i++ ) v_torques.push_back(torquess[i].get_buffer());
+    for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
+    return m_seq->setJointTorquesSequenceOfGroup(gname, v_torques, v_tms, torquess.length()>0?torquess[0].length():0);
+}
+
 bool SequencePlayer::clearJointAnglesOfGroup(const char *gname)
 {
     if ( m_debugLevel > 0 ) {
@@ -545,6 +583,68 @@ bool SequencePlayer::setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence
     return m_seq->setJointAnglesSequenceFull(v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals, v_tms);
 }
 
+bool SequencePlayer::setJointVelocities(const double *velocitys, double tm)
+{
+    if ( m_debugLevel > 0 ) {
+        std::cerr << __PRETTY_FUNCTION__ << std::endl;
+    }
+    Guard guard(m_mutex);
+    if (!setInitialState()) return false;
+    std::vector<const double*> v_vels;
+    std::vector<double> v_tms;
+    v_vels.push_back(velocitys);
+    v_tms.push_back(tm);
+    m_seq->setJointVelocitiesSequence(v_vels, v_tms);
+    return true;
+}
+
+bool SequencePlayer::setJointVelocitiesSequence(const OpenHRP::dSequenceSequence velocityss, const OpenHRP::dSequence& times)
+{
+    if ( m_debugLevel > 0 ) {
+        std::cerr << __PRETTY_FUNCTION__ << std::endl;
+    }
+    Guard guard(m_mutex);
+
+    if (!setInitialState()) return false;
+
+    std::vector<const double*> v_vels;
+    std::vector<double> v_tms;
+    for ( unsigned int i = 0; i < velocityss.length(); i++ ) v_vels.push_back(velocityss[i].get_buffer());
+    for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
+    return m_seq->setJointVelocitiesSequence(v_vels, v_tms);
+}
+
+bool SequencePlayer::setJointTorques(const double *torques, double tm)
+{
+    if ( m_debugLevel > 0 ) {
+        std::cerr << __PRETTY_FUNCTION__ << std::endl;
+    }
+    Guard guard(m_mutex);
+    if (!setInitialState()) return false;
+    std::vector<const double*> v_torques;
+    std::vector<double> v_tms;
+    v_torques.push_back(torques);
+    v_tms.push_back(tm);
+    m_seq->setJointTorquesSequence(v_torques, v_tms);
+    return true;
+}
+
+bool SequencePlayer::setJointTorquesSequence(const OpenHRP::dSequenceSequence torquess, const OpenHRP::dSequence& times)
+{
+    if ( m_debugLevel > 0 ) {
+        std::cerr << __PRETTY_FUNCTION__ << std::endl;
+    }
+    Guard guard(m_mutex);
+
+    if (!setInitialState()) return false;
+
+    std::vector<const double*> v_torques;
+    std::vector<double> v_tms;
+    for ( unsigned int i = 0; i < torquess.length(); i++ ) v_torques.push_back(torquess[i].get_buffer());
+    for ( unsigned int i = 0; i <  times.length();  i++ )  v_tms.push_back(times[i]);
+    return m_seq->setJointTorquesSequence(v_torques, v_tms);
+}
+
 bool SequencePlayer::setBasePos(const double *pos, double tm)
 {
     if ( m_debugLevel > 0 ) {
@@ -875,6 +975,30 @@ bool SequencePlayer::setJointAnglesOfGroup(const char *gname, const dSequence& j
     return m_seq->setJointAnglesOfGroup(gname, jvs.get_buffer(), jvs.length(), tm);
 }
 
+bool SequencePlayer::setJointVelocitiesOfGroup(const char *gname, const dSequence& jvs, double tm)
+{
+    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->setJointVelocitiesOfGroup(gname, jvs.get_buffer(), jvs.length(), tm);
+}
+
+bool SequencePlayer::setJointTorquesOfGroup(const char *gname, const dSequence& jvs, double tm)
+{
+    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->setJointTorquesOfGroup(gname, jvs.get_buffer(), jvs.length(), tm);
+}
+
 bool SequencePlayer::playPatternOfGroup(const char *gname, const dSequenceSequence& pos, const dSequence& tm)
 {
     if ( m_debugLevel > 0 ) {

rtc/SequencePlayer/SequencePlayer.h

@@ -104,6 +104,10 @@ class SequencePlayer
   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 setJointVelocities(const double *velocitys, double tm);
+  bool setJointVelocitiesSequence(const OpenHRP::dSequenceSequence velocityss, const OpenHRP::dSequence& times);
+  bool setJointTorques(const double *torques, double tm);
+  bool setJointTorquesSequence(const OpenHRP::dSequenceSequence torquess, const OpenHRP::dSequence& times);
   bool setBasePos(const double *pos, double tm);
   bool setBaseRpy(const double *rpy, double tm);
   bool setZmp(const double *zmp, double tm);
@@ -117,6 +121,10 @@ class SequencePlayer
   bool removeJointGroup(const char *gname);
   bool setJointAnglesOfGroup(const char *gname, const OpenHRP::dSequence& jvs, double tm);
   bool setJointAnglesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence angless, const OpenHRP::dSequence& times);
+  bool setJointVelocitiesOfGroup(const char *gname, const OpenHRP::dSequence& jvs, double tm);
+  bool setJointVelocitiesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence velocitiess, const OpenHRP::dSequence& times);
+  bool setJointTorquesOfGroup(const char *gname, const OpenHRP::dSequence& jvs, double tm);
+  bool setJointTorquesSequenceOfGroup(const char *gname, const OpenHRP::dSequenceSequence torquess, const OpenHRP::dSequence& times);
     bool clearJointAnglesOfGroup(const char *gname);
   bool playPatternOfGroup(const char *gname, const OpenHRP::dSequenceSequence& pos, const OpenHRP::dSequence& tm);
 
@@ -147,6 +155,8 @@ class SequencePlayer
   OutPort<TimedDoubleSeq> m_qRefOut;
   TimedDoubleSeq m_tqRef;
   OutPort<TimedDoubleSeq> m_tqRefOut;
+  TimedDoubleSeq m_dqRef;
+  OutPort<TimedDoubleSeq> m_dqRefOut;
   TimedPoint3D m_zmpRef;
   OutPort<TimedPoint3D> m_zmpRefOut;
   TimedAcceleration3D m_accRef;

rtc/SequencePlayer/SequencePlayerService_impl.cpp

@@ -194,6 +194,60 @@ CORBA::Boolean SequencePlayerService_impl::setJointAngle(const char *jname, CORB
     return m_player->setJointAngle(id, jv, tm);
 }
 
+CORBA::Boolean SequencePlayerService_impl::setJointVelocities(const dSequence& jvs, CORBA::Double tm)
+{
+  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
+      return false;
+  }
+  return m_player->setJointVelocities(jvs.get_buffer(), tm);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointVelocitiesSequence(const dSequenceSequence& jvss, const dSequence& tms)
+{
+  if (jvss.length() <= 0) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint velocitys sequence is invalid:" << jvss.length() << " > 0" << std::endl;
+      return false;
+  }
+  if (jvss.length() != tms.length()) {
+      std::cerr << __PRETTY_FUNCTION__ << " length of joint velocitys sequence and time sequence differ, joint velocity:" << jvss.length() << ", time:" << tms.length() << std::endl;
+      return false;
+  }
+  const dSequence& jvs = jvss[0];
+  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
+      return false;
+  }
+  return m_player->setJointVelocitiesSequence(jvss, tms);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointTorques(const dSequence& jvs, CORBA::Double tm)
+{
+  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
+      return false;
+  }
+  return m_player->setJointTorques(jvs.get_buffer(), tm);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointTorquesSequence(const dSequenceSequence& jvss, const dSequence& tms)
+{
+  if (jvss.length() <= 0) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint torques sequence is invalid:" << jvss.length() << " > 0" << std::endl;
+      return false;
+  }
+  if (jvss.length() != tms.length()) {
+      std::cerr << __PRETTY_FUNCTION__ << " length of joint torques sequence and time sequence differ, joint torque:" << jvss.length() << ", time:" << tms.length() << std::endl;
+      return false;
+  }
+  const dSequence& jvs = jvss[0];
+  if (jvs.length() != (unsigned int)(m_player->robot()->numJoints())) {
+      std::cerr << __PRETTY_FUNCTION__ << " num of joint is differ, input:" << jvs.length() << ", robot:" << (unsigned int)(m_player->robot()->numJoints()) << std::endl;
+      return false;
+  }
+  return m_player->setJointTorquesSequence(jvss, tms);
+}
+
 CORBA::Boolean SequencePlayerService_impl::setBasePos(const dSequence& pos, CORBA::Double tm)
 {
     if (pos.length() != 3) return false;
@@ -306,6 +360,34 @@ CORBA::Boolean SequencePlayerService_impl::setJointAnglesSequenceOfGroup(const c
     return m_player->setJointAnglesSequenceOfGroup(gname, jvss, tms);
 }
 
+CORBA::Boolean SequencePlayerService_impl::setJointVelocitiesOfGroup(const char *gname, const dSequence& jvs, CORBA::Double tm)
+{
+    return m_player->setJointVelocitiesOfGroup(gname, jvs, tm);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointVelocitiesSequenceOfGroup(const char *gname, const dSequenceSequence& jvss, const dSequence& tms)
+{
+    if (jvss.length() != tms.length()) {
+        std::cerr << __PRETTY_FUNCTION__ << " length of joint velocities sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
+        return false;
+    }
+    return m_player->setJointVelocitiesSequenceOfGroup(gname, jvss, tms);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointTorquesOfGroup(const char *gname, const dSequence& jvs, CORBA::Double tm)
+{
+    return m_player->setJointTorquesOfGroup(gname, jvs, tm);
+}
+
+CORBA::Boolean SequencePlayerService_impl::setJointTorquesSequenceOfGroup(const char *gname, const dSequenceSequence& jvss, const dSequence& tms)
+{
+    if (jvss.length() != tms.length()) {
+        std::cerr << __PRETTY_FUNCTION__ << " length of joint torques sequence and time sequence differ, joint angle:" << jvss.length() << ", time:" << tms.length() << std::endl;
+        return false;
+    }
+    return m_player->setJointTorquesSequenceOfGroup(gname, jvss, tms);
+}
+
 CORBA::Boolean SequencePlayerService_impl::clearJointAnglesOfGroup(const char *gname)
 {
     return m_player->clearJointAnglesOfGroup(gname);