Return total force or moment from getObjectForcesMoments and consider moment_center as foot mid frame.

idl/ImpedanceControllerService.idl

@@ -69,6 +69,11 @@ module OpenHRP
  MODE_DETECTED,
  MODE_MAX_TIME
  };
+
+ /**
+ * @enum DetectorTotalWrench
+ * @brief Flag for whether total force or total moment is checked.
+ */
  enum DetectorTotalWrench {
  TOTAL_FORCE,
  TOTAL_MOMENT
@@ -93,9 +98,9 @@ module OpenHRP
  double start_time_thre;
  /// Axis
  DblArray3 axis;
- /// Moment center [m]
+ /// Moment center [m] from foot mid frame (middle frame of rleg and lleg)
  DblArray3 moment_center;
- /// Moment center [m]
+ /// Current flag for whether total force or total moment is checked.
  DetectorTotalWrench detector_total_wrench;
  };
 
@@ -181,6 +186,6 @@ module OpenHRP
  * @param i_param output force and moment
  * @return true if set successfully, false otherwise
  */
- boolean getObjectForcesMoments(out Dbl3Sequence o_forces, out Dbl3Sequence o_moments);
+ boolean getObjectForcesMoments(out Dbl3Sequence o_forces, out Dbl3Sequence o_moments, out DblSequence3 o_3dofwrench);
  };
 };

rtc/ImpedanceController/ImpedanceController.cpp

@@ -16,7 +16,7 @@
 #include <hrpModel/JointPath.h>
 #include <hrpUtil/MatrixSolvers.h>
 #include "util/Hrpsys.h"
-
+#include <boost/assign.hpp>
 
 #define MAX_TRANSITION_COUNT (static_cast<int>(2/m_dt))
 typedef coil::Guard<coil::Mutex> Guard;
@@ -516,7 +516,8 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
  }
  }
 
- calcObjectTurnaroundDetectorState();
+ if (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot
+ calcObjectTurnaroundDetectorState();
  } else {
  if ( DEBUGP || loop % 100 == 0 ) {
  std::cerr << "ImpedanceController is not working..." << std::endl;
@@ -562,6 +563,20 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
  }
 */
 
+void ImpedanceController::calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot)
+{
+ std::vector<hrp::Vector3> foot_pos;
+ std::vector<hrp::Matrix33> foot_rot;
+ std::vector<std::string> leg_names = boost::assign::list_of("rleg")("lleg");
+ for (size_t i = 0; i < leg_names.size(); i++) {
+ hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name);
+ foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos);
+ foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR);
+ }
+ new_foot_mid_pos = (foot_pos[0]+foot_pos[1])/2.0;
+ rats::mid_rot(new_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
+}
+
 void ImpedanceController::calcForceMoment ()
 {
  for (unsigned int i=0; i<m_forceIn.size(); i++){
@@ -623,6 +638,8 @@ void ImpedanceController::calcForceMoment ()
 
 void ImpedanceController::calcObjectTurnaroundDetectorState()
 {
+ // TODO
+ // Currently only for legged robots
  // Store org state
  hrp::dvector org_q(m_robot->numJoints());
  for ( int i = 0; i < m_robot->numJoints(); i++ ) {
@@ -636,20 +653,24 @@ void ImpedanceController::calcObjectTurnaroundDetectorState()
  updateRootLinkPosRot(m_rpy);
  // Calc
  std::vector<hrp::Vector3> otd_fmv, otd_hposv;
+ hrp::Vector3 fmpos;
+ hrp::Matrix33 fmrot, fmrotT;
+ calcFootMidCoords(fmpos, fmrot);
+ fmrotT = fmrot.transpose();
  for (unsigned int i=0; i<m_forceIn.size(); i++) {
  std::string sensor_name = m_forceIn[i]->name();
  if ( find(otd_sensor_names.begin(), otd_sensor_names.end(), sensor_name) != otd_sensor_names.end() ) {
  hrp::ForceSensor* sensor = m_robot->sensor<hrp::ForceSensor>(sensor_name);
  hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
  hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]);
  hrp::Matrix33 sensorR = sensor->link->R * sensor->localR;
- otd_fmv.push_back(sensorR*data_p);
+ otd_fmv.push_back(fmrotT*(sensorR*data_p));
  hrp::Vector3 eePos;
  for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) {
  if ( it->second.sensor_name == sensor_name ) {
  ee_trans& eet = ee_map[it->first];
  hrp::Link* target_link = m_robot->link(eet.target_name);
- eePos = target_link->p + target_link->R * eet.localPos;
+ eePos = fmrotT * (target_link->p + target_link->R * eet.localPos - fmpos);
  }
  }
  otd_hposv.push_back(eePos);
@@ -907,28 +928,27 @@ bool ImpedanceController::getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceCon
  return true;
 }
 
-bool ImpedanceController::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments)
+bool ImpedanceController::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench)
 {
  std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl;
  if (otd_sensor_names.size() == 0) return false;
  hrp::Vector3 tmpv = otd->getAxis() * otd->getFilteredWrench();
- std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl;
  o_forces = new OpenHRP::ImpedanceControllerService::Dbl3Sequence ();
  o_moments = new OpenHRP::ImpedanceControllerService::Dbl3Sequence ();
  o_forces->length(otd_sensor_names.size());
  o_moments->length(otd_sensor_names.size());
- std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl;
  for (size_t i = 0; i < o_forces->length(); i++) o_forces[i].length(3);
  for (size_t i = 0; i < o_moments->length(); i++) o_moments[i].length(3);
- std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl;
  // Temp
  for (size_t i = 0; i < otd_sensor_names.size(); i++) {
  o_forces[i][0] = tmpv(0)/otd_sensor_names.size();
  o_forces[i][1] = tmpv(1)/otd_sensor_names.size();
  o_forces[i][2] = tmpv(2)/otd_sensor_names.size();
  o_moments[i][0] = o_moments[i][1] = o_moments[i][2] = 0.0;
  }
- std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl;
+ o_3dofwrench = new OpenHRP::ImpedanceControllerService::DblSequence3 ();
+ o_3dofwrench->length(3);
+ for (size_t i = 0; i < 3; i++) (*o_3dofwrench)[i] = tmpv(i);
  return true;
 }
 

