Skip to content

Commit

Permalink
Merge pull request fkanehiro#1239 from snozawa/update_abc_tf_rfu
Browse files Browse the repository at this point in the history 
Update for force balancing and filter of reference force updater
  • Loading branch information
@fkanehiro
fkanehiro authored Feb 18, 2018
2 parents 3832b38 + 7666b66 commit 3171fb2
Show file tree
Hide file tree
Showing 8 changed files with 136 additions and 74 deletions.
10 changes: 10 additions & 0 deletions idl/ReferenceForceUpdaterService.idl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ module OpenHRP
interface ReferenceForceUpdaterService
{
typedef sequence<double, 3> DblSequence3;
typedef sequence<string> StrSequence;

/**
* @struct ReferenceForceUpdaterParam
Expand All @@ -33,6 +34,8 @@ module OpenHRP
boolean is_hold_value;
/// Transition time[s]
double transition_time;
/// Cutoff frequency for actual force value [Hz]
double cutoff_freq;
};

/**
Expand Down Expand Up @@ -77,5 +80,12 @@ module OpenHRP
* @brief Wait for ReferenceForceUpdater mode transition.
*/
void waitReferenceForceUpdaterTransition(in string name);

/**
* @brief Get supported name sequence of ReferenceForceUpdater
* @param o_param is supported name sequence
* @return true if set successfully, false otherwise
*/
boolean getSupportedReferenceForceUpdaterNameSequence(out StrSequence o_names);
};
};
15 changes: 9 additions & 6 deletions rtc/AutoBalancer/AutoBalancer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -333,6 +333,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize()
registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]);
std::cerr << "[" << m_profile.instance_name << "] name = " << std::string("ref_"+sensor_names[i]) << std::endl;
ref_forces.push_back(hrp::Vector3(0,0,0));
ref_moments.push_back(hrp::Vector3(0,0,0));
}
// set force port
for (unsigned int i=0; i<nforce; i++){
Expand Down Expand Up @@ -913,6 +914,7 @@ void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords)
//ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
// world frame
ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]);
ref_moments[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[3], m_ref_force[i].data[4], m_ref_force[i].data[5]);
}
sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset);
};
Expand Down Expand Up @@ -2024,21 +2026,21 @@ void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, co
if ( use_force == MODE_NO_FORCE ) {
tmp_input_sbp = tmpcog + sbp_cog_offset;
} else {
calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces);
calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height);
tmp_input_sbp = target_sbp - sbp_offset;
sbp_cog_offset = tmp_input_sbp - tmpcog;
}
};

void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces)
void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height)
{
hrp::Vector3 denom, nume;
/* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */
double mass = m_robot->totalMass();
double mg = mass * gg->get_gravitational_acceleration();
hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero();
for (size_t i = 0; i < tmp_forces.size(); i++) {
total_sensor_ref_force += tmp_forces[i];
for (size_t i = 0; i < ref_forces.size(); i++) {
total_sensor_ref_force += ref_forces[i];
}
hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso
hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z);
Expand All @@ -2065,8 +2067,9 @@ void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point,
size_t idx = contact_states_index_map[it->first];
// Force applied point is assumed as end effector
hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos;
nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[idx](j) - fpos(j) * tmp_forces[idx](2) );
denom(j) -= tmp_forces[idx](2);
nume(j) += ( (fpos(2) - ref_com_height) * ref_forces[idx](j) - fpos(j) * ref_forces[idx](2) );
nume(j) += (j==0 ? ref_moments[idx](1):-ref_moments[idx](0));
denom(j) -= ref_forces[idx](2);
}
}
if ( use_force == MODE_REF_FORCE_WITH_FOOT ) {
Expand Down
4 changes: 2 additions & 2 deletions rtc/AutoBalancer/AutoBalancer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -232,7 +232,7 @@ class AutoBalancer
void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
// static balance point offsetting
void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector<hrp::Vector3>& tmp_forces);
void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height);
hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
bool isOptionalDataContact (const std::string& ee_name)
{
Expand Down Expand Up @@ -276,7 +276,7 @@ class AutoBalancer
// static balance point offsetting
hrp::Vector3 sbp_offset, sbp_cog_offset;
enum {MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT} use_force;
std::vector<hrp::Vector3> ref_forces;
std::vector<hrp::Vector3> ref_forces, ref_moments;

unsigned int m_debugLevel;
bool is_legged_robot, is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
Expand Down
143 changes: 81 additions & 62 deletions rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -206,14 +206,12 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize()
}
ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));

