RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) { //std::cout << "ImpedanceController::onExecute(" << ec_id << ")" << std::endl; 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(); } } if (m_rpyIn.isNew()) { m_rpyIn.read(); update_rpy = true; } if (m_qCurrentIn.isNew()) { m_qCurrentIn.read(); // if (update_rpy) updateRootLinkPosRot(m_rpy); 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(); 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]); if ( DEBUGP ) { std::cerr << "raw force : " << data_p[0] << " " << data_p[1] << " " << data_p[2] << std::endl; std::cerr << "raw moment : " << data_r[0] << " " << data_r[1] << " " << data_r[2] << std::endl; } hrp::Matrix33 sensorR; if ( sensor ) { // real force sensore sensorR = sensor->link->R * sensor->localR; } else if ( m_sensors.find(sensor_name) != m_sensors.end()) { // virtual force sensor if ( DEBUGP ) { //std::cerr << " targetR: " << target->R << std::endl; std::cerr << " sensorR: " << m_sensors[sensor_name].R << std::endl; } sensorR = m_robot->link(m_sensors[sensor_name].parent_link_name)->R * m_sensors[sensor_name].R; } else { std::cerr << "unknwon force param" << std::endl; } abs_forces[sensor_name] = sensorR * data_p; abs_moments[sensor_name] = sensorR * data_r; if ( DEBUGP ) { hrp::Vector3& tmpf = abs_forces[sensor_name]; hrp::Vector3& tmpm = abs_moments[sensor_name]; std::cerr << "world force : " << tmpf[0] << " " << tmpf[1] << " " << tmpf[2] << std::endl; std::cerr << "world moment : " << tmpm[0] << " " << tmpm[1] << " " << tmpm[2] << std::endl; } } } } if (m_qRefIn.isNew()) { m_qRefIn.read(); m_q.tm = m_qRef.tm; } if ( m_qRef.data.length() == m_robot->numJoints() && m_qCurrent.data.length() == m_robot->numJoints() ) { if ( DEBUGP ) { std::cerr << "qRef : "; for ( int i = 0; i < m_qRef.data.length(); i++ ){ std::cerr << " " << m_qRef.data[i]; } std::cerr << std::endl; } if ( m_impedance_param.size() == 0 ) { for ( int i = 0; i < m_qRef.data.length(); i++ ){ m_q.data[i] = m_qRef.data[i]; } m_qOut.write(); return RTC_OK; } Guard guard(m_mutex); { 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]; qrefv[i] = m_qRef.data[i]; } if (m_rpyRefIn.isNew()) { m_rpyRefIn.read(); //updateRootLinkPosRot(m_rpyRef); } m_robot->calcForwardKinematics(); // set sequencer position to target_p0 for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { ImpedanceParam& param = it->second; param.target_p0 = m_robot->link(param.target_name)->p; param.target_r0 = m_robot->link(param.target_name)->R; } // back to impedance robot model (only for controlled joint) for ( std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { ImpedanceParam& param = it->second; for ( int j = 0; j < param.manip->numJoints(); j++ ){ int i = param.manip->joint(j)->jointId; m_robot->joint(i)->q = qorg[i]; } } if (update_rpy) updateRootLinkPosRot(m_rpy); m_robot->calcForwardKinematics(); } // set m_robot to qRef when deleting status std::map<std::string, ImpedanceParam>::iterator it = m_impedance_param.begin(); while(it != m_impedance_param.end()){ std::string sensor_name = it->first; ImpedanceParam& param = it->second; if ( param.transition_count > 0 ) { hrp::JointPathExPtr manip = param.manip; for ( int j = 0; j < manip->numJoints(); j++ ) { int i = manip->joint(j)->jointId; // index in robot model hrp::Link* joint = m_robot->joint(i); // transition_smooth_gain moves from 0 to 1 // (/ (log (/ (- 1 0.99) 0.99)) 0.5) double transition_smooth_gain = 1/(1+exp(-9.19*(((MAX_TRANSITION_COUNT - param.transition_count) / MAX_TRANSITION_COUNT) - 0.5))); joint->q = ( m_qRef.data[i] - param.transition_joint_q[i] ) * transition_smooth_gain + param.transition_joint_q[i]; } param.transition_count--; if(param.transition_count <= 0){ // erase impedance param std::cerr << "Finished cleanup and erase impedance param " << sensor_name << std::endl; m_impedance_param.erase(it++); }else{ it++; } } else { // use impedance model hrp::Link* base = m_robot->link(param.base_name); hrp::Link* target = m_robot->link(param.target_name); assert(target); assert(base); param.current_p0 = target->p; param.current_r0 = target->R; hrp::JointPathExPtr manip = param.manip; assert(manip); //const int n = manip->numJoints(); hrp::Vector3 dif_pos = hrp::Vector3(0,0,0); hrp::Vector3 vel_pos0 = hrp::Vector3(0,0,0); hrp::Vector3 vel_pos1 = hrp::Vector3(0,0,0); hrp::Vector3 dif_target_pos = hrp::Vector3(0,0,0); hrp::Vector3 dif_rot = hrp::Vector3(0,0,0); hrp::Vector3 vel_rot0 = hrp::Vector3(0,0,0); hrp::Vector3 vel_rot1 = hrp::Vector3(0,0,0); hrp::Vector3 dif_target_rot = hrp::Vector3(0,0,0); // rats/plugins/impedancecontrol.cpp //double M = 5, D = 100, K = 200; // dif_pos = target_p0 (target_coords0) - current_p0(move_coords) // vel_pos0 = current_p0(move_coors) - current_p1(prev_coords0) // vel_pos1 = current_p1(prev_coords0) - current_p2(prev_coords1) // dif_target = target_p0(target_coords0) - target_p1(target_coords1) // // current_p2(prev_coords1) = current_p1(prev_coords0) // currnet_p1(prev_coords0) = current_p0(move_coords) + vel_p // target_p1(target_coords1) = target_p0(target_coords0) if ( DEBUGP ) { std::cerr << "cur0 : " << param.current_p0[0] << " " << param.current_p0[1] << " " << param.current_p0[2] << std::endl; std::cerr << "cur1 : " << param.current_p1[0] << " " << param.current_p1[1] << " " << param.current_p1[2] << std::endl; std::cerr << "cur2 : " << param.current_p2[0] << " " << param.current_p2[1] << " " << param.current_p2[2] << std::endl; std::cerr << "tgt0 : " << param.target_p0[0] << " " << param.target_p0[1] << " " << param.target_p0[2] << std::endl; std::cerr << "tgt1 : " << param.target_p1[0] << " " << param.target_p1[1] << " " << param.target_p1[2] << std::endl; } // if ( DEBUGP ) { // std::cerr << "cur0 : " << param.current_r0[0] << " " << param.current_r0[1] << " " << param.current_r0[2] << std::endl; // std::cerr << "cur1 : " << param.current_r1[0] << " " << param.current_r1[1] << " " << param.current_r1[2] << std::endl; // std::cerr << "cur2 : " << param.current_r2[0] << " " << param.current_r2[1] << " " << param.current_r2[2] << std::endl; // std::cerr << "tgt0 : " << param.target_r0[0] << " " << param.target_r0[1] << " " << param.target_r0[2] << std::endl; // std::cerr << "tgt1 : " << param.target_r1[0] << " " << param.target_r1[1] << " " << param.target_r1[2] << std::endl; // } dif_pos = param.target_p0 - param.current_p0; vel_pos0 = param.current_p0 - param.current_p1; vel_pos1 = param.current_p1 - param.current_p2; dif_target_pos = param.target_p0 - param.target_p1; rats::difference_rotation(dif_rot, param.current_r0, param.target_r0); rats::difference_rotation(vel_rot0, param.current_r1, param.current_r0); rats::difference_rotation(vel_rot1, param.current_r2, param.current_r1); rats::difference_rotation(dif_target_rot, param.target_r1, param.target_r0); if ( DEBUGP ) { std::cerr << "dif_p : " << dif_pos[0] << " " << dif_pos[1] << " " << dif_pos[2] << std::endl; std::cerr << "vel_p0: " << vel_pos0[0] << " " << vel_pos0[1] << " " << vel_pos0[2] << std::endl; std::cerr << "vel_p1: " << vel_pos1[0] << " " << vel_pos1[1] << " " << vel_pos1[2] << std::endl; std::cerr << "dif_t : " << dif_target_pos[0] << " " << dif_target_pos[1] << " " << dif_target_pos[2] << std::endl; } if ( DEBUGP ) { std::cerr << "dif_r : " << dif_rot[0] << " " << dif_rot[1] << " " << dif_rot[2] << std::endl; std::cerr << "vel_r0: " << vel_rot0[0] << " " << vel_rot0[1] << " " << vel_rot0[2] << std::endl; std::cerr << "vel_r1: " << vel_rot1[0] << " " << vel_rot1[1] << " " << vel_rot1[2] << std::endl; std::cerr << "dif_t : " << dif_target_rot[0] << " " << dif_target_rot[1] << " " << dif_target_rot[2] << std::endl; } hrp::Vector3 vel_p, vel_r; //std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << std::endl; //std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << std::endl; // std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << std::endl; // std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl; // ref_force/ref_moment and force_gain/moment_gain are expressed in global coordinates. vel_p = ( param.force_gain * (abs_forces[it->first] - param.ref_force) * m_dt * m_dt + param.M_p * ( vel_pos1 - vel_pos0 ) + param.D_p * ( dif_target_pos - vel_pos0 ) * m_dt + param.K_p * ( dif_pos * m_dt * m_dt ) ) / (param.M_p + (param.D_p * m_dt) + (param.K_p * m_dt * m_dt)); vel_r = ( param.moment_gain * (abs_moments[it->first] - param.ref_moment) * m_dt * m_dt + param.M_r * ( vel_rot1 - vel_rot0 ) + param.D_r * ( dif_target_rot - vel_rot0 ) * m_dt + param.K_r * ( dif_rot * m_dt * m_dt ) ) / (param.M_r + (param.D_r * m_dt) + (param.K_r * m_dt * m_dt)); // generate smooth motion just after impedance started if ( DEBUGP ) { std::cerr << "vel_p : " << vel_p[0] << " " << vel_p[1] << " " << vel_p[2] << std::endl; std::cerr << "vel_r : " << vel_r[0] << " " << vel_r[1] << " " << vel_r[2] << std::endl; } manip->solveLimbIK(vel_p, vel_r, param.transition_count, param.avoid_gain, param.reference_gain, MAX_TRANSITION_COUNT, qrefv, DEBUGP); param.current_p2 = param.current_p1; param.current_p1 = param.current_p0 + vel_p; param.target_p1 = param.target_p0; param.current_r2 = param.current_r1; // if ( std::fabs(vel_r.norm() - 0.0) < ::std::numeric_limits<double>::epsilon() ) { if ( vel_r.norm() != 0.0 ) { hrp::Matrix33 tmpm; rats::rotm3times(tmpm, rats::rotation_matrix(vel_r.norm(), vel_r.normalized()), param.current_r0); param.current_r1 = tmpm; } else { param.current_r1 = param.current_r0; } param.target_r1 = param.target_r0; if ( param.transition_count < 0 ) { param.transition_count++; } it++; } // else } // while if ( m_q.data.length() != 0 ) { // initialized for ( int i = 0; i < m_robot->numJoints(); i++ ){ m_q.data[i] = m_robot->joint(i)->q; } m_qOut.write(); if ( DEBUGP ) { std::cerr << "q : "; for ( int i = 0; i < m_q.data.length(); i++ ){ std::cerr << " " << m_q.data[i]; } std::cerr << std::endl; } } for (size_t i = 0; i < m_ref_forceOut.size(); i++) { m_ref_forceOut[i]->write(); } } else { if ( DEBUGP || loop % 100 == 0 ) { std::cerr << "ImpedanceController is not working..." << std::endl; std::cerr << " m_qRef " << m_qRef.data.length() << std::endl; std::cerr << " m_qCurrent " << m_qCurrent.data.length() << std::endl; } } return RTC::RTC_OK; }
RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) { loop ++; // check dataport input for (unsigned int i=0; i<m_forceIn.size(); i++){ 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(); } if (m_baseRpyIn.isNew()) { m_baseRpyIn.read(); } if (m_rpyIn.isNew()) { m_rpyIn.read(); } if (m_qRefIn.isNew()) { m_qRefIn.read(); } //syncronize m_robot to the real robot if ( m_qRef.data.length() == m_robot->numJoints() ) { Guard guard(m_mutex); // Interpolator for (std::map<std::string, ReferenceForceUpdaterParam>::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 { bool all_arm_is_not_active = true; for (std::map<std::string, ReferenceForceUpdaterParam>::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 if ( all_arm_is_not_active ) { 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]; } m_ref_force_out[i].tm = m_ref_force_in[i].tm; m_ref_forceOut[i]->write(); } return RTC::RTC_OK; } } // If RFU is active { hrp::dvector qorg(m_robot->numJoints()); // 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); 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<hrp::Vector3> foot_pos; std::vector<hrp::Matrix33> foot_rot; std::vector<std::string> 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(); } } for (std::map<std::string, ReferenceForceUpdaterParam>::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; if ( m_RFUParam[arm].frame=="local" ) abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir; else { hrp::Matrix33 current_foot_mid_rot; std::vector<hrp::Matrix33> foot_rot; std::vector<std::string> 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_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); } 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]; 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 (!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; 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(); } return RTC::RTC_OK; }