From a73012052bb3df085f0ac574cb537654909fb007 Mon Sep 17 00:00:00 2001 From: KOYAMA Ryo Date: Wed, 1 Jun 2016 15:45:18 +0900 Subject: [PATCH 1/3] [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] fix indent --- .../ReferenceForceUpdater.cpp | 486 +++++++++--------- 1 file changed, 243 insertions(+), 243 deletions(-) diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp index 8327e2cd51a..657b3645b1c 100644 --- a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp @@ -98,15 +98,15 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; int comPos = nameServer.find(","); if (comPos < 0){ - comPos = nameServer.length(); + comPos = nameServer.length(); } nameServer = nameServer.substr(0, comPos); RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), //load robot model for m_robot CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){ - std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; - return RTC::RTC_ERROR; + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; + return RTC::RTC_ERROR; } // Setting for wrench data ports (real + virtual) @@ -114,15 +114,15 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() // find names for real force sensors int npforce = m_robot->numSensors(hrp::Sensor::FORCE); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); + fsensor_names.push_back(m_robot->sensor(hrp::Sensor::FORCE, i)->name); } // load virtual force sensors readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name)); int nvforce = m_vfs.size(); for (unsigned int i=0; i::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { - if (it->second.id == i) fsensor_names.push_back(it->first); - } + for ( std::map::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { + if (it->second.id == i) fsensor_names.push_back(it->first); + } } // add ports for all force sensors (real force, input and output of ref_force) @@ -135,22 +135,22 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() m_ref_forceOut.resize(nforce); std::cerr << "[" << m_profile.instance_name << "] create force sensor ports" << std::endl; for (unsigned int i=0; i(fsensor_names[i].c_str(), m_force[i]); - m_force[i].data.length(6); - registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]); - // ref inport - m_ref_force_in[i].data.length(6); - for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0; - m_ref_forceIn[i] = new InPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]); - registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]); - std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; - // ref Outport - m_ref_force_out[i].data.length(6); - for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0; - m_ref_forceOut[i] = new OutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]); - registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]); - std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; + // actual inport + m_forceIn[i] = new InPort(fsensor_names[i].c_str(), m_force[i]); + m_force[i].data.length(6); + registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]); + // ref inport + m_ref_force_in[i].data.length(6); + for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0; + m_ref_forceIn[i] = new InPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]); + registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; + // ref Outport + m_ref_force_out[i].data.length(6); + for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0; + m_ref_forceOut[i] = new OutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]); + registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; } // setting from conf file @@ -158,47 +158,47 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ","); std::map base_name_map; if (end_effectors_str.size() > 0) { - size_t prop_num = 10; - size_t num = end_effectors_str.size()/prop_num; - for (size_t i = 0; i < num; i++) { - std::string ee_name, ee_target, ee_base; - coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str()); - coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); - coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); - ee_trans eet; - for (size_t j = 0; j < 3; j++) { - coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); - } - double tmpv[4]; - for (int j = 0; j < 4; j++ ) { - coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); - } - eet.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle - eet.target_name = ee_target; - // tmp - if (ee_name == "rarm") eet.sensor_name = "rhsensor"; - else eet.sensor_name = "lhsensor"; - ee_map.insert(std::pair(ee_name , eet)); - base_name_map.insert(std::pair(ee_name, ee_base)); - ee_index_map.insert(std::pair(ee_name, i)); - ref_force.push_back(hrp::Vector3::Zero()); - //ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt))); - ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); - std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; - std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl; - std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; + size_t prop_num = 10; + size_t num = end_effectors_str.size()/prop_num; + for (size_t i = 0; i < num; i++) { + std::string ee_name, ee_target, ee_base; + coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str()); + coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); + coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); + ee_trans eet; + for (size_t j = 0; j < 3; j++) { + coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); + } + double tmpv[4]; + for (int j = 0; j < 4; j++ ) { + coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); } + eet.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle + eet.target_name = ee_target; + // tmp + if (ee_name == "rarm") eet.sensor_name = "rhsensor"; + else eet.sensor_name = "lhsensor"; + ee_map.insert(std::pair(ee_name , eet)); + base_name_map.insert(std::pair(ee_name, ee_base)); + ee_index_map.insert(std::pair(ee_name, i)); + ref_force.push_back(hrp::Vector3::Zero()); + //ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt))); + ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); + std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; + std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; + } } transition_interpolator = new interpolator(1, m_dt); // check if the dof of m_robot match the number of joint in m_robot unsigned int dof = m_robot->numJoints(); for ( int i = 0 ; i < dof; i++ ){ - if ( i != m_robot->joint(i)->jointId ) { - std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl; - return RTC::RTC_ERROR; - } + if ( i != m_robot->joint(i)->jointId ) { + std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl; + return RTC::RTC_ERROR; + } } loop = 0; @@ -222,7 +222,7 @@ RTC::ReturnCode_t ReferenceForceUpdater::onFinalize() { std::cerr << "[" << m_profile.instance_name << "] onFinalize()" << std::endl; for ( std::map::iterator it = ref_force_interpolator.begin(); it != ref_force_interpolator.end(); it++ ) { - delete it->second; + delete it->second; } ref_force_interpolator.clear(); delete transition_interpolator; @@ -263,165 +263,165 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) // check dataport input for (unsigned int i=0; iisNew() ) { - m_forceIn[i]->read(); - } - if ( m_ref_forceIn[i]->isNew() ) { - m_ref_forceIn[i]->read(); - } + if ( m_forceIn[i]->isNew() ) { + m_forceIn[i]->read(); + } + if ( m_ref_forceIn[i]->isNew() ) { + m_ref_forceIn[i]->read(); + } } if (m_basePosIn.isNew()) { - m_basePosIn.read(); + m_basePosIn.read(); } if (m_baseRpyIn.isNew()) { - m_baseRpyIn.read(); + m_baseRpyIn.read(); } if (m_rpyIn.isNew()) { - m_rpyIn.read(); + m_rpyIn.read(); } if (m_qRefIn.isNew()) { - m_qRefIn.read(); + m_qRefIn.read(); } double transition_interpolator_ratio = 1.0; //syncronize m_robot to the real robot if ( m_qRef.data.length() == m_robot->numJoints() ) { - Guard guard(m_mutex); - - // Interpolator - bool transition_interpolator_isEmpty = transition_interpolator->isEmpty(); - // if (DEBUGP) { - // std::cerr << "[" << m_profile.instance_name << "] transition " << transition_interpolator_isEmpty << " " << transition_interpolator_ratio << std::endl; - // } - if (!transition_interpolator_isEmpty) { - transition_interpolator->get(&transition_interpolator_ratio, true); - if (transition_interpolator->isEmpty() && is_active && is_stopping) { - std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl; - is_active = false; - is_stopping = false; - } + Guard guard(m_mutex); + + // Interpolator + bool transition_interpolator_isEmpty = transition_interpolator->isEmpty(); + // if (DEBUGP) { + // std::cerr << "[" << m_profile.instance_name << "] transition " << transition_interpolator_isEmpty << " " << transition_interpolator_ratio << std::endl; + // } + if (!transition_interpolator_isEmpty) { + transition_interpolator->get(&transition_interpolator_ratio, true); + if (transition_interpolator->isEmpty() && is_active && is_stopping) { + std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl; + is_active = false; + is_stopping = false; } + } - // If RFU is not active - if ( !is_active ) { - //determin ref_force_out from ref_force_in - for (unsigned int i=0; iwrite(); - } - return RTC::RTC_OK; + // If RFU is not active + if ( !is_active ) { + //determin ref_force_out from ref_force_in + for (unsigned int i=0; iwrite(); } + return RTC::RTC_OK; + } - // If RFU is active - { - hrp::dvector qorg(m_robot->numJoints()); + // If RFU is active + { + hrp::dvector qorg(m_robot->numJoints()); - // reference model - for ( 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); - m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); - m_robot->calcForwardKinematics(); - if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot - && !use_sh_base_pos_rpy ) { - // TODO - // Tempolarily modify root coords to fix foot pos rot - // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 - - // get current foot mid pos + rot - std::vector foot_pos; - std::vector foot_rot; - std::vector leg_names; - leg_names.push_back("rleg"); - leg_names.push_back("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); - } - hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); - hrp::Matrix33 current_foot_mid_rot; - rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); - // calculate fix pos + rot - hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); - hrp::Matrix33 new_foot_mid_rot; - { - hrp::Vector3 ex = hrp::Vector3::UnitX(); - hrp::Vector3 ez = hrp::Vector3::UnitZ(); - hrp::Vector3 xv1 (current_foot_mid_rot * ex); - xv1(2) = 0.0; - xv1.normalize(); - hrp::Vector3 yv1(ez.cross(xv1)); - new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); - new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); - new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); - } - // fix root pos + rot to fix "coords" = "current_foot_mid_xx" - hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); - m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); - rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); - m_robot->calcForwardKinematics(); - } + // reference model + for ( 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); + m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); + m_robot->calcForwardKinematics(); + if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot + && !use_sh_base_pos_rpy ) { + // TODO + // Tempolarily modify root coords to fix foot pos rot + // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 + + // get current foot mid pos + rot + std::vector foot_pos; + std::vector foot_rot; + std::vector leg_names; + leg_names.push_back("rleg"); + leg_names.push_back("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); + } + hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); + hrp::Matrix33 current_foot_mid_rot; + rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + // calculate fix pos + rot + hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); + hrp::Matrix33 new_foot_mid_rot; + { + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 xv1 (current_foot_mid_rot * ex); + xv1(2) = 0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); + new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); + new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); + } + // fix root pos + rot to fix "coords" = "current_foot_mid_xx" + hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); + m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); + rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + } + } - // Update reference force - hrp::Vector3 internal_force = hrp::Vector3::Zero(); - size_t arm_idx = ee_index_map[arm]; - double interpolation_time = 0; - if (is_active && loop % update_count == 0) { - 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; - ee_rot = target_link->R * ee_map[arm].localR; - abs_motion_dir = ee_rot * 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]; - double inner_product = 0; - if ( !std::fabs((abs_motion_dir.norm()- 0.0)) < 1e-5) { - abs_motion_dir.normalize(); - inner_product = df.dot(abs_motion_dir); - if ( !(inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) { - hrp::Vector3 in_f = ee_rot * internal_force; - ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (p_gain * inner_product * transition_interpolator_ratio) * abs_motion_dir; - interpolation_time = (1/update_freq) * update_time_ratio; - if (ref_force_interpolator[arm]->isEmpty()) { - ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); - } - } - } - if (DEBUGP) { - std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", 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, ", ", ", ", "", "", " [", "]")) << std::endl; - std::cerr << "[" << m_profile.instance_name << "] act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; - std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + // Update reference force + hrp::Vector3 internal_force = hrp::Vector3::Zero(); + size_t arm_idx = ee_index_map[arm]; + double interpolation_time = 0; + if (is_active && loop % update_count == 0) { + 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; + ee_rot = target_link->R * ee_map[arm].localR; + abs_motion_dir = ee_rot * 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]; + double inner_product = 0; + if ( !std::fabs((abs_motion_dir.norm()- 0.0)) < 1e-5) { + abs_motion_dir.normalize(); + inner_product = df.dot(abs_motion_dir); + if ( !(inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) { + hrp::Vector3 in_f = ee_rot * internal_force; + ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (p_gain * inner_product * transition_interpolator_ratio) * abs_motion_dir; + interpolation_time = (1/update_freq) * update_time_ratio; + if (ref_force_interpolator[arm]->isEmpty()) { + ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); } + } } - if (!ref_force_interpolator[arm]->isEmpty()) { - ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true); + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", 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, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; } + } + if (!ref_force_interpolator[arm]->isEmpty()) { + ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true); + } } //determin ref_force_out from ref_force_in for (unsigned int i=0; iwrite(); + 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 + m_ref_force_in[i].data[j] * (1-transition_interpolator_ratio); + } + m_ref_forceOut[i]->write(); } return RTC::RTC_OK; @@ -430,7 +430,7 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) /* RTC::ReturnCode_t ReferenceForceUpdater::onAborting(RTC::UniqueId ec_id) { - return RTC::RTC_OK; +return RTC::RTC_OK; } */ @@ -465,72 +465,72 @@ RTC::ReturnCode_t ReferenceForceUpdater::onRateChanged(RTC::UniqueId ec_id) bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) { - std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam" << std::endl; - Guard guard(m_mutex); - p_gain = i_param.p_gain; - d_gain = i_param.d_gain; - i_gain = i_param.i_gain; - update_freq = i_param.update_freq; - update_count = round((1/update_freq)/m_dt); - update_time_ratio = i_param.update_time_ratio; - arm = std::string(i_param.arm); - for (size_t i = 0; i < 3; i++) motion_dir(i) = i_param.motion_dir[i]; - std::cerr << "[" << m_profile.instance_name << "] p_gain = " << p_gain << ", d_gain = " << d_gain << ", i_gain = " << i_gain << std::endl; - std::cerr << "[" << m_profile.instance_name << "] update_freq = " << update_freq << "[Hz], update_time_ratio = " << update_time_ratio << std::endl; - std::cerr << "[" << m_profile.instance_name << "] motion_dir = " << motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; - return true; + std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam" << std::endl; + Guard guard(m_mutex); + p_gain = i_param.p_gain; + d_gain = i_param.d_gain; + i_gain = i_param.i_gain; + update_freq = i_param.update_freq; + update_count = round((1/update_freq)/m_dt); + update_time_ratio = i_param.update_time_ratio; + arm = std::string(i_param.arm); + for (size_t i = 0; i < 3; i++) motion_dir(i) = i_param.motion_dir[i]; + std::cerr << "[" << m_profile.instance_name << "] p_gain = " << p_gain << ", d_gain = " << d_gain << ", i_gain = " << i_gain << std::endl; + std::cerr << "[" << m_profile.instance_name << "] update_freq = " << update_freq << "[Hz], update_time_ratio = " << update_time_ratio << std::endl; + std::cerr << "[" << m_profile.instance_name << "] motion_dir = " << motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + return true; }; bool ReferenceForceUpdater::getReferenceForceUpdaterParam(OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) { - std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam" << std::endl; - Guard guard(m_mutex); - i_param->p_gain = p_gain; - i_param->d_gain = d_gain; - i_param->i_gain = i_gain; - i_param->update_freq = update_freq; - i_param->update_time_ratio = update_time_ratio; - i_param->arm = arm.c_str(); - for (size_t i = 0; i < 3; i++) i_param->motion_dir[i] = motion_dir(i); - return true; + std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam" << std::endl; + Guard guard(m_mutex); + i_param->p_gain = p_gain; + i_param->d_gain = d_gain; + i_param->i_gain = i_gain; + i_param->update_freq = update_freq; + i_param->update_time_ratio = update_time_ratio; + i_param->arm = arm.c_str(); + for (size_t i = 0; i < 3; i++) i_param->motion_dir[i] = motion_dir(i); + return true; }; bool ReferenceForceUpdater::startReferenceForceUpdater() { - std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater" << std::endl; - { - Guard guard(m_mutex); - if (transition_interpolator->isEmpty()) { - is_active = true; - double tmpstart = 0.0, tmpgoal = 1.0; - transition_interpolator->set(&tmpstart); - transition_interpolator->setGoal(&tmpgoal, 1.0, true); - } else { - return false; - } + std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater" << std::endl; + { + Guard guard(m_mutex); + if (transition_interpolator->isEmpty()) { + is_active = true; + double tmpstart = 0.0, tmpgoal = 1.0; + transition_interpolator->set(&tmpstart); + transition_interpolator->setGoal(&tmpgoal, 1.0, true); + } else { + return false; } - while (!transition_interpolator->isEmpty()) usleep(1000); - usleep(1000); - return true; + } + while (!transition_interpolator->isEmpty()) usleep(1000); + usleep(1000); + return true; }; bool ReferenceForceUpdater::stopReferenceForceUpdater() { - std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater" << std::endl; - { - Guard guard(m_mutex); - if (transition_interpolator->isEmpty()) { - double tmpstart = 1.0, tmpgoal = 0.0; - transition_interpolator->set(&tmpstart); - transition_interpolator->setGoal(&tmpgoal, 1.0, true); - is_stopping = true; - } else { - return false; - } + std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater" << std::endl; + { + Guard guard(m_mutex); + if (transition_interpolator->isEmpty()) { + double tmpstart = 1.0, tmpgoal = 0.0; + transition_interpolator->set(&tmpstart); + transition_interpolator->setGoal(&tmpgoal, 1.0, true); + is_stopping = true; + } else { + return false; } - while (!transition_interpolator->isEmpty()) usleep(1000); - usleep(1000); - return true; + } + while (!transition_interpolator->isEmpty()) usleep(1000); + usleep(1000); + return true; }; extern "C" From 94650f72b17b1801033e2263e453227bb7d96368 Mon Sep 17 00:00:00 2001 From: KOYAMA Ryo Date: Tue, 31 May 2016 22:52:29 +0900 Subject: [PATCH 2/3] [idl/ReferenceForceUpdaterService.idl] remove arm parameter from ReferenceForceUpdaterParam and add arm arg to interfaces of rfu [rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl{.h,.cpp}] remove arm parameter from ReferenceForceUpdaterParam [rtc/ReferenceForceUpdater/ReferenceForceUpdater.h] add ReferenceForceUpdaterParam structure [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] add Initialization for use_sh_base_pos_rpy [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] enable to set both arms parameters independently in rfu --- idl/ReferenceForceUpdaterService.idl | 10 +- .../ReferenceForceUpdater.cpp | 260 ++++++++++-------- .../ReferenceForceUpdater.h | 46 +++- .../ReferenceForceUpdaterService_impl.cpp | 16 +- .../ReferenceForceUpdaterService_impl.h | 8 +- 5 files changed, 201 insertions(+), 139 deletions(-) diff --git a/idl/ReferenceForceUpdaterService.idl b/idl/ReferenceForceUpdaterService.idl index c4b9a7f471c..3a53824acc6 100644 --- a/idl/ReferenceForceUpdaterService.idl +++ b/idl/ReferenceForceUpdaterService.idl @@ -16,8 +16,6 @@ module OpenHRP { /// Motion direction to update reference force DblSequence3 motion_dir; - /// Name of used arm - string arm; /// Update frequency [Hz] double update_freq; /// Update time ratio \in [0,1] @@ -35,25 +33,25 @@ module OpenHRP * @param i_param is input parameter * @return true if set successfully, false otherwise */ - boolean setReferenceForceUpdaterParam(in ReferenceForceUpdaterParam i_param); + boolean setReferenceForceUpdaterParam(in string name, in ReferenceForceUpdaterParam i_param); /** * @brief Get ReferenceForceUpdater parameters * @param i_param is input parameter * @return true if set successfully, false otherwise */ - boolean getReferenceForceUpdaterParam(out ReferenceForceUpdaterParam i_param); + boolean getReferenceForceUpdaterParam(in string name, out ReferenceForceUpdaterParam i_param); /** * @brief Start ReferenceForceUpdater * @return true if set successfully, false otherwise */ - boolean startReferenceForceUpdater(); + boolean startReferenceForceUpdater(in string name); /** * @brief Stop ReferenceForceUpdater * @return true if set successfully, false otherwise */ - boolean stopReferenceForceUpdater(); + boolean stopReferenceForceUpdater(in string name); }; }; diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp index 657b3645b1c..bccfb060218 100644 --- a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp @@ -51,6 +51,7 @@ ReferenceForceUpdater::ReferenceForceUpdater(RTC::Manager* manager) m_rpyIn("rpy", m_rpy), // m_robot(hrp::BodyPtr()), + use_sh_base_pos_rpy(false), m_debugLevel(0) { m_ReferenceForceUpdaterService.rfu(this); @@ -179,18 +180,25 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() if (ee_name == "rarm") eet.sensor_name = "rhsensor"; else eet.sensor_name = "lhsensor"; ee_map.insert(std::pair(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(ee_name , rfu_param)); + base_name_map.insert(std::pair(ee_name, ee_base)); ee_index_map.insert(std::pair(ee_name, i)); ref_force.push_back(hrp::Vector3::Zero()); //ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt))); ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); + if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt))); std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl; std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; } } - transition_interpolator = new interpolator(1, m_dt); // check if the dof of m_robot match the number of joint in m_robot unsigned int dof = m_robot->numJoints(); @@ -202,16 +210,8 @@ RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() } loop = 0; - update_freq = 50; // Hz - p_gain = 0.02; - d_gain = 0; - i_gain = 0; - arm = "rarm"; - is_active = false; - update_count = round((1/update_freq)/m_dt); - update_time_ratio = 0.5; - motion_dir = hrp::Vector3::UnitZ(); - is_stopping = false; + transition_interpolator_ratio.reserve(transition_interpolator.size()); + for (int i=0; i::iterator it = ref_force_interpolator.begin(); it != ref_force_interpolator.end(); it++ ) { delete it->second; } + for ( std::map::iterator it = transition_interpolator.begin(); it != transition_interpolator.end(); it++ ) { + delete it->second; + } ref_force_interpolator.clear(); - delete transition_interpolator; + transition_interpolator.clear(); return RTC::RTC_OK; } @@ -283,38 +286,43 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) m_qRefIn.read(); } - double transition_interpolator_ratio = 1.0; //syncronize m_robot to the real robot if ( m_qRef.data.length() == m_robot->numJoints() ) { Guard guard(m_mutex); // Interpolator - bool transition_interpolator_isEmpty = transition_interpolator->isEmpty(); - // if (DEBUGP) { - // std::cerr << "[" << m_profile.instance_name << "] transition " << transition_interpolator_isEmpty << " " << transition_interpolator_ratio << std::endl; - // } - if (!transition_interpolator_isEmpty) { - transition_interpolator->get(&transition_interpolator_ratio, true); - if (transition_interpolator->isEmpty() && is_active && is_stopping) { - std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl; - is_active = false; - is_stopping = false; + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + std::string arm = itr->first; + size_t arm_idx = ee_index_map[arm]; + bool transition_interpolator_isEmpty = transition_interpolator[arm]->isEmpty(); + if ( ! transition_interpolator_isEmpty ) { + transition_interpolator[arm]->get(&transition_interpolator_ratio[arm_idx], true); + if ( transition_interpolator[arm]->isEmpty() && m_RFUParam[arm].is_active && m_RFUParam[arm].is_stopping ) { + std::cerr << "[" << m_profile.instance_name << "] ReferenceForceUpdater active => inactive." << std::endl; + m_RFUParam[arm].is_active = false; + m_RFUParam[arm].is_stopping = false; + } } } - // If RFU is not active - if ( !is_active ) { + { + bool all_arm_is_not_active = true; + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + std::string arm = itr->first; + size_t arm_idx = ee_index_map[arm]; + if ( m_RFUParam[arm].is_active ) all_arm_is_not_active = false; + else for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = m_ref_force_in[arm_idx].data[j]; + } //determin ref_force_out from ref_force_in - for (unsigned int i=0; iwrite(); } - m_ref_forceOut[i]->write(); + return RTC::RTC_OK; } - return RTC::RTC_OK; } // If RFU is active @@ -371,55 +379,59 @@ RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) } } - // Update reference force - hrp::Vector3 internal_force = hrp::Vector3::Zero(); - size_t arm_idx = ee_index_map[arm]; - double interpolation_time = 0; - if (is_active && loop % update_count == 0) { - 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; - ee_rot = target_link->R * ee_map[arm].localR; - abs_motion_dir = ee_rot * 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]; - double inner_product = 0; - if ( !std::fabs((abs_motion_dir.norm()- 0.0)) < 1e-5) { - abs_motion_dir.normalize(); - inner_product = df.dot(abs_motion_dir); - if ( !(inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) { - hrp::Vector3 in_f = ee_rot * internal_force; - ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (p_gain * inner_product * transition_interpolator_ratio) * abs_motion_dir; - interpolation_time = (1/update_freq) * update_time_ratio; - if (ref_force_interpolator[arm]->isEmpty()) { - ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + // Update reference force + hrp::Vector3 internal_force = hrp::Vector3::Zero(); + std::string arm = itr->first; + size_t arm_idx = ee_index_map[arm]; + double interpolation_time = 0; + bool is_active = m_RFUParam[arm].is_active; + if ( is_active && loop % m_RFUParam[arm].update_count == 0 ) { + 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; + ee_rot = target_link->R * ee_map[arm].localR; + abs_motion_dir = ee_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]; + double inner_product = 0; + if ( ! std::fabs((abs_motion_dir.norm()- 0.0)) < 1e-5 ) { + abs_motion_dir.normalize(); + inner_product = df.dot(abs_motion_dir); + if ( ! (inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) { + hrp::Vector3 in_f = ee_rot * internal_force; + ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (m_RFUParam[arm].p_gain * inner_product * transition_interpolator_ratio[arm_idx]) * abs_motion_dir; + interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio; + if ( ref_force_interpolator[arm]->isEmpty() ) { + ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); + } } } + if ( DEBUGP ) { + std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", 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, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + } } - if (DEBUGP) { - std::cerr << "[" << m_profile.instance_name << "] Updating reference force" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << ", ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << ", 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, ", ", ", ", "", "", " [", "]")) << std::endl; - std::cerr << "[" << m_profile.instance_name << "] act_force = " << tmp_act_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; - std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + if (!ref_force_interpolator[arm]->isEmpty()) { + ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true); } } - if (!ref_force_interpolator[arm]->isEmpty()) { - ref_force_interpolator[arm]->get(ref_force[arm_idx].data(), true); - } } //determin ref_force_out from ref_force_in - for (unsigned int i=0; iwrite(); } @@ -463,72 +475,98 @@ RTC::ReturnCode_t ReferenceForceUpdater::onRateChanged(RTC::UniqueId ec_id) */ -bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) +bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const std::string& i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) { - std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam" << std::endl; + std::string arm = std::string(i_name_); + std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl; Guard guard(m_mutex); - p_gain = i_param.p_gain; - d_gain = i_param.d_gain; - i_gain = i_param.i_gain; - update_freq = i_param.update_freq; - update_count = round((1/update_freq)/m_dt); - update_time_ratio = i_param.update_time_ratio; - arm = std::string(i_param.arm); - for (size_t i = 0; i < 3; i++) motion_dir(i) = i_param.motion_dir[i]; - std::cerr << "[" << m_profile.instance_name << "] p_gain = " << p_gain << ", d_gain = " << d_gain << ", i_gain = " << i_gain << std::endl; - std::cerr << "[" << m_profile.instance_name << "] update_freq = " << update_freq << "[Hz], update_time_ratio = " << update_time_ratio << std::endl; - std::cerr << "[" << m_profile.instance_name << "] motion_dir = " << motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + if ( m_RFUParam[arm].is_active ) { + std::cerr << "[" << m_profile.instance_name << "] Could not set parameters because rfu [" << i_name_ << "] is active" << std::endl; + return false; + } + m_RFUParam[arm].p_gain = i_param.p_gain; + m_RFUParam[arm].d_gain = i_param.d_gain; + m_RFUParam[arm].i_gain = i_param.i_gain; + m_RFUParam[arm].update_freq = i_param.update_freq; + m_RFUParam[arm].update_time_ratio = i_param.update_time_ratio; + m_RFUParam[arm].update_count=round((1/m_RFUParam[arm].update_freq)/m_dt); + + for (size_t i = 0; i < 3; i++ ) m_RFUParam[arm].motion_dir(i) = i_param.motion_dir[i]; + + 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 << std::endl; + std::cerr << "[" << m_profile.instance_name << "] motion_dir = " << m_RFUParam[arm].motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; return true; }; -bool ReferenceForceUpdater::getReferenceForceUpdaterParam(OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) +bool ReferenceForceUpdater::getReferenceForceUpdaterParam(const std::string& i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) { - std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam" << std::endl; + std::string arm = std::string(i_name_); + std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl; + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } Guard guard(m_mutex); - i_param->p_gain = p_gain; - i_param->d_gain = d_gain; - i_param->i_gain = i_gain; - i_param->update_freq = update_freq; - i_param->update_time_ratio = update_time_ratio; - i_param->arm = arm.c_str(); - for (size_t i = 0; i < 3; i++) i_param->motion_dir[i] = motion_dir(i); + i_param->p_gain = m_RFUParam[arm].p_gain; + i_param->d_gain = m_RFUParam[arm].d_gain; + i_param->i_gain = m_RFUParam[arm].i_gain; + i_param->update_freq = m_RFUParam[arm].update_freq; + i_param->update_time_ratio = m_RFUParam[arm].update_time_ratio; + for (size_t i = 0; i < 3; i++ ) i_param->motion_dir[i] = m_RFUParam[arm].motion_dir(i); return true; }; -bool ReferenceForceUpdater::startReferenceForceUpdater() +bool ReferenceForceUpdater::startReferenceForceUpdater(const std::string& i_name_) { - std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater [" << i_name_ << "]" << std::endl; { Guard guard(m_mutex); - if (transition_interpolator->isEmpty()) { - is_active = true; + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + if ( m_RFUParam[i_name_].is_active == true ) + return true; + if (transition_interpolator[i_name_]->isEmpty()) { + m_RFUParam[i_name_].is_active = true; double tmpstart = 0.0, tmpgoal = 1.0; - transition_interpolator->set(&tmpstart); - transition_interpolator->setGoal(&tmpgoal, 1.0, true); + size_t arm_idx = ee_index_map[i_name_]; + hrp::Vector3 currentRefForce( m_ref_force_in[arm_idx].data[0], m_ref_force_in[arm_idx].data[1], m_ref_force_in[arm_idx].data[2] ); + ref_force_interpolator[i_name_]->set(currentRefForce.data()); + transition_interpolator[i_name_]->set(&tmpstart); + transition_interpolator[i_name_]->setGoal(&tmpgoal, 1.0, true); } else { return false; } } - while (!transition_interpolator->isEmpty()) usleep(1000); + while (!transition_interpolator[i_name_]->isEmpty()) usleep(1000); usleep(1000); return true; }; -bool ReferenceForceUpdater::stopReferenceForceUpdater() +bool ReferenceForceUpdater::stopReferenceForceUpdater(const std::string& i_name_) { - std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater [" << i_name_ << "]" << std::endl; { Guard guard(m_mutex); - if (transition_interpolator->isEmpty()) { - double tmpstart = 1.0, tmpgoal = 0.0; - transition_interpolator->set(&tmpstart); - transition_interpolator->setGoal(&tmpgoal, 1.0, true); - is_stopping = true; - } else { + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; return false; } + if ( m_RFUParam[i_name_].is_active == false ){//already not active + return true; + } + double tmpstart = 1.0, tmpgoal = 0.0; + transition_interpolator[i_name_]->set(&tmpstart); + transition_interpolator[i_name_]->setGoal(&tmpgoal, 1.0, true); + m_RFUParam[i_name_].is_stopping = true; } - while (!transition_interpolator->isEmpty()) usleep(1000); + while (!transition_interpolator[i_name_]->isEmpty()) usleep(1000); usleep(1000); return true; }; diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h index 2038358ebcc..ae61135bf0d 100644 --- a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h @@ -101,10 +101,10 @@ class ReferenceForceUpdater // no corresponding operation exists in OpenRTm-aist-0.2.0 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - bool setReferenceForceUpdaterParam(const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); - bool getReferenceForceUpdaterParam(OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); - bool startReferenceForceUpdater(); - bool stopReferenceForceUpdater(); + bool setReferenceForceUpdaterParam(const std::string& i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); + bool getReferenceForceUpdaterParam(const std::string& i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); + bool startReferenceForceUpdater(const std::string& i_name_); + bool stopReferenceForceUpdater(const std::string& i_name_); protected: // Configuration variable declaration @@ -161,6 +161,34 @@ class ReferenceForceUpdater hrp::Vector3 localPos; hrp::Matrix33 localR; }; + struct ReferenceForceUpdaterParam { + // Update frequency [Hz] + double update_freq; + // Update time ratio \in [0,1] + double update_time_ratio; + // P gain + double p_gain; + // D gain + double d_gain; + // I gain + double i_gain; + // Motion direction to update reference force + hrp::Vector3 motion_dir; + int update_count; + bool is_active,is_stopping; + ReferenceForceUpdaterParam () { + //params defined in idl + motion_dir = hrp::Vector3::UnitZ(); + update_freq = 50; // Hz + update_time_ratio = 0.5; + p_gain = 0.02; + d_gain = 0; + i_gain = 0; + //additional params (not defined in idl) + is_active = false; + is_stopping = false; + }; + }; std::map m_vfs; hrp::BodyPtr m_robot; double m_dt; @@ -168,15 +196,13 @@ class ReferenceForceUpdater coil::Mutex m_mutex; std::map ee_map; std::map ee_index_map; + std::map m_RFUParam; std::vector ref_force; std::map ref_force_interpolator; - interpolator* transition_interpolator; - double update_freq, p_gain, d_gain, i_gain, update_time_ratio; - hrp::Vector3 motion_dir; - std::string arm; - bool use_sh_base_pos_rpy, is_active, is_stopping; + std::map transition_interpolator; + std::vector transition_interpolator_ratio; + bool use_sh_base_pos_rpy; int loop;//counter in onExecute - int update_count; }; diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp index 16fbeaa5a07..6a13c216dcb 100644 --- a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp @@ -11,26 +11,26 @@ ReferenceForceUpdaterService_impl::~ReferenceForceUpdaterService_impl() { } -CORBA::Boolean ReferenceForceUpdaterService_impl::setReferenceForceUpdaterParam(const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) +CORBA::Boolean ReferenceForceUpdaterService_impl::setReferenceForceUpdaterParam(const char *i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) { - return m_rfu->setReferenceForceUpdaterParam(i_param); + return m_rfu->setReferenceForceUpdaterParam(std::string(i_name_), i_param); }; -CORBA::Boolean ReferenceForceUpdaterService_impl::getReferenceForceUpdaterParam(OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) +CORBA::Boolean ReferenceForceUpdaterService_impl::getReferenceForceUpdaterParam(const char *i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) { i_param = new OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam(); i_param->motion_dir.length(3); - return m_rfu->getReferenceForceUpdaterParam(i_param); + return m_rfu->getReferenceForceUpdaterParam(std::string(i_name_), i_param); }; -CORBA::Boolean ReferenceForceUpdaterService_impl::startReferenceForceUpdater() +CORBA::Boolean ReferenceForceUpdaterService_impl::startReferenceForceUpdater(const char *i_name_) { - return m_rfu->startReferenceForceUpdater(); + return m_rfu->startReferenceForceUpdater(std::string(i_name_)); }; -CORBA::Boolean ReferenceForceUpdaterService_impl::stopReferenceForceUpdater() +CORBA::Boolean ReferenceForceUpdaterService_impl::stopReferenceForceUpdater(const char *i_name_) { - return m_rfu->stopReferenceForceUpdater(); + return m_rfu->stopReferenceForceUpdater(std::string(i_name_)); }; void ReferenceForceUpdaterService_impl::rfu(ReferenceForceUpdater *i_rfu) diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h index 81caa1fde6d..0c01a399e9f 100644 --- a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h @@ -23,10 +23,10 @@ class ReferenceForceUpdaterService_impl */ virtual ~ReferenceForceUpdaterService_impl(); - CORBA::Boolean setReferenceForceUpdaterParam(const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); - CORBA::Boolean getReferenceForceUpdaterParam(OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); - CORBA::Boolean startReferenceForceUpdater(); - CORBA::Boolean stopReferenceForceUpdater(); + CORBA::Boolean setReferenceForceUpdaterParam(const char *i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); + CORBA::Boolean getReferenceForceUpdaterParam(const char *i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); + CORBA::Boolean startReferenceForceUpdater(const char *i_name_); + CORBA::Boolean stopReferenceForceUpdater(const char *i_name_); void rfu(ReferenceForceUpdater *i_rfu); private: From 3615862a6d2565fa9b9a5d9c05b11a331af6dcf6 Mon Sep 17 00:00:00 2001 From: KOYAMA Ryo Date: Thu, 2 Jun 2016 21:01:19 +0900 Subject: [PATCH 3/3] [sample/SampleRobot/samplerobot_reference_force_updater.py] update rfu sample to check data port --- .../samplerobot_reference_force_updater.py | 62 ++++++++++++++++--- 1 file changed, 54 insertions(+), 8 deletions(-) diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py index bb306be1c69..b32ff4a6536 100755 --- a/sample/SampleRobot/samplerobot_reference_force_updater.py +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -20,17 +20,63 @@ def init (): hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version print("hrpsys_version = %s"%hrpsys_version) + if hcf.rfu != None: + hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut') + hcf.connectLoggerPort(hcf.rfu, 'ref_lhsensorOut') # demo functions def demoReferenceForceUpdater (): - print >> sys.stderr, "1. get param" - p = hcf.rfu_svc.getReferenceForceUpdaterParam()[1] - print >> sys.stderr, "2. start/stop param for left arm" - p.arm = 'larm' - hcf.rfu_svc.setReferenceForceUpdaterParam(p) - hcf.rfu_svc.startReferenceForceUpdater() - time.sleep(1) - hcf.rfu_svc.stopReferenceForceUpdater() + import numpy as np + import sys + i=1; + print >> sys.stderr, i,". get param";i+=1 + p = hcf.rfu_svc.getReferenceForceUpdaterParam('larm')[1] + p.update_freq=100 + p.p_gain=0.1 + # set rmfo + r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] + r_fmop.link_offset_centroid = [0,0.0368,-0.076271] + r_fmop.link_offset_mass = 0.80011 + l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1] + l_fmop.link_offset_centroid = [0,-0.0368,-0.076271] + l_fmop.link_offset_mass = 0.80011 + # Set param + hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop) + hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop) + for armName,portName in zip(['rarm', 'larm'],['ref_rhsensorOut','ref_lhsensorOut']): + hcf.rfu_svc.setReferenceForceUpdaterParam(armName,p) + print >> sys.stderr, i,". set ref_force from seq [10,0,0]";i+=1 + # set ref_force from seq + hcf.seq_svc.setWrenches([0]*12+[10,0,0,0,0,0]*2,1);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) > 9.9; + assert (ret) + # start/stop rfu + print >> sys.stderr, i,". start/stop param for " + armName; i+=1 + ##start rfu + hcf.rfu_svc.startReferenceForceUpdater(armName);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) < 0.1; + assert (ret) + ##stop rfu + hcf.rfu_svc.stopReferenceForceUpdater(armName);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) > 9.9; + assert (ret) + # reset ref_force from seq + print >> sys.stderr, i,". set ref_force from seq [0,0,0]";i+=1 + hcf.seq_svc.setWrenches([0]*24,1);time.sleep(1) + +def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-reference-force-updater-check-port"): + hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) + +def checkDataPortFromLog(port_name, log_fname="/tmp/test-samplerobot-reference-force-updater-check-port",save_log=True, rtc_name="rfu"): + if save_log: + saveLogForCheckParameter(log_fname) + return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) def demo(): init()