rtc/ImpedanceController/ImpedanceController.h

@@ -101,7 +101,7 @@ class ImpedanceController
  OpenHRP::ImpedanceControllerService::DetectorMode checkObjectTurnaroundDetection();
  bool setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_);
  bool getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_);
- bool getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments);
+ bool getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench);
 
  protected:
  // Configuration variable declaration
@@ -177,6 +177,7 @@ class ImpedanceController
 
  void copyImpedanceParam (OpenHRP::ImpedanceControllerService::impedanceParam& i_param_, const ImpedanceParam& param);
  void updateRootLinkPosRot (TimedOrientation3D tmprpy);
+ void calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot);
  void calcForceMoment();
  void calcObjectTurnaroundDetectorState();
 

rtc/ImpedanceController/ImpedanceControllerService_impl.cpp

@@ -70,9 +70,9 @@ CORBA::Boolean ImpedanceControllerService_impl::getObjectTurnaroundDetectorParam
  return m_impedance->getObjectTurnaroundDetectorParam(i_param_);
 }
 
-CORBA::Boolean ImpedanceControllerService_impl::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments)
+CORBA::Boolean ImpedanceControllerService_impl::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench)
 {
- return m_impedance->getObjectForcesMoments(o_forces, o_moments);
+ return m_impedance->getObjectForcesMoments(o_forces, o_moments, o_3dofwrench);
 }
 
 void ImpedanceControllerService_impl::impedance(ImpedanceController *i_impedance)

rtc/ImpedanceController/ImpedanceControllerService_impl.h

@@ -27,7 +27,7 @@ class ImpedanceControllerService_impl
  OpenHRP::ImpedanceControllerService::DetectorMode checkObjectTurnaroundDetection();
  CORBA::Boolean setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_);
  CORBA::Boolean getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_);
- CORBA::Boolean getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments);
+ CORBA::Boolean getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench);
 
  //
  void impedance(ImpedanceController *i_impedance);

rtc/ImpedanceController/ObjectTurnaroundDetector.h

@@ -25,9 +25,10 @@ class ObjectTurnaroundDetector
  process_mode pmode;
  detector_total_wrench dtw;
  std::string print_str;
+ bool is_dwr_changed;
  public:
  ObjectTurnaroundDetector (const double _dt) : axis(-1*hrp::Vector3::UnitZ()), moment_center(hrp::Vector3::Zero()), prev_wrench(0.0), dt(_dt), detect_ratio_thre(0.01), start_ratio_thre(0.5),
-  count(0), detect_count_thre(5), start_count_thre(5), pmode(MODE_IDLE), dtw(TOTAL_FORCE)
+ count(0), detect_count_thre(5), start_count_thre(5), pmode(MODE_IDLE), dtw(TOTAL_FORCE), is_dwr_changed(false)
  {
  double default_cutoff_freq = 1; // [Hz]
  wrench_filter = boost::shared_ptr<FirstOrderLowPassFilter<double> >(new FirstOrderLowPassFilter<double>(default_cutoff_freq, _dt, 0));
@@ -75,6 +76,11 @@ class ObjectTurnaroundDetector
  };
  bool checkDetection (const double wrench_value)
  {
+ if (is_dwr_changed) {
+ wrench_filter->reset(wrench_value);
+ dwrench_filter->reset(0);
+ is_dwr_changed = false;
+ }
  double tmp_wr = wrench_filter->passFilter(wrench_value);
  double tmp_dwr = dwrench_filter->passFilter((tmp_wr-prev_wrench)/dt);
  prev_wrench = tmp_wr;
@@ -140,8 +146,7 @@ class ObjectTurnaroundDetector
  void setDetectorTotalWrench (const detector_total_wrench _dtw)
  {
  if (_dtw != dtw) {
- wrench_filter->reset(0);
- dwrench_filter->reset(0);
+ is_dwr_changed = true;
  }
  dtw = _dtw;
  };