Skip to content

Commit

Permalink
Merge pull request fkanehiro#930 from snozawa/update_for_rot_otd
Browse files Browse the repository at this point in the history 
Enable total moment detection by object turnaround detection.
  • Loading branch information
@fkanehiro
fkanehiro committed Dec 27, 2015
2 parents 685f73a + 4ec2d5c commit c6b6b49
Show file tree
Hide file tree
Showing 4 changed files with 107 additions and 23 deletions.
8 changes: 8 additions & 0 deletions idl/ImpedanceControllerService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,6 +69,10 @@ module OpenHRP
MODE_DETECTED,
MODE_MAX_TIME
};
enum DetectorTotalWrench {
TOTAL_FORCE,
TOTAL_MOMENT
};

/**
* @struct objectTurnaroundDetectorParam
Expand All @@ -89,6 +93,10 @@ module OpenHRP
double start_time_thre;
/// Axis
DblArray3 axis;
/// Moment center [m]
DblArray3 moment_center;
/// Moment center [m]
DetectorTotalWrench detector_total_wrench;
};

/**
Expand Down
73 changes: 56 additions & 17 deletions rtc/ImpedanceController/ImpedanceController.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,7 +306,6 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
loop ++;

// check dataport input
bool update_rpy = false;
for (unsigned int i=0; i<m_forceIn.size(); i++){
if ( m_forceIn[i]->isNew() ) {
m_forceIn[i]->read();
Expand All @@ -323,12 +322,9 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
}
if (m_rpyIn.isNew()) {
m_rpyIn.read();
update_rpy = true;
}
if (m_qCurrentIn.isNew()) {
m_qCurrentIn.read();
//
if (update_rpy) updateRootLinkPosRot(m_rpy);
}
if (m_qRefIn.isNew()) {
m_qRefIn.read();
Expand Down Expand Up @@ -432,7 +428,6 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
}
}
}
if (update_rpy) updateRootLinkPosRot(m_rpy);
m_robot->calcForwardKinematics();

}
Expand Down Expand Up @@ -520,6 +515,8 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)
std::cerr << std::endl;
}
}

calcObjectTurnaroundDetectorState();
} else {
if ( DEBUGP || loop % 100 == 0 ) {
std::cerr << "ImpedanceController is not working..." << std::endl;
Expand Down Expand Up @@ -567,7 +564,6 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id)

void ImpedanceController::calcForceMoment ()
{
std::vector<hrp::Vector3> otd_fmv;
for (unsigned int i=0; i<m_forceIn.size(); i++){
if ( m_force[i].data.length()==6 ) {
std::string sensor_name = m_forceIn[i]->name();
Expand Down Expand Up @@ -615,12 +611,6 @@ void ImpedanceController::calcForceMoment ()
// World frame
abs_ref_forces[sensor_name] = ref_data_p;
abs_ref_moments[sensor_name] = ref_data_r;
// tmp
if ( find(otd_sensor_names.begin(), otd_sensor_names.end(), sensor_name) != otd_sensor_names.end() ) {
otd_fmv.push_back(abs_forces[sensor_name]);
// if ( DEBUGP ) {
// }
}
if ( DEBUGP ) {
std::cerr << "[" << m_profile.instance_name << "] abs force = " << abs_forces[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] abs moment = " << abs_moments[sensor_name].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl;
Expand All @@ -629,7 +619,48 @@ void ImpedanceController::calcForceMoment ()
}
}
}
otd->checkDetection(otd_fmv);
};

void ImpedanceController::calcObjectTurnaroundDetectorState()
{
// Store org state
hrp::dvector org_q(m_robot->numJoints());
for ( int i = 0; i < m_robot->numJoints(); i++ ) {
org_q[i] = m_robot->joint(i)->q;
}
hrp::Matrix33 orgR = m_robot->rootLink()->R;
// Set actual state
for ( int i = 0; i < m_robot->numJoints(); i++ ) {
m_robot->joint(i)->q = m_qCurrent.data[i];
}
updateRootLinkPosRot(m_rpy);
// Calc
std::vector<hrp::Vector3> otd_fmv, otd_hposv;
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);
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;
}
}
otd_hposv.push_back(eePos);
}
}
otd->checkDetection(otd_fmv, otd_hposv);
// Revert to org state
for ( int i = 0; i < m_robot->numJoints(); i++ ) {
m_robot->joint(i)->q = org_q[i];
}
m_robot->rootLink()->R = orgR;
};

//
Expand Down Expand Up @@ -772,10 +803,12 @@ void ImpedanceController::copyImpedanceParam (ImpedanceControllerService::impeda

void ImpedanceController::updateRootLinkPosRot (TimedOrientation3D tmprpy)
{
hrp::Sensor *sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
hrp::Matrix33 tmpr;
rats::rotm3times(tmpr, hrp::Matrix33(sensor->link->R*sensor->localR).transpose(), m_robot->rootLink()->R);
rats::rotm3times(m_robot->rootLink()->R, hrp::rotFromRpy(tmprpy.data.r, tmprpy.data.p, tmprpy.data.y), tmpr);
if ( m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) {
hrp::Sensor *sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0);
hrp::Matrix33 tmpr;
rats::rotm3times(tmpr, hrp::Matrix33(sensor->link->R*sensor->localR).transpose(), m_robot->rootLink()->R);
rats::rotm3times(m_robot->rootLink()->R, hrp::rotFromRpy(tmprpy.data.r, tmprpy.data.p, tmprpy.data.y), tmpr);
}
}

bool ImpedanceController::getImpedanceControllerParam(const std::string& i_name_, ImpedanceControllerService::impedanceParam& i_param_)
Expand Down Expand Up @@ -850,6 +883,9 @@ bool ImpedanceController::setObjectTurnaroundDetectorParam(const OpenHRP::Impeda
hrp::Vector3 tmp;
for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.axis[i];
otd->setAxis(tmp);
for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.moment_center[i];
otd->setMomentCenter(tmp);
otd->setDetectorTotalWrench((i_param_.detector_total_wrench==OpenHRP::ImpedanceControllerService::TOTAL_FORCE)?ObjectTurnaroundDetector::TOTAL_FORCE:ObjectTurnaroundDetector::TOTAL_MOMENT);
otd->printParams();
return true;
};
Expand All @@ -865,6 +901,9 @@ bool ImpedanceController::getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceCon
i_param_.start_time_thre = otd->getStartTimeThre();
hrp::Vector3 tmp = otd->getAxis();
for (size_t i = 0; i < 3; i++) i_param_.axis[i] = tmp(i);
tmp = otd->getMomentCenter();
for (size_t i = 0; i < 3; i++) i_param_.moment_center[i] = tmp(i);
i_param_.detector_total_wrench = (otd->getDetectorTotalWrench()==ObjectTurnaroundDetector::TOTAL_FORCE)?OpenHRP::ImpedanceControllerService::TOTAL_FORCE:OpenHRP::ImpedanceControllerService::TOTAL_MOMENT;
return true;
}

Expand Down
1 change: 1 addition & 0 deletions rtc/ImpedanceController/ImpedanceController.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,7 @@ class ImpedanceController
void copyImpedanceParam (OpenHRP::ImpedanceControllerService::impedanceParam& i_param_, const ImpedanceParam& param);
void updateRootLinkPosRot (TimedOrientation3D tmprpy);
void calcForceMoment();
void calcObjectTurnaroundDetectorState();

std::map<std::string, ImpedanceParam> m_impedance_param;
std::map<std::string, ee_trans> ee_map;
Expand Down
48 changes: 42 additions & 6 deletions rtc/ImpedanceController/ObjectTurnaroundDetector.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,10 +10,11 @@ class ObjectTurnaroundDetector
{
public:
typedef enum {MODE_IDLE, MODE_STARTED, MODE_DETECTED, MODE_MAX_TIME} process_mode;
typedef enum {TOTAL_FORCE, TOTAL_MOMENT} detector_total_wrench;
private:
boost::shared_ptr<FirstOrderLowPassFilter<double> > wrench_filter;
boost::shared_ptr<FirstOrderLowPassFilter<double> > dwrench_filter;
hrp::Vector3 axis;
hrp::Vector3 axis, moment_center;
double prev_wrench, dt;
double detect_ratio_thre, start_ratio_thre, ref_dwrench, max_time, current_time;
size_t count;
Expand All @@ -22,10 +23,11 @@ class ObjectTurnaroundDetector
// start_count_thre*dt : Threshould for time [s] after the first starting detection (Wait start_time_thre [s] after first start detection).
size_t detect_count_thre, start_count_thre;
process_mode pmode;
detector_total_wrench dtw;
std::string print_str;
public:
ObjectTurnaroundDetector (const double _dt) : axis(-1*hrp::Vector3::UnitZ()), 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)
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)
{
double default_cutoff_freq = 1; // [Hz]
wrench_filter = boost::shared_ptr<FirstOrderLowPassFilter<double> >(new FirstOrderLowPassFilter<double>(default_cutoff_freq, _dt, 0));
Expand All @@ -42,13 +44,34 @@ class ObjectTurnaroundDetector
<< ", detect_thre = " << detect_ratio_thre * ref_dwrench << ", start_thre = " << start_ratio_thre * ref_dwrench << "), max_time = " << max_time << "[s]" << std::endl;
pmode = MODE_IDLE;
};
bool checkDetection (const std::vector<hrp::Vector3>& fmv)
double calcTotalForce (const std::vector<hrp::Vector3>& fmv)
{
hrp::Vector3 tmpv = hrp::Vector3::Zero();
for (size_t i = 0; i < fmv.size(); i++) {
tmpv += fmv[i];
}
checkDetection(axis.dot(tmpv));
return axis.dot(tmpv);
};
double calcTotalMoment (const std::vector<hrp::Vector3>& fmv, const std::vector<hrp::Vector3>& hposv)
{
hrp::Vector3 tmpv = hrp::Vector3::Zero();
for (size_t i = 0; i < fmv.size(); i++) {
tmpv += (hposv[i]-moment_center).cross(fmv[i]);
}
return axis.dot(tmpv);
};
bool checkDetection (const std::vector<hrp::Vector3>& fmv, const std::vector<hrp::Vector3>& hposv)
{
switch(dtw) {
case TOTAL_FORCE:
checkDetection(calcTotalForce(fmv));
break;
case TOTAL_MOMENT:
checkDetection(calcTotalMoment(fmv, hposv));
break;
default:
break;
};
};
bool checkDetection (const double wrench_value)
{
Expand Down Expand Up @@ -98,10 +121,12 @@ class ObjectTurnaroundDetector
process_mode getMode () const { return pmode; };
void printParams () const
{
std::cerr << "[" << print_str << "] ObjectTurnaroundDetector params" << std::endl;
std::cerr << "[" << print_str << "] ObjectTurnaroundDetector params (" << (dtw==TOTAL_FORCE?"TOTAL_FORCE":"TOTAL_MOMENT") << ")" << std::endl;
std::cerr << "[" << print_str << "] wrench_cutoff_freq = " << wrench_filter->getCutOffFreq() << "[Hz], dwrench_cutoff_freq = " << dwrench_filter->getCutOffFreq() << "[Hz]" << std::endl;
std::cerr << "[" << print_str << "] detect_ratio_thre = " << detect_ratio_thre << ", start_ratio_thre = " << start_ratio_thre
<< ", start_time_thre = " << start_count_thre*dt << "[s], detect_time_thre = " << detect_count_thre*dt << "[s]" << std::endl;
std::cerr << "[" << print_str << "] axis = [" << axis(0) << ", " << axis(1) << ", " << axis(2)
<< "], moment_center = " << moment_center(0) << ", " << moment_center(1) << ", " << moment_center(2) << "][m]" << std::endl;
};
void setPrintStr (const std::string& str) { print_str = str; };
void setWrenchCutoffFreq (const double a) { wrench_filter->setCutOffFreq(a); };
Expand All @@ -111,13 +136,24 @@ class ObjectTurnaroundDetector
void setDetectTimeThre (const double a) { detect_count_thre = static_cast<size_t>(a/dt); };
void setStartTimeThre (const double a) { start_count_thre = static_cast<size_t>(a/dt); };
void setAxis (const hrp::Vector3& a) { axis = a; };
void setMomentCenter (const hrp::Vector3& a) { moment_center = a; };
void setDetectorTotalWrench (const detector_total_wrench _dtw)
{
if (_dtw != dtw) {
wrench_filter->reset(0);
dwrench_filter->reset(0);
}
dtw = _dtw;
};
double getWrenchCutoffFreq () const { return wrench_filter->getCutOffFreq(); };
double getDwrenchCutoffFreq () const { return dwrench_filter->getCutOffFreq(); };
double getDetectRatioThre () const { return detect_ratio_thre; };
double getStartRatioThre () const { return start_ratio_thre; };
double getDetectTimeThre () const { return detect_count_thre*dt; };
double getStartTimeThre () const { return start_count_thre*dt; };
hrp::Vector3 getAxis () const { return axis; };
hrp::Vector3 getMomentCenter () const { return moment_center; };
detector_total_wrench getDetectorTotalWrench () const { return dtw; };
double getFilteredWrench () const { return wrench_filter->getCurrentValue(); };
double getFilteredDwrench () const { return dwrench_filter->getCurrentValue(); };
};
Expand Down

0 comments on commit c6b6b49

Please sign in to comment.