ReferenceForceUpdaterParam rfu_param;
//set rfu param
rfu_param.update_count = round((1/rfu_param.update_freq)/m_dt);
if (( ee_name != "rleg" ) && ( ee_name != "lleg" ))
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name , rfu_param));
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name , ReferenceForceUpdaterParam(m_dt)));

ee_index_map.insert(std::pair<std::string, size_t>(ee_name, i));
ref_force.push_back(hrp::Vector3::Zero());
act_force.push_back(hrp::Vector3::Zero());
//ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt)));
ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
Expand All @@ -225,16 +223,15 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize()
// For FootOriginExtMoment
{
std::string ee_name = "footoriginextmoment";
ReferenceForceUpdaterParam rfu_param;
rfu_param.update_count = round((1/rfu_param.update_freq)/m_dt);
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name, rfu_param));
m_RFUParam.insert(std::pair<std::string, ReferenceForceUpdaterParam>(ee_name, ReferenceForceUpdaterParam(m_dt)));
ee_trans eet;
eet.localPos = hrp::Vector3::Zero();
eet.localR = hrp::Matrix33::Identity();
eet.target_name = "";
ee_map.insert(std::pair<std::string, ee_trans>(ee_name , eet));
ee_index_map.insert(std::pair<std::string, size_t>(ee_name, ref_force.size()));
ref_force.push_back(hrp::Vector3::Zero());
act_force.push_back(hrp::Vector3::Zero());
ref_force_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(3, m_dt, interpolator::LINEAR)));
transition_interpolator.insert(std::pair<std::string, interpolator*>(ee_name, new interpolator(1, m_dt)));
}
Expand Down Expand Up @@ -347,6 +344,20 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id)
}
}
}

// Get and set reference (target) parameters
getTargetParameters();

// Get force sensor values
// Force sensor's force value is absolute in reference frame
for (unsigned int i=0; i<m_force.size(); i++ ){
hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, i);
act_force[i] = (sensor->link->R * sensor->localR) * hrp::Vector3(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]);
for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
if (ee_index_map[itr->first] == i) itr->second.act_force_filter->passFilter(act_force[i]);
}
}

// If RFU is not active
{
bool all_arm_is_not_active = true;
Expand Down Expand Up @@ -385,12 +396,50 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id)
}

// If RFU is active
{
hrp::dvector qorg(m_robot->numJoints());

// Update reference force
for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
std::string arm = itr->first;
if ( m_RFUParam[arm].is_active && loop % m_RFUParam[arm].update_count == 0 ) {
if ( isFootOriginExtMoment(arm) ) updateRefFootOriginExtMoment(arm);
else updateRefForces(arm);
}
if (!ref_force_interpolator[arm]->isEmpty()) {
ref_force_interpolator[arm]->get(ref_force[ee_index_map[arm]].data(), true);
}
}
}

//determin ref_force_out from ref_force_in
for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
for (unsigned int j=0; j<6; j++ ) {
m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
}
for (unsigned int j=0; j<3; j++ ) {
m_ref_force_out[i].data[j] = ref_force[i](j) * transition_interpolator_ratio[i] + m_ref_force_in[i].data[j] * (1-transition_interpolator_ratio[i]);
}
m_ref_force_out[i].tm = m_ref_force_in[i].tm;
m_ref_forceOut[i]->write();
}
// FootOriginExtMoment
size_t idx = ee_index_map["footoriginextmoment"];
hrp::Vector3 tmp_moment = (foot_origin_rot.transpose() * ref_force[idx]) * transition_interpolator_ratio[idx];
m_refFootOriginExtMoment.data.x = tmp_moment(0);
m_refFootOriginExtMoment.data.y = tmp_moment(1);
m_refFootOriginExtMoment.data.z = tmp_moment(2);
m_refFootOriginExtMoment.tm = m_qRef.tm;
m_refFootOriginExtMomentOut.write();
m_refFootOriginExtMomentIsHoldValue.tm = m_qRef.tm;
m_refFootOriginExtMomentIsHoldValue.data = m_RFUParam["footoriginextmoment"].is_hold_value;
m_refFootOriginExtMomentIsHoldValueOut.write();

return RTC::RTC_OK;
}

void ReferenceForceUpdater::getTargetParameters ()
{
// reference model
for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
qorg[i] = m_robot->joint(i)->q;
m_robot->joint(i)->q = m_qRef.data[i];
}
m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z);
Expand Down Expand Up @@ -436,48 +485,10 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id)
rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R);
m_robot->calcForwardKinematics();
}

hrp::Vector3 foot_origin_pos;
calcFootOriginCoords(foot_origin_pos, foot_origin_rot);
}

for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
// Update reference force
std::string arm = itr->first;
if ( m_RFUParam[arm].is_active && loop % m_RFUParam[arm].update_count == 0 ) {
if ( isFootOriginExtMoment(arm) ) updateRefFootOriginExtMoment(arm);
else updateRefForces(arm);
}
if (!ref_force_interpolator[arm]->isEmpty()) {
ref_force_interpolator[arm]->get(ref_force[ee_index_map[arm]].data(), true);
}
}
}

//determin ref_force_out from ref_force_in
for (unsigned int i=0; i<m_ref_force_in.size(); i++ ){
for (unsigned int j=0; j<6; j++ ) {
m_ref_force_out[i].data[j] = m_ref_force_in[i].data[j];
}
for (unsigned int j=0; j<3; j++ ) {
m_ref_force_out[i].data[j] = ref_force[i](j) * transition_interpolator_ratio[i] + m_ref_force_in[i].data[j] * (1-transition_interpolator_ratio[i]);
}
m_ref_force_out[i].tm = m_ref_force_in[i].tm;
m_ref_forceOut[i]->write();
}
// FootOriginExtMoment
size_t idx = ee_index_map["footoriginextmoment"];
hrp::Vector3 tmp_moment = (foot_origin_rot.transpose() * ref_force[idx]) * transition_interpolator_ratio[idx];
m_refFootOriginExtMoment.data.x = tmp_moment(0);
m_refFootOriginExtMoment.data.y = tmp_moment(1);
m_refFootOriginExtMoment.data.z = tmp_moment(2);
m_refFootOriginExtMoment.tm = m_qRef.tm;
m_refFootOriginExtMomentOut.write();
m_refFootOriginExtMomentIsHoldValue.tm = m_qRef.tm;
m_refFootOriginExtMomentIsHoldValue.data = m_RFUParam["footoriginextmoment"].is_hold_value;
m_refFootOriginExtMomentIsHoldValueOut.write();

return RTC::RTC_OK;
}
};

void ReferenceForceUpdater::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot)
{
Expand Down Expand Up @@ -528,8 +539,8 @@ void ReferenceForceUpdater::updateRefForces (const std::string& arm)
double interpolation_time = 0;
size_t arm_idx = ee_index_map[arm];
hrp::Link* target_link = m_robot->link(ee_map[arm].target_name);
hrp::Vector3 abs_motion_dir, tmp_act_force, df;
hrp::Matrix33 ee_rot, sensor_rot;
hrp::Vector3 abs_motion_dir, df;
hrp::Matrix33 ee_rot;
ee_rot = target_link->R * ee_map[arm].localR;
if ( m_RFUParam[arm].frame=="local" )
abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir;
Expand All @@ -546,12 +557,8 @@ void ReferenceForceUpdater::updateRefForces (const std::string& arm)
rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]);
abs_motion_dir = current_foot_mid_rot * m_RFUParam[arm].motion_dir;
}
for (size_t i = 0; i < 3; i++ ) tmp_act_force(i) = m_force[arm_idx].data[i];
hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, arm_idx);
sensor_rot = sensor->link->R * sensor->localR;
tmp_act_force = sensor_rot * tmp_act_force;
// Calc abs force diff
df = tmp_act_force - ref_force[arm_idx];
df = m_RFUParam[arm].act_force_filter->getCurrentValue() - ref_force[arm_idx];
double inner_product = 0;
if ( ! std::fabs((abs_motion_dir.norm() - 0.0)) < 1e-5 ) {
abs_motion_dir.normalize();
Expand All @@ -570,7 +577,7 @@ void ReferenceForceUpdater::updateRefForces (const std::string& arm)
std::cerr << "[" << m_profile.instance_name << "] Updating reference force [" << arm << "]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << "[N], ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << "[N], interpolation_time = " << interpolation_time << "[s]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] new ref_force = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] filtered act_force = " << m_RFUParam[arm].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl;
}
};
Expand Down Expand Up @@ -655,13 +662,11 @@ bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const std::string& i_n
m_RFUParam[arm].i_gain = i_param.i_gain;
m_RFUParam[arm].is_hold_value = i_param.is_hold_value;
m_RFUParam[arm].transition_time = i_param.transition_time;
m_RFUParam[arm].act_force_filter->setCutOffFreq(i_param.cutoff_freq);
for (size_t i = 0; i < 3; i++ ) m_RFUParam[arm].motion_dir(i) = i_param.motion_dir[i];

// Print values
std::cerr << "[" << m_profile.instance_name << "] p_gain = " << m_RFUParam[arm].p_gain << ", d_gain = " << m_RFUParam[arm].d_gain << ", i_gain = " << m_RFUParam[arm].i_gain << std::endl;
std::cerr << "[" << m_profile.instance_name << "] update_freq = " << m_RFUParam[arm].update_freq << "[Hz], update_time_ratio = " << m_RFUParam[arm].update_time_ratio << ", transition_time = " << m_RFUParam[arm].transition_time << "[s]" << std::endl;
std::cerr << "[" << m_profile.instance_name << "] motion_dir = " << m_RFUParam[arm].motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl;
std::cerr << "[" << m_profile.instance_name << "] frame = " << m_RFUParam[arm].frame << ", is_hold_value = " << (m_RFUParam[arm].is_hold_value?"true":"false") << std::endl;
m_RFUParam[arm].printParam(std::string(m_profile.instance_name));
return true;
};

Expand All @@ -682,6 +687,7 @@ bool ReferenceForceUpdater::getReferenceForceUpdaterParam(const std::string& i_n
i_param->frame = m_RFUParam[arm].frame.c_str();
i_param->is_hold_value = m_RFUParam[arm].is_hold_value;
i_param->transition_time = m_RFUParam[arm].transition_time;
i_param->cutoff_freq = m_RFUParam[arm].act_force_filter->getCutOffFreq();
for (size_t i = 0; i < 3; i++ ) i_param->motion_dir[i] = m_RFUParam[arm].motion_dir(i);
return true;
};
Expand Down Expand Up @@ -757,6 +763,19 @@ void ReferenceForceUpdater::waitReferenceForceUpdaterTransition(const std::strin
usleep(1000);
};

bool ReferenceForceUpdater::getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names)
{
std::cerr << "[" << m_profile.instance_name << "] getSupportedReferenceForceUpdaterNameSequence" << std::endl;
Guard guard(m_mutex);
o_names->length(m_RFUParam.size());
size_t i = 0;
for (std::map<std::string, ReferenceForceUpdaterParam>::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) {
o_names[i] = itr->first.c_str();
i++;
}
return true;
};

extern "C"
{

Expand Down
Loading

0 comments on commit 3171fb2

Please sign in to comment.