diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt index 08d5f23bdf6..7599436c7bc 100644 --- a/idl/CMakeLists.txt +++ b/idl/CMakeLists.txt @@ -29,6 +29,8 @@ set(idl_files ReferenceForceUpdaterService.idl AccelerationFilterService.idl ObjectContactTurnaroundDetectorService.idl + WholeBodyMasterSlaveService.idl + HapticControllerService.idl PCDLoaderService.idl ) diff --git a/idl/HapticControllerService.idl b/idl/HapticControllerService.idl new file mode 100644 index 00000000000..e3402e45b28 --- /dev/null +++ b/idl/HapticControllerService.idl @@ -0,0 +1,53 @@ +/** + * @file HapticControllerService.idl + * @brief Services for the HapticController interface + */ +//#include "OpenHRPCommon.idl" +module OpenHRP +{ + + interface HapticControllerService + { + typedef sequence DblSequence2; + typedef sequence DblSequence3; + typedef sequence DblSequence4; + typedef sequence DblSequence6; + typedef sequence , 4> DblSeqSeq6; + typedef double DblArray2[2]; + typedef double DblArray3[3]; + typedef double DblArray4[4]; + typedef sequence StrSequence; + + struct HapticControllerParam + { + double dqAct_filter_cutoff_hz; + double ee_vel_filter_cutoff_hz; + double ex_gravity_compensation_ratio_lower; + double ex_gravity_compensation_ratio_upper; + double baselink_height_from_floor; + double foot_min_distance; + double force_feedback_ratio; + double gravity_compensation_ratio; + double q_friction_coeff; + double q_ref_max_torque_ratio; + double torque_feedback_ratio; + double wrench_hpf_cutoff_hz; + double wrench_lpf_cutoff_hz; + double wrench_hpf_gain; + double wrench_lpf_gain; + DblSequence2 ee_pos_rot_friction_coeff; + DblSequence2 floor_pd_gain; + DblSequence2 foot_horizontal_pd_gain; + DblSequence2 force_feedback_limit_ft; + DblSequence2 q_ref_pd_gain; + DblSeqSeq6 ex_ee_ref_wrench; + }; + boolean startHapticController(); + boolean stopHapticController(); + boolean pauseHapticController(); + boolean resumeHapticController(); + void resetOdom(); + void setParams(in HapticControllerParam i_param); + void getParams(out HapticControllerParam i_param); + }; +}; diff --git a/idl/WholeBodyMasterSlaveService.idl b/idl/WholeBodyMasterSlaveService.idl new file mode 100644 index 00000000000..6c52798c09d --- /dev/null +++ b/idl/WholeBodyMasterSlaveService.idl @@ -0,0 +1,53 @@ +/** + * @file WholeBodyMasterSlaveService.idl + * @brief Services for the WholeBodyMasterSlave interface + */ +//#include "OpenHRPCommon.idl" +module OpenHRP +{ + + interface WholeBodyMasterSlaveService + { + typedef sequence DblSequence2; + typedef sequence DblSequence3; + typedef sequence DblSequence4; + typedef double DblArray2[2]; + typedef double DblArray3[3]; + typedef double DblArray4[4]; + typedef sequence StrSequence; + + struct WholeBodyMasterSlaveParam + { + boolean auto_com_mode; + boolean auto_floor_h_mode; + boolean auto_foot_landing_by_act_cp; + boolean auto_foot_landing_by_act_zmp; + double additional_double_support_time; + double auto_com_foot_move_detect_height; + double auto_floor_h_detect_fz; + double auto_floor_h_reset_fz; + double base_to_hand_min_distance; + double capture_point_extend_ratio; + double com_filter_cutoff_hz; + double foot_collision_avoidance_distance; + double foot_landing_vel; + double force_double_support_com_h; + double human_to_robot_ratio; + double max_double_support_width; + double upper_body_rmc_ratio; + double single_foot_zmp_safety_distance; + double swing_foot_height_offset; + DblSequence3 com_offset; + DblSequence4 actual_foot_vert_fbio; + DblSequence4 safety_foot_vert_fbio; + StrSequence use_joints; + StrSequence use_targets; + }; + boolean startWholeBodyMasterSlave(); + boolean stopWholeBodyMasterSlave(); + boolean pauseWholeBodyMasterSlave(); + boolean resumeWholeBodyMasterSlave(); + void setParams(in WholeBodyMasterSlaveParam i_param); + void getParams(out WholeBodyMasterSlaveParam i_param); + }; +}; diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index 9038aa40369..f58181da606 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -267,6 +267,16 @@ class HrpsysConfigurator(object): octd = None octd_svc = None octd_version = None + + # WholeBodyMasterSLave + wbms = None + wbms_svc = None + wbms_version = None + + # HapticController + hc = None + hc_svc = None + hc_version = None # rtm manager ms = None @@ -379,14 +389,39 @@ def connectComps(self): connectPorts(self.seq.port(sen + "Ref"), self.sh.port(sen + "In")) + # connection for wbms + if self.wbms: + connectPorts(self.sh.port("qOut"), self.wbms.port("qRef")) + connectPorts(self.rh.port("q"), self.wbms.port("qAct")) + connectPorts(self.sh.port("zmpOut"), self.wbms.port("zmpIn")) + connectPorts(self.sh.port("basePosOut"), self.wbms.port("basePosIn")) + connectPorts(self.sh.port("baseRpyOut"), self.wbms.port("baseRpyIn")) + connectPorts(self.sh.port("optionalDataOut"), self.wbms.port("optionalData")) + connectPorts(self.rmfo.port("off_lfsensor"), self.wbms.port("local_lleg_wrench")) + connectPorts(self.rmfo.port("off_rfsensor"), self.wbms.port("local_rleg_wrench")) + connectPorts(self.rmfo.port("off_lhsensor"), self.wbms.port("local_larm_wrench")) + connectPorts(self.rmfo.port("off_rhsensor"), self.wbms.port("local_rarm_wrench")) + # connection for st if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort( self.rh.ref, "rfsensor") and self.st: connectPorts(self.kf.port("rpy"), self.st.port("rpy")) - connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn")) - connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn")) - connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn")) - connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData")) + if self.wbms: +# connectPorts(self.wbms.port("q"), self.ic.port("qRef")) // really needed ? + connectPorts(self.st.port("actCapturePoint"), self.wbms.port("actCapturePoint")) + connectPorts(self.st.port("zmp"), self.wbms.port("zmp")) + connectPorts(self.wbms.port("zmpOut"), self.abc.port("zmpIn")) + connectPorts(self.wbms.port("basePosOut"), self.abc.port("basePosIn")) + connectPorts(self.wbms.port("baseRpyOut"), self.abc.port("baseRpyIn")) + connectPorts(self.wbms.port("optionalDataOut"), self.abc.port("optionalData")) + connectPorts(self.wbms.port("AutoBalancerService"), self.abc.port("AutoBalancerService")) + connectPorts(self.wbms.port("StabilizerService"), self.st.port("StabilizerService")) + else: + connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn")) + connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn")) + connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn")) + connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData")) + connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef")) connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn")) connectPorts(self.abc.port("basePosOut"), self.st.port("basePosIn")) @@ -397,6 +432,7 @@ def connectComps(self): connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq")) connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates")) connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset")) + connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio")) if self.es: connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal")) @@ -409,6 +445,15 @@ def connectComps(self): connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue")) if self.octd: connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates")) + + # connection for hc + if self.hc: + connectPorts(self.sh.port("qOut"), self.hc.port("qRef")) + connectPorts(self.rh.port("q"), self.hc.port("qAct")) + connectPorts(self.rh.port("dq"), self.hc.port("dqAct")) + connectPorts(self.hc.port("tau"), self.rh.port("tauRef")) +# connectPorts(self.hc.port("q"), self.rh.port("??????")) +# connectPorts(self.hc.port("teleopOdom"), self.rh.port("??????")) # ref force moment connection for sen in self.getForceSensorNames(): @@ -502,7 +547,6 @@ def connectComps(self): if self.rmfo: for sen in filter(lambda x: x.type == "Force", self.sensors): connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name)) - # connection for gc if self.gc: connectPorts(self.rh.port("q"), self.gc.port("qCurrent")) # other connections @@ -749,6 +793,8 @@ def getRTCListUnstable(self): ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], ['rfu', "ReferenceForceUpdater"], + ['wbms', "WholeBodyMasterSlave"], + ['hc', "HapticController"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], @@ -767,8 +813,8 @@ def getJointAngleControllerList(self): '''!@brief Get list of controller list that need to control joint angles ''' - controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, - self.tc, self.hes, self.el] +# controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] + controller_list = [self.es, self.wbms, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] return filter(lambda c: c != None, controller_list) # only return existing controllers def getRTCInstanceList(self, verbose=True): @@ -882,6 +928,36 @@ def setupLogger(self, maxLength=4000): self.connectLoggerPort(self.sh, 'zmpOut') if self.ic != None: self.connectLoggerPort(self.ic, 'q') + if self.hc != None: + self.connectLoggerPort(self.hc, 'tau') + self.connectLoggerPort(self.hc, 'debugData') + self.connectLoggerPort(self.hc, 'master_rleg_pose') + self.connectLoggerPort(self.hc, 'master_lleg_pose') + self.connectLoggerPort(self.hc, 'master_rarm_pose') + self.connectLoggerPort(self.hc, 'master_larm_pose') + self.connectLoggerPort(self.hc, 'master_com_pose') + self.connectLoggerPort(self.hc, 'master_head_pose') + self.connectLoggerPort(self.hc, 'master_rleg_wrench_dbg') + self.connectLoggerPort(self.hc, 'master_lleg_wrench_dbg') + self.connectLoggerPort(self.hc, 'master_rarm_wrench_dbg') + self.connectLoggerPort(self.hc, 'master_larm_wrench_dbg') + + if self.wbms != None: + self.connectLoggerPort(self.wbms, 'q') + self.connectLoggerPort(self.wbms, 'basePosOut') + self.connectLoggerPort(self.wbms, 'baseRpyOut') + self.connectLoggerPort(self.wbms, 'zmpOut') + self.connectLoggerPort(self.wbms, 'baseTformOut') + self.connectLoggerPort(self.wbms, 'slave_rleg_wrench') + self.connectLoggerPort(self.wbms, 'slave_lleg_wrench') + self.connectLoggerPort(self.wbms, 'slave_rarm_wrench') + self.connectLoggerPort(self.wbms, 'slave_larm_wrench') + self.connectLoggerPort(self.wbms, 'slave_rleg_pose') + self.connectLoggerPort(self.wbms, 'slave_lleg_pose') + self.connectLoggerPort(self.wbms, 'slave_rarm_pose') + self.connectLoggerPort(self.wbms, 'slave_larm_pose') + self.connectLoggerPort(self.wbms, 'slave_com_pose') + self.connectLoggerPort(self.wbms, 'slave_head_pose') if self.abc != None: self.connectLoggerPort(self.abc, 'zmpOut') self.connectLoggerPort(self.abc, 'baseTformOut') diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h new file mode 100644 index 00000000000..a4201b892fd --- /dev/null +++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h @@ -0,0 +1,302 @@ +#ifndef FULLBODYIK_H +#define FULLBODYIK_H + +#include +#include +#include "../ImpedanceController/RatsMatrix.h" +#include "../TorqueFilter/IIRFilter.h" +#include "MyUtils.h" +//#include +//#include + + +#define OPENHRP_PACKAGE_VERSION_320 + +class IKConstraint { + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + std::string target_link_name; + hrp::Vector3 targetPos; + hrp::Vector3 targetRpy; + hrp::Vector3 localPos; + hrp::Matrix33 localR; + hrp::dvector6 constraint_weight; + double pos_precision, rot_precision; + IKConstraint () + :targetPos( hrp::Vector3::Zero()), + targetRpy( hrp::Vector3::Zero()), + localPos( hrp::Vector3::Zero()), + localR( hrp::Matrix33::Identity()), + constraint_weight( hrp::dvector6::Ones()), + pos_precision(1e-4), rot_precision(deg2rad(0.1)){} + hrp::Vector3 getCurrentTargetPos(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->p + _robot->link(target_link_name)->R * localPos; } + hrp::Matrix33 getCurrentTargetRot(const hrp::BodyPtr _robot){ return _robot->link(target_link_name)->R * localR; } + hrp::Pose3 getCurrentTargetPose(const hrp::BodyPtr _robot){ return hrp::Pose3(getCurrentTargetPos(_robot), getCurrentTargetRot(_robot)); } +}; + + +class FullbodyInverseKinematicsSolver{ + protected: + const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF; + const double m_dt; + hrp::dmatrix J_all; + hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old; + // BiquadIIRFilterVec am_filters; + public: + hrp::BodyPtr m_robot; + hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight; + hrp::Vector3 cur_momentum_around_COM, cur_momentum_around_COM_filtered, rootlink_rpy_llimit, rootlink_rpy_ulimit; + FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt): + m_robot(_robot), + m_dt(_dt), + WS_DOF(6), + BASE_DOF(6), + J_DOF(_robot->numJoints()), + ALL_DOF(J_DOF+BASE_DOF){ + dq_weight_all = hrp::dvector::Ones(ALL_DOF); + q_ref = hrp::dvector::Zero(ALL_DOF); + q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF); + q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 1e-2); + jlim_avoid_weight_old = hrp::dvector::Zero(ALL_DOF); + cur_momentum_around_COM = hrp::Vector3::Zero(); + cur_momentum_around_COM_filtered = hrp::Vector3::Zero(); + rootlink_rpy_llimit = hrp::Vector3::Constant(-DBL_MAX); + rootlink_rpy_ulimit = hrp::Vector3::Constant(DBL_MAX); +// am_filters.resize(3); +// am_filters.setParameter(10, 1.0/m_dt, Q_BUTTERWORTH); +// am_filters.reset(0); + }; + ~FullbodyInverseKinematicsSolver () {}; + int numStates(){ return ALL_DOF; }; + bool checkIKConvergence(const std::vector& _ikc_list){ + for ( int i=0; i<_ikc_list.size(); i++ ) { + const hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name); + hrp::Vector3 pos_err, rot_err; + if(link_tgt_ptr){ + pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos); + rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy)); + } + else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ // COM + pos_err = _ikc_list[i].targetPos - (m_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos); + rot_err = _ikc_list[i].targetRpy - cur_momentum_around_COM; + } + else{ std::cerr<<"Unknown Link Target !!"< 0 && fabs(pos_err(j)) > _ikc_list[i].pos_precision){return false;} + if(_ikc_list[i].constraint_weight.tail(3)(j) > 0 && fabs(rot_err(j)) > _ikc_list[i].rot_precision){return false;} + } + } + return true; + } + int solveFullbodyIKLoop (const std::vector& _ikc_list, const int _max_iteration) { + #ifdef OPENHRP_PACKAGE_VERSION_320 + const hrp::Vector3 base_p_old = m_robot->rootLink()->p; + const hrp::Matrix33 base_R_old = m_robot->rootLink()->R; + const hrp::dvector q_old = hrp::getQAll(m_robot); + unsigned int loop; + for(loop=0; loop < _max_iteration;){ + solveFullbodyIKOnce(_ikc_list); + //check ang moment + m_robot->rootLink()->v = (m_robot->rootLink()->p - base_p_old)/ m_dt; + m_robot->rootLink()->w = base_R_old * hrp::omegaFromRotEx(base_R_old.transpose() * m_robot->rootLink()->R) / m_dt; + for(int i=0;ijoint(i)->dq = (m_robot->joint(i)->q - q_old(i)) / m_dt; } + m_robot->calcForwardKinematics(true,false); + hrp::Vector3 tmp_P, tmp_L; + m_robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す + cur_momentum_around_COM = tmp_L - m_robot->calcCM().cross(tmp_P); +// m_robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す +// cur_momentum_around_COM_filtered = am_filters.passFilter(cur_momentum_around_COM);//フィルター位相遅れのせいか常にウネウネしてる + cur_momentum_around_COM_filtered = cur_momentum_around_COM; + loop++; + if(checkIKConvergence(_ikc_list)){ break; } + } + return loop; + #else + std::cerr<<"solveFullbodyIKLoop() needs OPENHRP_PACKAGE_VERSION_320 !!!"<& _ikc_list){ + #ifdef OPENHRP_PACKAGE_VERSION_320 + + // count up all valid constraint DOF and allocate Jacobian and vectors + const hrp::dmatrix q_select_mat = hrp::to_SelectionMat(dq_weight_all); + const int VALID_Q_NUM = q_select_mat.rows(); + if(VALID_Q_NUM == 0){ return; } + int VALID_C_NUM = 0; // count up valid constraint num + for (int i=0;i<_ikc_list.size();i++){ VALID_C_NUM += (_ikc_list[i].constraint_weight.array()>0.0).count(); }// count up valid end effector constraint num + VALID_C_NUM += (q_ref_constraint_weight.array()>0.0).count();// count up valid reference joint angle constraint num + J_all = hrp::dmatrix::Zero(VALID_C_NUM, VALID_Q_NUM); + err_all = hrp::dvector::Zero(VALID_C_NUM); + constraint_weight_all = hrp::dvector::Ones(VALID_C_NUM); + + // set end effector constraints into Jacobian + int CURRENT_C_COUNT = 0; + for ( int i=0; i<_ikc_list.size(); i++ ) { + hrp::Link* link_tgt_ptr = m_robot->link(_ikc_list[i].target_link_name); + hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン + hrp::dvector6 dp_part; //pos + rot 速度ではなく差分 + if(link_tgt_ptr){//ベースリンク，通常リンク共通 + hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos; + hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR; + dp_part.head(3) = _ikc_list[i].targetPos - tgt_cur_pos; + dp_part.tail(3) = tgt_cur_rot * hrp::omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy)); + hrp::JointPath tgt_jpath(m_robot->rootLink(), link_tgt_ptr); + hrp::dmatrix J_jpath; + tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos); + for(int id_in_jpath=0; id_in_jpathjointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え + J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF, BASE_DOF ); + J_part.rightCols(BASE_DOF).topRightCorner(3,3) = - hrp::hat(tgt_cur_pos - m_robot->rootLink()->p); + } + else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定 + dp_part.head(3) = _ikc_list[i].targetPos - m_robot->calcCM(); + dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ) + hrp::dmatrix J_com, J_am; + m_robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる + m_robot->calcAngularMomentumJacobian(NULL, J_am);//base=NULLの時は重心周りの角運動量っぽい？ + J_part << J_com, J_am; + } + else{ + std::cerr<<"Unknown Link Target !! "<< _ikc_list[i].target_link_name << std::endl; continue; //不明なリンク指定 + } + // set one of the end effector constraints into Jacobian + hrp::dmatrix c_part_selection_mat = hrp::to_SelectionMat(_ikc_list[i].constraint_weight); // select only valid end effector constraint + if(c_part_selection_mat.rows() != 0 && c_part_selection_mat.cols() != 0 ){ + + // 明らかに可到達でない条件の誤差は頭打ちしないと，いくら重み付きでも引きずられる? + dp_part = dp_part.cwiseMin(hrp::dvector6::Constant(0.1)).cwiseMax(hrp::dvector6::Constant(-0.1)); + + J_all.middleRows (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * J_part * q_select_mat.transpose(); + err_all.segment (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * dp_part; + constraint_weight_all.segment (CURRENT_C_COUNT, c_part_selection_mat.rows()) = c_part_selection_mat * _ikc_list[i].constraint_weight; + CURRENT_C_COUNT += c_part_selection_mat.rows(); + } + } + + // set reference joint angle constraints into Jacobian + const hrp::dmatrix q_ref_selection_mat = hrp::to_SelectionMat(q_ref_constraint_weight); + if(q_ref_selection_mat.rows() != 0 && q_ref_selection_mat.cols() != 0 ){ + J_all.middleRows (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * hrp::dmatrix::Identity(ALL_DOF,ALL_DOF) * q_select_mat.transpose(); + err_all.segment (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * ((q_ref - hrp::getRobotStateVec(m_robot)).cwiseMin(q_ref_max_dq).cwiseMax(-q_ref_max_dq)); + constraint_weight_all.segment (CURRENT_C_COUNT, q_ref_selection_mat.rows()) = q_ref_selection_mat * q_ref_constraint_weight; + CURRENT_C_COUNT += q_ref_selection_mat.rows(); + } + if(CURRENT_C_COUNT != VALID_C_NUM){ std::cerr <<"[FullbodyIK] CURRENT_C_COUNT != VALID_C_NUM, something wrong !" << std::endl; } + + // joint limit avoidance (copy from JointPathEx) + hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF); + for ( int j = 0; j < ALL_DOF ; j++ ) { + double jang, jmax, jmin; + if(jjoint(j)->q; + jmax = m_robot->joint(j)->ulimit; + jmin = m_robot->joint(j)->llimit; + }else if(jrootLink()->R)(rpy_id); + jmax = rootlink_rpy_ulimit(rpy_id); + jmin = rootlink_rpy_llimit(rpy_id); + } + const double e = deg2rad(1); + if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { + } else if ( eps_eq(jang, jmax,e) ) { + jang = jmax - e; + } else if ( eps_eq(jang, jmin,e) ) { + jang = jmin + e; + } + double jlim_avoid_weight; + if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { + jlim_avoid_weight = DBL_MAX; + } else { + jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) ); + if (std::isnan(jlim_avoid_weight)) jlim_avoid_weight = 0; + } + if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit + dq_weight_all_jlim(j) += jlim_avoid_weight; + } + jlim_avoid_weight_old(j) = jlim_avoid_weight; + } + hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array(); + dq_weight_all_final = q_select_mat * dq_weight_all_final; + + // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009] + // q = q + H^(-1) * g + // g = J^T * We * e + // H = J^T * We * J + Wn + // Wn = (e^T * We * e) * 1 + \bar{Wn} // \bar{Wn} = \bar{wn} * E + // = (e^T * We * e + \bar{wn}) * E + // Wn = Wn * Wq // modify to insert dq weight + const double wn_const = 1e-6; + const hrp::dmatrix Wn = (static_cast(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * dq_weight_all_final.asDiagonal(); + const hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn; + const hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all; + dq_all = H.ldlt().solve(g); // dq_all = H.inverse() * g; is slow + + // rtconf localhost:15005/wbms.rtc set debugLevel 1 とかにしたい + static int count; +#if 0 + if(count++ % 10000 == 0){ + std::cout<joint(i)->q += dq_all(i); + LIMIT_MINMAX(m_robot->joint(i)->q, m_robot->joint(i)->llimit, m_robot->joint(i)->ulimit); + } + + // rootlink rpy limit ??? + for(int i=0;i<3;i++){ + if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0; + if(hrp::rpyFromRot(m_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0; + } + + // update rootlink pos rot + m_robot->rootLink()->p += dq_all.tail(6).head(3); + const hrp::Vector3 omega = dq_all.tail(6).tail(3); + hrp::Matrix33 dR; + if(omega.norm() > 1e-12){ + hrp::calcRodrigues(dR, omega.normalized(), omega.norm()); + }else{ + dR = hrp::Matrix33::Identity(); + } + rats::rotm3times(m_robot->rootLink()->R, dR, m_robot->rootLink()->R); // safe rot operation with quartanion normalization + if(!m_robot->rootLink()->R.isUnitary()){ + std::cerr <<"[FullbodyIK] WARN m_robot->rootLink()->R is not Unitary, something wrong !" << std::endl; + } + m_robot->calcForwardKinematics(); + #else + std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"< +#include +#include +#include +#include "../RobotHardware/defs.h" + +#include + +enum { R, L, LR }; +//enum {X, Y, Z}; //already exist in RobotHardware/defs.h +enum { XY = 2 }; +enum { XYZ = 3 }; +enum { r, p, y, rpy }; +enum { fx, fy, fz, tx, ty, tz, ft_xyz }; +enum { com, rf, lf, rh, lh, head, zmp, num_pose_tgt }; +enum { num_ee_tgt=4 }; + +#define LR_STR(lr) (lr==R ? "R" : "L") +#define OPPOSITE(lr) (lr==R ? L : R) + +static const double G = 9.80665; +static const double Q_NOOVERSHOOT = 0.5; +static const double Q_BUTTERWORTH = 0.707106781; + +#define dbg(var) std::cerr<<#var"= "<<(var)<max ? max : x))) +#define LIMIT_MIN(x,min) (x= ( xmax ? max : x )) +#define LIMIT_MINMAX(x,min,max) (x= ( xmax ? max : x ))) +#define LIMIT_NORM_V(v,max) if(v.norm()>max){v=v.normalized()*max;} +#define LIMIT_MIN_V(v,minv) (v= v.cwiseMax(minv)) +#define LIMIT_MAX_V(v,maxv) (v= v.cwiseMin(maxv)) +#define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv)) +#define MIN_LIMITED(x,min) ( xmax ? max : x ) +#define MINMAX_LIMITED(x,min,max) ( xmax ? max : x )) +#define NORM_LIMITED(x,max) ( x<(-max) ? -max : (x>max ? max : x)) + + +namespace hrp{ + class Pose3{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + hrp::Vector3 p; + hrp::Matrix33 R; + Pose3() { reset();} + Pose3(const double _X, const double _Y, const double _Z, const double _r, const double _p, const double _y) { p << _X,_Y,_Z; R = hrp::rotFromRpy(_r,_p,_y); } + Pose3(const hrp::dvector6& _xyz_rpy) { p = _xyz_rpy.head(3); R = hrp::rotFromRpy(_xyz_rpy.tail(3)); } +// Pose3(const hrp::Vector3& _xyz, const hrp::Vector3& _rpy) { p = _xyz; R = hrp::rotFromRpy(_rpy); }// ambiguous overload... + Pose3(const hrp::Vector3& _xyz, const hrp::Matrix33& _R) { p = _xyz; R = _R; } + void reset() { p.fill(0); R.setIdentity(); } + hrp::Vector3 rpy() const { return hrp::rpyFromRot(R); } + hrp::dvector6 to_dvector6() const { return (hrp::dvector6() << p, hrp::rpyFromRot(R)).finished(); } + void setRpy(const double _r, const double _p, const double _y) { R = rotFromRpy(_r,_p,_y); } + void setRpy(const hrp::Vector3& _rpy) { R = rotFromRpy(_rpy); } + }; + inline Pose3 calcRelPose3(const Pose3& from, const Pose3& to){ + return Pose3( from.R.transpose() * (to.p - from.p), from.R.transpose() * to.R ); + } + inline Pose3 applyRelPose3(const Pose3& tgt, const Pose3& transform){ + return Pose3( tgt.p + tgt.R * transform.p, tgt.R * transform.R); + } + inline Pose3 to_2DPlanePose3(const Pose3& in){ + return Pose3(in.p(X), in.p(Y), 0, 0, 0, in.rpy()(y)); + } + + inline hrp::Vector3 to_Vector3 (const RTC::Point3D& in) { return hrp::Vector3(in.x, in.y, in.z); } + inline hrp::Vector3 to_Vector3 (const RTC::Orientation3D& in) { return hrp::Vector3(in.r, in.p, in.y); } + inline hrp::dvector6 to_dvector6 (const RTC::Pose3D& in) { return (hrp::dvector6() << in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y).finished(); } + inline hrp::Pose3 to_Pose3 (const RTC::Pose3D& in) { return hrp::Pose3(in.position.x, in.position.y, in.position.z, in.orientation.r, in.orientation.p, in.orientation.y); } + inline hrp::dvector6 to_dvector6 (const OpenHRP::Wrench& in) { return (hrp::dvector6() << in.force.x, in.force.y, in.force.z, in.torque.x, in.torque.y, in.torque.z).finished(); } + inline hrp::dvector to_dvector (const RTC::TimedDoubleSeq::_data_seq& in) { return hrp::dvector::Map(in.get_buffer(), in.length()); } + + inline RTC::Point3D to_Point3D (const hrp::Vector3& in) { return (RTC::Point3D){in(X),in(Y),in(Z)}; } + inline RTC::Orientation3D to_Orientation3D(const hrp::Vector3& in) { return (RTC::Orientation3D){in(X),in(Y),in(Z)}; } + inline RTC::Pose3D to_Pose3D (const hrp::dvector6& in) { return (RTC::Pose3D){in(X),in(Y),in(Z),in(r),in(p),in(y)}; } + inline RTC::Pose3D to_Pose3D (const hrp::Pose3& in) { return (RTC::Pose3D){in.p(X),in.p(Y),in.p(Z),in.rpy()(r),in.rpy()(p),in.rpy()(y)}; } + inline OpenHRP::Wrench to_Wrench (const hrp::dvector6& in) { return (OpenHRP::Wrench){in(X),in(Y),in(Z),in(r),in(p),in(y)}; } + inline RTC::TimedDoubleSeq::_data_seq to_DoubleSeq (const hrp::dvector& in) { RTC::TimedDoubleSeq::_data_seq out; out.length(in.size()); hrp::dvector::Map(out.get_buffer(), in.size()) = in; return out; } + + inline hrp::Pose3 getLinkPose3 (const hrp::Link* _link) { return hrp::Pose3(_link->p, _link->R); } + inline void setLinkPose3 (hrp::Link* _link, const hrp::Pose3& val){ _link->p = val.p; _link->R = val.R; } + inline hrp::dvector getQAll (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; } + inline void setQAll (hrp::BodyPtr _robot, const hrp::dvector& in){ assert(in.size() <= _robot->numJoints()); for(int i=0;ijoint(i)->q = in(i); } } + inline hrp::dvector getRobotStateVec(const hrp::BodyPtr _robot){ return (hrp::dvector(_robot->numJoints()+6) << getQAll(_robot), _robot->rootLink()->p, hrp::rpyFromRot(_robot->rootLink()->R)).finished(); } + inline void setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q_bpos_brpy){ assert(_q_bpos_brpy.size() == _robot->numJoints()+6); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q_bpos_brpy(i); }; _robot->rootLink()->p = _q_bpos_brpy.tail(6).head(3); _robot->rootLink()->R = hrp::rotFromRpy(_q_bpos_brpy.tail(6).tail(3)); } + inline void setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Vector3& _brpy){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = hrp::rotFromRpy(_brpy); } + inline void setRobotStateVec(hrp::BodyPtr _robot, const hrp::dvector& _q, const hrp::Vector3& _bpos, const hrp::Matrix33& _bR){ assert(_q.size() == _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = _q(i); }; _robot->rootLink()->p = _bpos; _robot->rootLink()->R = _bR; } + inline std::vector getJointNameAll (const hrp::BodyPtr _robot){ std::vector ret(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ ret[i] = _robot->joint(i)->name; } return ret; } + inline hrp::dvector getUAll (const hrp::BodyPtr _robot){ hrp::dvector tmp(_robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->u; } return tmp; } + + + inline std::vector to_string_vector (const RTC::TimedStringSeq::_data_seq& in) { + std::vector ret(in.length()); for(int i=0; i::epsilon()) { //th=PI + return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th ); + } + double k = -0.5 * th / s; + return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k ); + } + } + inline double hrpVector2Cross(const hrp::Vector2& a, const hrp::Vector2& b){ return a(X)*b(Y)-a(Y)*b(X); } + inline hrp::dmatrix to_SelectionMat(const hrp::dvector& in) { + hrp::dmatrix ret = hrp::dmatrix::Zero( (in.array() > 0.0).count(), in.size() ); + if(ret.rows() != 0 && ret.cols() != 0){ + for (int row=0, col=0; col< in.size(); col++){ + if(in(col) > 0.0){ + ret(row, col) = 1; + row++; + } + } + } + return ret; + } +} + + +class BiquadIIRFilterVec{/// safe ver + private: + std::vector filters; + hrp::dvector ans; + public: + BiquadIIRFilterVec(){} + BiquadIIRFilterVec(const int len){resize(len);} + void resize(const int len){filters.resize(len); ans.resize(len);} + ~BiquadIIRFilterVec(){} + void setParameter(const hrp::dvector& fc_in, const double HZ, const double Q = 0.5){ for(int i=0;i v, const std::string& s){ return (std::find(v.begin(), v.end(), s) != v.end());} + +inline std::ostream& operator<<(std::ostream& os, hrp::Pose3& in){ + os << "p = " << in.p.transpose() << std::endl; + os << "R =\n" << in.R << std::endl; + os << "(rpy) = " << hrp::rpyFromRot(in.R).transpose() << std::endl; + return os; +} + +template +inline std::ostream& operator<<(std::ostream& os, std::vector& in){ + for(int i=0; i #include "../ImpedanceController/JointPathEx.h" #include "../ImpedanceController/RatsMatrix.h" +#include "AutoBalancerService_impl.h" // Class for Simple Fullbody Inverse Kinematics // Input : target root pos and rot, target COG, target joint angles, target EE coords @@ -11,7 +12,7 @@ // Algorithm : Limb IK + move base class SimpleFullbodyInverseKinematicsSolver { -private: +protected: // Robot model for IK hrp::BodyPtr m_robot; // Org (current) joint angles before IK diff --git a/rtc/CMakeLists.txt b/rtc/CMakeLists.txt index a292b3849f6..c3bca7b9ddc 100644 --- a/rtc/CMakeLists.txt +++ b/rtc/CMakeLists.txt @@ -58,6 +58,8 @@ add_subdirectory(Beeper) add_subdirectory(ReferenceForceUpdater) add_subdirectory(AccelerationFilter) add_subdirectory(ObjectContactTurnaroundDetector) +add_subdirectory(WholeBodyMasterSlave) +add_subdirectory(HapticController) if (NOT APPLE AND USE_HRPSYSUTIL) add_subdirectory(VideoCapture) diff --git a/rtc/HapticController/CMakeLists.txt b/rtc/HapticController/CMakeLists.txt new file mode 100644 index 00000000000..b5a203ecc74 --- /dev/null +++ b/rtc/HapticController/CMakeLists.txt @@ -0,0 +1,21 @@ +set(comp_sources HapticController.cpp HapticControllerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp) +set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +add_library(HapticController SHARED ${comp_sources}) +target_link_libraries(HapticController ${libs}) +set_target_properties(HapticController PROPERTIES PREFIX "") + +add_executable(HapticControllerComp HapticControllerComp.cpp ${comp_sources}) +target_link_libraries(HapticControllerComp ${libs}) + +include_directories(${PROJECT_SOURCE_DIR}/rtc/SequencePlayer) + +set(target HapticController HapticControllerComp ) + +#add_test(testPreviewControllerNoGP testPreviewController --use-gnuplot false) + +install(TARGETS ${target} + RUNTIME DESTINATION bin CONFIGURATIONS Release Debug + LIBRARY DESTINATION lib CONFIGURATIONS Release Debug +) + diff --git a/rtc/HapticController/HapticController.cpp b/rtc/HapticController/HapticController.cpp new file mode 100644 index 00000000000..e558e214ced --- /dev/null +++ b/rtc/HapticController/HapticController.cpp @@ -0,0 +1,727 @@ +#include "HapticController.h" + +#define DEBUGP (loop%200==0) +#define DEBUGP_ONCE (loop==0) + +static const char* HapticController_spec[] = { + "implementation_id", "HapticController", + "type_name", "HapticController", + "description", "hapticcontroller component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + "conf.default.debugLevel", "0", + "" +}; + +HapticController::HapticController(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), + m_qRefIn("qRef", m_qRef), + m_qActIn("qAct", m_qAct), + m_dqActIn("dqAct", m_dqAct), + m_qOut("q", m_qRef), + m_tauOut("tau", m_tau), + m_teleopOdomOut("teleopOdom", m_teleopOdom), + m_debugDataOut("debugData", m_debugData), + m_delayCheckPacketInboundIn("delay_check_packet_inbound", m_delayCheckPacket), + m_delayCheckPacketOutboundOut("delay_check_packet_outbound", m_delayCheckPacket), + m_HapticControllerServicePort("HapticControllerService"), + m_debugLevel(0) +{ + m_service0.hapticcontroller(this); +} + +HapticController::~HapticController(){} + +RTC::ReturnCode_t HapticController::onInitialize(){ + RTC_INFO_STREAM("onInitialize()"); + bindParameter("debugLevel", m_debugLevel, "0"); + addInPort("qRef", m_qRefIn); + addInPort("qAct", m_qActIn); + addInPort("dqAct", m_dqActIn); + addOutPort("q", m_qOut); + addOutPort("tau", m_tauOut); + addOutPort("teleopOdom", m_teleopOdomOut); + addOutPort("debugData", m_debugDataOut); + addInPort("delay_check_packet_inbound", m_delayCheckPacketInboundIn); + addOutPort("delay_check_packet_outbound", m_delayCheckPacketOutboundOut); + m_HapticControllerServicePort.registerProvider("service0", "HapticControllerService", m_service0); + addPort(m_HapticControllerServicePort); + + RTC::Properties& prop = getProperties(); + coil::stringTo(m_dt, prop["dt"].c_str()); + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ comPos = nameServer.length(); } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + + m_robot = hrp::BodyPtr(new hrp::Body()); + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){ + RTC_WARN_STREAM("failed to load model[" << prop["model"] << "]"); + return RTC::RTC_ERROR; + } + m_robot_odom = hrp::BodyPtr(new hrp::Body(*m_robot)); //copy + + setupEEIKConstraintFromConf(ee_ikc_map, m_robot, prop); + RTC_INFO_STREAM("setupEEIKConstraintFromConf finished"); + + const double tmp_init = 0; + t_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1); + t_ip->clear(); + t_ip->set(&tmp_init); + t_ip->get(&output_ratio, false); + q_ref_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1); + q_ref_ip->clear(); + q_ref_ip->set(&tmp_init); + q_ref_ip->get(&q_ref_output_ratio, false); + default_baselink_h_from_floor = 1.5; + hcp.baselink_height_from_floor = default_baselink_h_from_floor; + baselink_h_ip = new interpolator(1, m_dt, interpolator::LINEAR, 1); + baselink_h_ip->clear(); + baselink_h_ip->set(&default_baselink_h_from_floor); + baselink_h_ip->get(&baselink_h_from_floor, false); + RTC_INFO_STREAM("setup interpolator finished"); + + current_adjust_floor_h = 0; + + dqAct_filter = BiquadIIRFilterVec(m_robot->numJoints()); + dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + dqAct_filter.reset(0); + for (auto ee : ee_names){ + ee_vel_filter[ee] = BiquadIIRFilterVec(6); + ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + ee_vel_filter[ee].reset(0); + wrench_lpf_for_hpf[ee] = BiquadIIRFilterVec(6); + wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + wrench_lpf_for_hpf[ee].reset(0); + wrench_lpf[ee] = BiquadIIRFilterVec(6); + wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + wrench_lpf[ee].reset(0); + } + + tgt_names = ee_names; + tgt_names.push_back("com"); + tgt_names.push_back("head"); + tgt_names.push_back("rhand"); + tgt_names.push_back("lhand"); + tgt_names.push_back("rfloor"); + tgt_names.push_back("lfloor"); + + for (auto ee : ee_names){ + std::string n = "slave_"+ee+"_wrench"; + m_slaveEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort(n.c_str(), m_slaveEEWrenches[ee])); + registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee]); + m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input + RTC_INFO_STREAM(" registerInPort " << n ); + } + for (auto tgt : tgt_names){ + std::string n = "master_"+tgt+"_pose"; + m_masterTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort(n.c_str(), m_masterTgtPoses[tgt])); + registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt]); + RTC_INFO_STREAM(" registerOutPort " << n); + } + + // sub usage + for (auto ee : ee_names){ + std::string n = "master_"+ee+"_wrench"; + m_masterEEWrenchesOut[ee] = OTDS_Ptr(new RTC::OutPort(n.c_str(), m_masterEEWrenches[ee])); + registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee]); + m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(hrp::dvector6::Zero()); // avoid non-zero initial input + RTC_INFO_STREAM(" registerOutPort " << n ); + } + for (auto tgt : tgt_names){ + std::string n = "slave_"+tgt+"_pose"; + m_slaveTgtPosesIn[tgt] = ITP3_Ptr(new RTC::InPort(n.c_str(), m_slaveTgtPoses[tgt])); + registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt]); + RTC_INFO_STREAM(" registerInPort " << n ); + } + + m_tau.data = hrp::to_DoubleSeq(hrp::dvector::Zero(m_robot->numJoints())); + + loop = 0; + resetOdom_request = false; + RTC_INFO_STREAM("onInitialize() OK"); + return RTC::RTC_OK; +} + +RTC::ReturnCode_t HapticController::setupEEIKConstraintFromConf(std::map& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){ + std::vector ee_conf_all = coil::split(_prop["end_effectors"], ","); + size_t prop_num = 10; // limbname + linkname + basename + pos(3) + angleaxis(4) + if (ee_conf_all.size() > 0) { + size_t ee_num = ee_conf_all.size()/prop_num; + for (size_t i = 0; i < ee_num; i++) { + std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST + ee_name = ee_conf_all[i*prop_num]; + target_link_name = ee_conf_all[i*prop_num+1]; + base_name = ee_conf_all[i*prop_num+2]; + ee_names.push_back(ee_name); + _ee_ikc_map[ee_name].target_link_name = target_link_name; + for (size_t j = 0; j < XYZ; j++){ _ee_ikc_map[ee_name].localPos(j) = std::atof( ee_conf_all[i*prop_num+3+j].c_str() ); } + hrp::Vector4 tmp_aa; + for (int j = 0; j < 4; j++ ){ tmp_aa(j) = std::atof( ee_conf_all[i*prop_num+6+j].c_str() ); } + _ee_ikc_map[ee_name].localR = Eigen::AngleAxis(tmp_aa(3), tmp_aa.head(3)).toRotationMatrix(); // rotation in VRML is represented by axis + angle + jpath_ee[ee_name] = hrp::JointPath(m_robot->rootLink(), m_robot->link(target_link_name)); + if(_robot->link(target_link_name)){ + RTC_INFO_STREAM("End Effector [" << ee_name << "]"); + RTC_INFO_STREAM(" target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name); + RTC_INFO_STREAM(" offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]"); + RTC_INFO_STREAM(" has_toe_joint = " << "fix to false now"); + }else{ + RTC_WARN_STREAM("Target link [" << target_link_name << "] not found !"); + return RTC::RTC_ERROR; + } + } + } + return RTC::RTC_OK; +} + +RTC::ReturnCode_t HapticController::onExecute(RTC::UniqueId ec_id){ + if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ") dt = " << m_dt); + if(m_qRefIn.isNew() ){ m_qRefIn.read(); } + if(m_qActIn.isNew() ){ m_qActIn.read(); } + if(m_dqActIn.isNew() ){ m_dqActIn.read(); } + if(m_delayCheckPacketInboundIn.isNew() ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();} + for(auto p : m_slaveEEWrenchesIn){ if( p.second->isNew() ) { p.second->read(); } } + for(auto p : m_slaveTgtPosesIn){ if( p.second->isNew() ) { p.second->read(); } } + + mode.update(); + processTransition(); + + calcCurrentState(); + calcTorque(); + calcOdometry(); + + if (mode.isRunning()) { + if(mode.isInitialize()){ + idsb.setInitState(m_robot, m_dt);//逆動力学初期化 + } + + /////////////////////////// + + } + + + m_tau.tm = m_qRef.tm; + m_tau.data = hrp::to_DoubleSeq(hrp::getUAll(m_robot) * output_ratio); + + // write + m_qOut.write(); + m_tauOut.write(); + loop ++; + return RTC::RTC_OK; +} + +class ContactInfo{ + private: + public: + ContactInfo(hrp::Link* l, const hrp::Vector3& lp) + :target_link_(l), + is_contact(false), + local_pos_(lp){} + ContactInfo(){} + hrp::Link* target_link_; + hrp::Vector3 local_pos_; + hrp::Vector3 world_pos_; + hrp::Vector3 world_pos_old_; + hrp::Vector3 world_f_; + bool is_contact; + hrp::Vector3 CalcWorldPos(){ world_pos_ = target_link_->p + target_link_->R * local_pos_; return world_pos_;} + void UpdateState(){world_pos_old_ = world_pos_;} + friend std::ostream& operator<<(std::ostream& os, const ContactInfo& ci){ + os<< "target_link: "<name<<" local_pos: "<numJoints();i++)m_robot->joint(i)->q = m_qAct.data[i]; + dqAct_filtered = dqAct_filter.passFilter(hrp::to_dvector(m_dqAct.data)); + + ///// calc inverse dynamics (forward kinematics included) + if(!idsb.is_initialized){ idsb.setInitState(m_robot, m_dt); } + for(int i=0;inumJoints();i++)idsb.q(i) = m_robot->joint(i)->q; + idsb.dq.fill(0); + idsb.ddq.fill(0); + const hrp::Vector3 g(0, 0, 9.80665* hcp.gravity_compensation_ratio); + idsb.base_p = m_robot->rootLink()->p; + idsb.base_v.fill(0); + idsb.base_dv = g; + idsb.base_R = hrp::Matrix33::Identity();//m_robot->rootLink()->R; + idsb.base_dR.fill(0); + idsb.base_w_hat.fill(0); + idsb.base_w.fill(0); + idsb.base_dw.fill(0); + hrp::Vector3 f_base_wld, t_base_wld; + calcRootLinkWrenchFromInverseDynamics(m_robot, idsb, f_base_wld, t_base_wld); // torque (m_robot->joint(i)->u) set + updateInvDynStateBuffer(idsb); + for(int i=0; inumJoints(); i++){ + if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos){ + m_robot->joint(i)->u *= hcp.ex_gravity_compensation_ratio_lower; + }else if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos){ + m_robot->joint(i)->u *= hcp.ex_gravity_compensation_ratio_upper; + } + } + + ///// calc end effector position and velocity + for(auto ee : ee_names){ + master_ee_pose[ee] = ee_ikc_map[ee].getCurrentTargetPose(m_robot); + slave_ee_pose[ee] = hrp::to_Pose3(m_slaveTgtPoses[ee].data); + } + if(master_ee_pose_old.size()==0 ){ master_ee_pose_old = master_ee_pose; } //init + if(slave_ee_pose_old.size()==0 ){ slave_ee_pose_old = slave_ee_pose; } //init + for(auto ee : ee_names){ + master_ee_vel[ee].head(3) = (master_ee_pose[ee].p - master_ee_pose_old[ee].p) / m_dt; + master_ee_vel[ee].tail(3) = master_ee_pose_old[ee].R * hrp::omegaFromRot(master_ee_pose_old[ee].R.transpose() * master_ee_pose[ee].R) / m_dt; // world angular velocity + master_ee_vel_filtered[ee] = ee_vel_filter[ee].passFilter(master_ee_vel[ee]); + slave_ee_vel[ee].head(3) = (slave_ee_pose[ee].p - slave_ee_pose_old[ee].p) / m_dt; + slave_ee_vel[ee].tail(3) = slave_ee_pose_old[ee].R * hrp::omegaFromRot(slave_ee_pose_old[ee].R.transpose() * slave_ee_pose[ee].R) / m_dt; // world angular velocity + } + master_ee_pose_old = master_ee_pose; + slave_ee_pose_old = slave_ee_pose; + + ///// calc base to end effector Jacobian + for(auto ee : ee_names){ + hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name)); + jp.calcJacobian(J_ee[ee], ee_ikc_map[ee].localPos); + } + + for(std::string lr : {"l","r"}){ //m_robot->rootLink()->p(Z)は0付近で更新されないのでworld座標系におけるfloor_hは-1.0とからへん + // const double slave_act_floor_z = MINMAX_LIMITED(m_slaveTgtPoses[lr+"floor"].data.position.z, 0, 0.3); + const double slave_act_floor_z = m_slaveTgtPoses[lr+"floor"].data.position.z; + const double basic_floor_h_wld = m_robot->rootLink()->p(Z) - baselink_h_from_floor; + foot_h_from_floor[lr+"leg"] = master_ee_pose[lr+"leg"].p(Z) - (basic_floor_h_wld + slave_act_floor_z - current_adjust_floor_h); + } + const double target_adjust_floor_h = std::min(m_slaveTgtPoses["lfloor"].data.position.z, m_slaveTgtPoses["rfloor"].data.position.z); + current_adjust_floor_h += NORM_LIMITED(target_adjust_floor_h - current_adjust_floor_h, 0.01*m_dt); + +} + + +void HapticController::calcTorque(){ + +// static std::vector civ; +// if(civ.size() == 0){ +// const double a = 4; +// civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3( 0.1, 0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3( 0.1, -0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3(-0.1, 0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("RLEG_JOINT5"), stikp[0].localp + hrp::Vector3(-0.1, -0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3( 0.1, 0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3( 0.1, -0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3(-0.1, 0.05, 0)*a )); +// civ.push_back(ContactInfo(m_robot->link("LLEG_JOINT5"), stikp[1].localp + hrp::Vector3(-0.1, -0.05, 0)*a )); +// +//// civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3( 0.1, 0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3( 0.1, -0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3(-0.1, 0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("RARM_JOINT6"), stikp[2].localp + hrp::Vector3(-0.1, -0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3( 0.1, 0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3( 0.1, -0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1, 0.05, 0)*a )); +//// civ.push_back(ContactInfo(m_robot->link("LARM_JOINT6"), stikp[3].localp + hrp::Vector3(-0.1, -0.05, 0)*a )); +// } + // calc virtual floor reaction force +// { +// for (int i=0; iname.find("LEG") != std::string::npos){ +// vfloor_h = m_robot->rootLink()->p(Z) - 1.2; +// }else{ +// vfloor_h = m_robot->rootLink()->p(Z) - 0.1; +// } +// if(civ[i].world_pos_(2) < vfloor_h){ +// civ[i].is_contact = true; +// const double pos_err = vfloor_h - civ[i].world_pos_(2); +// const double vel_err = 0 - (civ[i].world_pos_(2) - civ[i].world_pos_old_(2)) / dt; +// civ[i].world_f_ << 0, 0, 5000 * pos_err + 50 * vel_err;// P,D = ?,10 OUT +// if(civ[i].world_f_.norm() > 500){ +// civ[i].world_f_ = civ[i].world_f_.normalized() * 500; +// } +// const hrp::Vector3 world_f_rel_base = m_robot->rootLink()->R.transpose() * civ[i].world_f_; +// hrp::JointPath jp(m_robot->rootLink(), civ[i].target_link_); +// hrp::dmatrix J_contact; +// jp.calcJacobian(J_contact, civ[i].local_pos_); +// hrp::dvector tq_for_virtual_reaction_force = (J_contact.topRows(3)).transpose() * world_f_rel_base; +// for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_for_virtual_reaction_force(j); +// }else{ +// civ[i].is_contact = false; +// } +// civ[i].UpdateState(); +// } +// if(loop%500==0)dbgv(hrp::getUAll(m_robot)); +// } + + std::map masterEEWrenches; + for (auto ee : ee_names){ masterEEWrenches[ee].fill(0); } + { + const std::vector legs = {"lleg", "rleg"}; + + ///// fix ee rot horizontal + for (auto leg : legs){ + hrp::Vector3 diff_rot; + rats::difference_rotation(diff_rot, master_ee_pose[leg].R, hrp::Matrix33::Identity()); + const hrp::Vector3 diff_rot_vel = hrp::Vector3::Zero() - master_ee_vel_filtered[leg].tail(3); + hrp::dvector6 wrench = (hrp::dvector6()<< 0,0,0, diff_rot * hcp.foot_horizontal_pd_gain(0) + diff_rot_vel * hcp.foot_horizontal_pd_gain(1)).finished(); + //wrench(tz) = 0; // unlock foot yaw + LIMIT_NORM_V(wrench.tail(3), 50); + hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench; + for(int j=0; ju += tq_tmp(j); } + masterEEWrenches[leg] += wrench; + } + + ///// virtual floor + for (auto leg : legs){ + if(foot_h_from_floor[leg] < 0){ + hrp::dvector6 wrench = hrp::dvector6::Unit(fz) * (-foot_h_from_floor[leg]*hcp.floor_pd_gain(0) + (0-master_ee_vel_filtered[leg](fz))*hcp.floor_pd_gain(1)); + LIMIT_NORM_V(wrench, 200); + const hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench; + for(int j=0; ju += tq_tmp(j); } + masterEEWrenches[leg] += wrench; + } + is_contact_to_floor[leg] = (foot_h_from_floor[leg] < 0 + 0.03); + if(loop%1000==0){ dbg(foot_h_from_floor[leg]); } + } + + ///// virtual back wall + for (auto leg : legs){ + const double wall_x_rel_base = -0.1; + const double current_x = master_ee_pose[leg].p(X) - m_robot->rootLink()->p(X); + if(current_x < wall_x_rel_base){ + hrp::dvector6 wrench = (wall_x_rel_base - current_x) * 1000 * hrp::dvector6::Unit(fx); + LIMIT_NORM_V(wrench, 100); + hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench; + for(int j=0; ju += tq_tmp(j); } + masterEEWrenches[leg] += wrench; + } + } + + ///// keep apart each other + const double current_dist = master_ee_pose["lleg"].p(Y) - master_ee_pose["rleg"].p(Y); + if(current_dist < hcp.foot_min_distance){ + for (auto leg : legs){ + hrp::dvector6 wrench = (leg=="lleg" ? 1:-1) * (hcp.foot_min_distance - current_dist) * 1000 * hrp::dvector6::Unit(fy); + LIMIT_NORM_V(wrench, 50); + hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench; + for (int j=0; ju += tq_tmp(j); } + masterEEWrenches[leg] += wrench; + } + } + + ///// virtual floor, leg constraint + static hrp::Pose3 locked_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]); + if(is_contact_to_floor["lleg"] && is_contact_to_floor["rleg"]){ + hrp::Pose3 cur_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]); + hrp::Vector3 diff_pos, diff_rot; + diff_pos = locked_l2r_pose.p - cur_l2r_pose.p; + rats::difference_rotation(diff_rot, cur_l2r_pose.R, locked_l2r_pose.R); + hrp::dvector6 rleg_wrench = (hrp::dvector6()<< diff_pos * 1000, diff_rot * 100).finished(); + rleg_wrench(fz) = 0; + LIMIT_NORM_V(rleg_wrench, 200); + for (auto leg : legs){ + hrp::dvector6 wrench = (leg=="rleg" ? 1:-1) * rleg_wrench; + hrp::dvector tq_tmp = J_ee[leg].transpose() * wrench; + for (int j=0; ju += tq_tmp(j); } + masterEEWrenches[leg] += wrench; + } + }else{ + locked_l2r_pose = hrp::calcRelPose3(master_ee_pose["lleg"], master_ee_pose["rleg"]); + } + + ///// ex + for (auto ee : ee_names){ + hrp::dvector6 wrench = hcp.ex_ee_ref_wrench[ee]; + LIMIT_NORM_V(wrench, 500); + hrp::dvector tq_tmp = J_ee[ee].transpose() * wrench; + for (int j=0; ju += tq_tmp(j); } + masterEEWrenches[ee] += wrench; + } + } + + // calc real robot feedback force + for (auto ee : ee_names){ + hrp::dvector6 slave_w_raw = hrp::to_dvector(m_slaveEEWrenches[ee].data); + if(ee == "lleg" || ee == "rleg" ){ slave_w_raw.tail(4).fill(0); }// disable Fz Tx Ty Tz + const hrp::dvector6 slave_w_hp = slave_w_raw - wrench_lpf_for_hpf[ee].passFilter( slave_w_raw ); + const hrp::dvector6 slave_w_lp = wrench_lpf[ee].passFilter( slave_w_raw ); + const hrp::dvector6 slave_w_shaped = slave_w_raw * hcp.force_feedback_ratio + + slave_w_hp * hcp.wrench_hpf_gain + + slave_w_lp * hcp.wrench_lpf_gain; + + // F_master + v_master * a = F_slave + v_slave * a + hrp::dvector6 coeff_v; + coeff_v.head(XYZ).fill(hcp.ee_pos_rot_friction_coeff(0)); + coeff_v.tail(XYZ).fill(hcp.ee_pos_rot_friction_coeff(1)); + hrp::dvector6 master_w_ans = slave_w_shaped + slave_ee_vel[ee].cwiseProduct(coeff_v) - master_ee_vel[ee].cwiseProduct(coeff_v); + LIMIT_NORM_V(master_w_ans.head(3), hcp.force_feedback_limit_ft(0)); + LIMIT_NORM_V(master_w_ans.tail(3), hcp.force_feedback_limit_ft(1)); + + hrp::JointPath jp(m_robot->rootLink(), m_robot->link(ee_ikc_map[ee].target_link_name)); + hrp::dvector tq_from_feedbackWrench = J_ee[ee].transpose() * master_w_ans; + for (int j = 0; j < jp.numJoints(); j++) jp.joint(j)->u += tq_from_feedbackWrench(j); + masterEEWrenches[ee] += master_w_ans; + if(loop%1000==0)dbg(ee); + if(loop%1000==0)dbgv(master_w_ans); + } + + + { + // soft joint limit + m_robot->link("RARM_JOINT1")->ulimit = deg2rad(0);// hotfix + m_robot->link("LARM_JOINT1")->llimit = deg2rad(0); + for(int i=0;inumJoints();i++){ + const double soft_ulimit = m_robot->joint(i)->ulimit - deg2rad(15); + const double soft_llimit = m_robot->joint(i)->llimit + deg2rad(15); + double soft_jlimit_tq = 0; + if(m_qAct.data[i] > soft_ulimit){ soft_jlimit_tq = 100 * (soft_ulimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); } + if(m_qAct.data[i] < soft_llimit){ soft_jlimit_tq = 100 * (soft_llimit - m_qAct.data[i]) + 0 * (0 - dqAct_filtered(i)); } + m_robot->joint(i)->u += soft_jlimit_tq; + } + } + + { // real robot joint friction damper + hrp::dvector friction_tq = - hcp.q_friction_coeff * dqAct_filtered; + for (int i=0; inumJoints(); i++){ + LIMIT_MINMAX(friction_tq(i), -10, 10); + m_robot->joint(i)->u += friction_tq(i); + } + } + + + const hrp::dvector max_torque = (hrp::dvector(m_robot->numJoints()) << 20,40,60,60,20,20, 20,40,60,60,20,20, 40, 15,15,10,10,6,4,4, 15,15,10,10,6,4,4).finished(); + hrp::dvector j_power_coeff = max_torque/max_torque.maxCoeff(); // reduce gain of light inertia joint + + {// calc qref pd control torque + const hrp::dvector q_diff = hrp::to_dvector(m_qRef.data) - hrp::to_dvector(m_qAct.data); + const hrp::dvector dq_diff = - dqAct_filtered; + hrp::dvector torque = (q_diff * hcp.q_ref_pd_gain(0) + dq_diff * hcp.q_ref_pd_gain(1)) * j_power_coeff; + for (int i=0; inumJoints(); i++){ + LIMIT_NORM(torque(i), max_torque(i) * hcp.q_ref_max_torque_ratio);// pd torque will be limited to 1/5 max torque + m_robot->joint(i)->u += torque(i); + } + } + + // torque limit + for(int i=0;inumJoints();i++){ + LIMIT_NORM(m_robot->joint(i)->u, max_torque(i)*2);// up to x2 max torque +// double tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst;////????? + } + if(loop%1000==0)dbgv(hrp::getUAll(m_robot)); + + + // for debug plot + for (auto ee : ee_names){ + m_masterEEWrenches[ee].tm = m_qRef.tm; + m_masterEEWrenches[ee].data = hrp::to_DoubleSeq(masterEEWrenches[ee]); + m_masterEEWrenchesOut[ee]->write(); + } +} + + + +void HapticController::calcOdometry(){ + // m_robotの軸足-ベースリンク間の相対姿勢を使って, m_robot_odomの世界座標でのベースリンク絶対姿勢を更新 + // しゃがんでる時の運足はなかったことにしてもいいかなと思っている + static std::map leg_pose_from_floor_origin; + if(mode.now() != MODE_HC || resetOdom_request){ // reset odom + for (auto leg : {"rleg", "lleg"}){ leg_pose_from_floor_origin[leg] = hrp::to_2DPlanePose3(master_ee_pose[leg]); } + if(resetOdom_request){ resetOdom_request = false; } + } + const std::string parent_leg = (master_ee_pose["rleg"].p(Z) < master_ee_pose["lleg"].p(Z)) ? "rleg" : "lleg"; // choose lower height leg for odom parent + const std::string child_leg = (parent_leg == "rleg") ? "lleg" : "rleg"; + const hrp::Pose3 cur_base_pose = hrp::getLinkPose3 (m_robot->rootLink()); + const hrp::Pose3 pleg_to_base = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]), hrp::to_2DPlanePose3(cur_base_pose) ); + const hrp::Pose3 pleg_to_cleg = hrp::calcRelPose3 ( hrp::to_2DPlanePose3(master_ee_pose[parent_leg]), hrp::to_2DPlanePose3(master_ee_pose[child_leg]) ); + hrp::Pose3 base_pose_from_floor_origin = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_base) ); + const double floating_h = MIN_LIMITED(std::min(foot_h_from_floor["rleg"], foot_h_from_floor["lleg"]), 0); + base_pose_from_floor_origin.p(Z) = baselink_h_from_floor - floating_h + current_adjust_floor_h; // adjust not to float with both feet. + leg_pose_from_floor_origin[child_leg] = hrp::applyRelPose3( hrp::to_2DPlanePose3(leg_pose_from_floor_origin[parent_leg]), hrp::to_2DPlanePose3(pleg_to_cleg) ); + + //update base link pos from world + hrp::setLinkPose3 (m_robot_odom->rootLink(), base_pose_from_floor_origin); + hrp::setQAll (m_robot_odom, hrp::getQAll(m_robot)); + m_robot_odom->calcForwardKinematics(); + + m_teleopOdom.data = hrp::to_Pose3D(hrp::getLinkPose3(m_robot_odom->rootLink())); + m_teleopOdom.tm = m_qRef.tm; + m_teleopOdomOut.write(); + m_masterTgtPoses["com"].data = hrp::to_Pose3D(hrp::Pose3(m_robot_odom->calcCM(), m_robot_odom->rootLink()->R)); + for (auto ee : ee_names){ + m_masterTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_odom));//四肢揃ってないと危険 + } + for (auto tgt : tgt_names){ + m_masterTgtPoses[tgt].tm = m_qRef.tm; + m_masterTgtPosesOut[tgt]->write(); + } +} + +void HapticController::processTransition(){ + if(!q_ref_ip ->isEmpty()){q_ref_ip ->get( &q_ref_output_ratio, true); } + if(!baselink_h_ip ->isEmpty()){baselink_h_ip ->get( &baselink_h_from_floor, true); } + if(!t_ip ->isEmpty()){t_ip ->get( &output_ratio, true); } + if(mode.now() == MODE_SYNC_TO_HC && output_ratio == 1.0){ mode.setNextMode(MODE_HC); } + if(mode.now() == MODE_SYNC_TO_IDLE && output_ratio == 0.0){ mode.setNextMode(MODE_IDLE); } +} + + +bool HapticController::startHapticController(){ + if(mode.now() == MODE_IDLE){ + RTC_INFO_STREAM("startHapticController"); + mode.setNextMode(MODE_SYNC_TO_HC); + const double next_goal = 1.0; t_ip->setGoal(&next_goal, 10.0, true); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to startHapticController"); + return false; + } +} + + +bool HapticController::pauseHapticController(){ + if(mode.now() == MODE_HC){ + RTC_INFO_STREAM("pauseHapticController"); + mode.setNextMode(MODE_PAUSE); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to pauseHapticController"); + return false; + } +} + + +bool HapticController::resumeHapticController(){ + if(mode.now() == MODE_PAUSE){ + RTC_INFO_STREAM("resumeHapticController"); + mode.setNextMode(MODE_HC); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to resumeHapticController"); + return false; + } +} + + +bool HapticController::stopHapticController(){ + if(mode.now() == MODE_HC || mode.now() == MODE_PAUSE ){ + RTC_INFO_STREAM("stopHapticController"); + mode.setNextMode(MODE_SYNC_TO_IDLE); + const double next_goal = 0.0; t_ip->setGoal(&next_goal, 5.0, true); + baselink_h_ip->setGoal(&default_baselink_h_from_floor, 5.0, true); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to stopHapticController"); + return false; + } +} + + +void HapticController::resetOdom(){ + resetOdom_request = true; + RTC_INFO_STREAM("resetOdom"); +} + +namespace hrp{ + hrp::Vector2 to_Vector2(const OpenHRP::HapticControllerService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); } + OpenHRP::HapticControllerService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){ + OpenHRP::HapticControllerService::DblSequence2 ret; ret.length(2); ret[0] = in(0); ret[1] = in(1); return ret; } + OpenHRP::HapticControllerService::DblSequence6 to_DblSequence6(const hrp::dvector6& in){ + OpenHRP::HapticControllerService::DblSequence6 ret; + ret.length(6); for(int i=0; i<6; i++){ ret[i] = in(i); } return ret; + } +} + +bool HapticController::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param){ + RTC_INFO_STREAM("setHapticControllerParam"); + hcp.baselink_height_from_floor = i_param.baselink_height_from_floor; + hcp.dqAct_filter_cutoff_hz = i_param.dqAct_filter_cutoff_hz; + hcp.ee_vel_filter_cutoff_hz = i_param.ee_vel_filter_cutoff_hz; + hcp.ex_gravity_compensation_ratio_lower = i_param.ex_gravity_compensation_ratio_lower; + hcp.ex_gravity_compensation_ratio_upper = i_param.ex_gravity_compensation_ratio_upper; + hcp.foot_min_distance = i_param.foot_min_distance; + hcp.force_feedback_ratio = i_param.force_feedback_ratio; + hcp.gravity_compensation_ratio = i_param.gravity_compensation_ratio; + hcp.q_friction_coeff = i_param.q_friction_coeff; + hcp.q_ref_max_torque_ratio = i_param.q_ref_max_torque_ratio; + hcp.torque_feedback_ratio = i_param.torque_feedback_ratio; + hcp.wrench_hpf_cutoff_hz = i_param.wrench_hpf_cutoff_hz; + hcp.wrench_lpf_cutoff_hz = i_param.wrench_lpf_cutoff_hz; + hcp.wrench_hpf_gain = i_param.wrench_hpf_gain; + hcp.wrench_lpf_gain = i_param.wrench_lpf_gain; + hcp.ee_pos_rot_friction_coeff = hrp::to_Vector2(i_param.ee_pos_rot_friction_coeff); + hcp.floor_pd_gain = hrp::to_Vector2(i_param.floor_pd_gain); + hcp.foot_horizontal_pd_gain = hrp::to_Vector2(i_param.foot_horizontal_pd_gain); + hcp.force_feedback_limit_ft = hrp::to_Vector2(i_param.force_feedback_limit_ft); + hcp.q_ref_pd_gain = hrp::to_Vector2(i_param.q_ref_pd_gain); + for (int i=0; isetGoal(&hcp.baselink_height_from_floor, 5.0, true); + dqAct_filter.setParameter(hcp.dqAct_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + dqAct_filter.reset(0); + for (auto ee : ee_names){ + ee_vel_filter[ee].setParameter(hcp.ee_vel_filter_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + ee_vel_filter[ee].reset(0); + wrench_lpf_for_hpf[ee].setParameter(hcp.wrench_hpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + wrench_lpf_for_hpf[ee].reset(0); + wrench_lpf[ee].setParameter(hcp.wrench_lpf_cutoff_hz, 1/m_dt, Q_BUTTERWORTH); + wrench_lpf[ee].reset(0); + } + return true; +} + + +bool HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param){ + RTC_INFO_STREAM("getHapticControllerParam"); + i_param.baselink_height_from_floor = hcp.baselink_height_from_floor; + i_param.dqAct_filter_cutoff_hz = hcp.dqAct_filter_cutoff_hz; + i_param.ee_vel_filter_cutoff_hz = hcp.ee_vel_filter_cutoff_hz; + i_param.ex_gravity_compensation_ratio_lower = hcp.ex_gravity_compensation_ratio_lower; + i_param.ex_gravity_compensation_ratio_upper = hcp.ex_gravity_compensation_ratio_upper; + i_param.foot_min_distance = hcp.foot_min_distance; + i_param.force_feedback_ratio = hcp.force_feedback_ratio; + i_param.gravity_compensation_ratio = hcp.gravity_compensation_ratio; + i_param.q_friction_coeff = hcp.q_friction_coeff; + i_param.q_ref_max_torque_ratio = hcp.q_ref_max_torque_ratio; + i_param.torque_feedback_ratio = hcp.torque_feedback_ratio; + i_param.wrench_hpf_cutoff_hz = hcp.wrench_hpf_cutoff_hz; + i_param.wrench_lpf_cutoff_hz = hcp.wrench_lpf_cutoff_hz; + i_param.wrench_hpf_gain = hcp.wrench_hpf_gain; + i_param.wrench_lpf_gain = hcp.wrench_lpf_gain; + i_param.ee_pos_rot_friction_coeff = hrp::to_DblSequence2(hcp.ee_pos_rot_friction_coeff); + i_param.floor_pd_gain = hrp::to_DblSequence2(hcp.floor_pd_gain); + i_param.foot_horizontal_pd_gain = hrp::to_DblSequence2(hcp.foot_horizontal_pd_gain); + i_param.force_feedback_limit_ft = hrp::to_DblSequence2(hcp.force_feedback_limit_ft); + i_param.q_ref_pd_gain = hrp::to_DblSequence2(hcp.q_ref_pd_gain); + + i_param.ex_ee_ref_wrench.length(4); + for (int i=0; iregisterFactory(profile, RTC::Create, RTC::Delete); + } +}; diff --git a/rtc/HapticController/HapticController.h b/rtc/HapticController/HapticController.h new file mode 100644 index 00000000000..c5f9102130b --- /dev/null +++ b/rtc/HapticController/HapticController.h @@ -0,0 +1,217 @@ +#ifndef HAPTICCONTROLLER_H +#define HAPTICCONTROLLER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "HapticControllerService_impl.h" +//#include "../Stabilizer/StabilizerService_impl.h" +//#include "../AutoBalancer/AutoBalancerService_impl.h" +//#include "wbms_core.h" +#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h" + +#include "../SequencePlayer/interpolator.h" +#include "../ImpedanceController/JointPathEx.h" + +enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_HC, MODE_HC, MODE_PAUSE, MODE_SYNC_TO_IDLE}; + +class ControlMode{ + private: + mode_enum current, previous, next; + public: + ControlMode(){ current = previous = next = MODE_IDLE;} + ~ControlMode(){} + bool setNextMode(const mode_enum _request){ + switch(_request){ + case MODE_SYNC_TO_HC: + if(current == MODE_IDLE) { next = _request; return true; }else{ return false; } + case MODE_HC: + if(current == MODE_SYNC_TO_HC || current == MODE_PAUSE ) { next = _request; return true; }else{ return false; } + case MODE_PAUSE: + if(current == MODE_HC) { next = _request; return true; }else{ return false; } + case MODE_SYNC_TO_IDLE: + if(current == MODE_HC || current == MODE_PAUSE ) { next = _request; return true; }else{ return false; } + case MODE_IDLE: + if(current == MODE_SYNC_TO_IDLE ) { next = _request; return true; }else{ return false; } + default: + return false; + } + } + void update(){ previous = current; current = next; } + mode_enum now(){ return current; } + mode_enum pre(){ return previous; } + bool isRunning(){ return (current==MODE_SYNC_TO_HC) || (current==MODE_HC) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;} + bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_HC) ;} +}; + +class HapticController : public RTC::DataFlowComponentBase{ + public: + HapticController(RTC::Manager* manager); + virtual ~HapticController(); + virtual RTC::ReturnCode_t onInitialize(); + virtual RTC::ReturnCode_t onFinalize(); + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + bool startHapticController(); + bool stopHapticController(); + bool pauseHapticController(); + bool resumeHapticController(); + void resetOdom(); + bool setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param); + bool getParams(OpenHRP::HapticControllerService::HapticControllerParam& i_param); + typedef boost::shared_ptr > ITP3_Ptr; + typedef boost::shared_ptr > ITDS_Ptr; + typedef boost::shared_ptr > OTP3_Ptr; + typedef boost::shared_ptr > OTDS_Ptr; + + protected: + RTC::TimedDoubleSeq m_qRef; + RTC::InPort m_qRefIn; + RTC::TimedDoubleSeq m_qAct; + RTC::InPort m_qActIn; + RTC::TimedDoubleSeq m_dqAct; + RTC::InPort m_dqActIn; + std::map m_slaveEEWrenches; + std::map m_slaveEEWrenchesIn; + RTC::TimedDoubleSeq m_tau; + RTC::OutPort m_tauOut; + RTC::TimedPose3D m_teleopOdom; + RTC::OutPort m_teleopOdomOut; + RTC::OutPort m_qOut; + std::map m_masterTgtPoses; + std::map m_masterTgtPosesOut; + // sub usage + std::map m_masterEEWrenches; + std::map m_masterEEWrenchesOut; + std::map m_slaveTgtPoses; + std::map m_slaveTgtPosesIn; + RTC::TimedDoubleSeq m_debugData; + RTC::OutPort m_debugDataOut; + RTC::CorbaPort m_HapticControllerServicePort; + HapticControllerService_impl m_service0; + + RTC::Time m_delayCheckPacket; + RTC::InPort m_delayCheckPacketInboundIn; + RTC::OutPort m_delayCheckPacketOutboundOut; + + private: + double m_dt; + unsigned int loop; + unsigned int m_debugLevel; + hrp::BodyPtr m_robot, m_robot_odom; + + double output_ratio, q_ref_output_ratio, baselink_h_from_floor, default_baselink_h_from_floor; + interpolator *t_ip, *q_ref_ip, *baselink_h_ip; + + hrp::InvDynStateBuffer idsb; + BiquadIIRFilterVec dqAct_filter; + hrp::dvector dqAct_filtered; + std::map ee_ikc_map; + std::map ee_vel_filter; + std::map wrench_lpf_for_hpf, wrench_lpf; + std::map master_ee_pose, master_ee_pose_old, slave_ee_pose, slave_ee_pose_old; + std::map master_ee_vel, master_ee_vel_filtered, slave_ee_vel; // = twist + std::map jpath_ee; + std::map J_ee; + std::map is_contact_to_floor; + std::map foot_h_from_floor; + bool resetOdom_request; + + double current_adjust_floor_h; + + ControlMode mode; + + std::vector ee_names, tgt_names; + class HCParams { + public: + double baselink_height_from_floor; + double default_baselink_h_from_floor; + double dqAct_filter_cutoff_hz; + double ee_vel_filter_cutoff_hz; + double ex_gravity_compensation_ratio_lower; + double ex_gravity_compensation_ratio_upper; + double foot_min_distance; + double force_feedback_ratio; + double gravity_compensation_ratio; + double q_friction_coeff; + double q_ref_max_torque_ratio; + double torque_feedback_ratio; + double wrench_hpf_cutoff_hz; + double wrench_lpf_cutoff_hz; + double wrench_hpf_gain; + double wrench_lpf_gain; + hrp::Vector2 ee_pos_rot_friction_coeff; + hrp::Vector2 floor_pd_gain; + hrp::Vector2 foot_horizontal_pd_gain; + hrp::Vector2 force_feedback_limit_ft; + hrp::Vector2 q_ref_pd_gain; + std::map ex_ee_ref_wrench; + HCParams(){ + baselink_height_from_floor = 1.5;// will be overwrited + dqAct_filter_cutoff_hz = 500;// 10以下で確実に位相遅れによる振動 + ee_vel_filter_cutoff_hz = 500;// 10以下で確実に位相遅れによる振動 + ex_gravity_compensation_ratio_lower = 1.0; + ex_gravity_compensation_ratio_upper = 0.9; + foot_min_distance = 0.25; + force_feedback_ratio = 0.1; + gravity_compensation_ratio = 1.0; + q_friction_coeff = 0; + q_ref_max_torque_ratio = 0.01; + torque_feedback_ratio = 0.01; + wrench_hpf_cutoff_hz = 20; + wrench_lpf_cutoff_hz = 0.3; + wrench_hpf_gain = 0.1; + wrench_lpf_gain = 0; + ee_pos_rot_friction_coeff << 10, 0.1; + floor_pd_gain << 10000, 500; + foot_horizontal_pd_gain << 300, 30; + force_feedback_limit_ft << 100, 10; + q_ref_pd_gain << 50, 0; + ex_ee_ref_wrench["rleg"] = hrp::dvector6::Zero(); + ex_ee_ref_wrench["lleg"] = hrp::dvector6::Zero(); + ex_ee_ref_wrench["rarm"] = hrp::dvector6::Zero(); + ex_ee_ref_wrench["larm"] = hrp::dvector6::Zero(); + CheckSafeLimit(); + } + void CheckSafeLimit(){ + LIMIT_MINMAX(baselink_height_from_floor , 0.5, 3); + LIMIT_MINMAX(dqAct_filter_cutoff_hz , 0, 500); + LIMIT_MINMAX(ee_vel_filter_cutoff_hz , 0, 500); + LIMIT_MINMAX(ex_gravity_compensation_ratio_lower , -1, 2); + LIMIT_MINMAX(ex_gravity_compensation_ratio_upper , -1, 2); + LIMIT_MINMAX(foot_min_distance , 0, 1); + LIMIT_MINMAX(force_feedback_ratio , 0, 2); + LIMIT_MINMAX(gravity_compensation_ratio , 0, 2); + LIMIT_MINMAX(q_friction_coeff , 0, 0.1); + LIMIT_MINMAX(q_ref_max_torque_ratio , 0, 2); + LIMIT_MINMAX(torque_feedback_ratio , 0, 1); + LIMIT_MINMAX(wrench_hpf_cutoff_hz , 0, 500); + LIMIT_MINMAX(wrench_lpf_cutoff_hz , 0, 500); + LIMIT_MINMAX(wrench_hpf_gain , 0, 2); + LIMIT_MINMAX(wrench_lpf_gain , 0, 2); + } + } hcp; + + RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop); + void calcCurrentState(); + void calcTorque(); + void calcOdometry(); + void processTransition(); +}; + + +extern "C" { void HapticControllerInit(RTC::Manager* manager); }; + +#endif diff --git a/rtc/HapticController/HapticControllerComp.cpp b/rtc/HapticController/HapticControllerComp.cpp new file mode 100644 index 00000000000..41e5b717239 --- /dev/null +++ b/rtc/HapticController/HapticControllerComp.cpp @@ -0,0 +1,50 @@ +// -*- C++ -*- +/*! + * @file HapticControllerComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "HapticController.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + HapticControllerInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("HapticController"); + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/HapticController/HapticControllerService_impl.cpp b/rtc/HapticController/HapticControllerService_impl.cpp new file mode 100644 index 00000000000..897f147bc9d --- /dev/null +++ b/rtc/HapticController/HapticControllerService_impl.cpp @@ -0,0 +1,59 @@ +#include "HapticControllerService_impl.h" +#include "HapticController.h" + +HapticControllerService_impl::HapticControllerService_impl() : m_hapticcontroller(NULL) +{ +} + +HapticControllerService_impl::~HapticControllerService_impl() +{ +} + +void HapticControllerService_impl::hapticcontroller(HapticController *i_hapticcontroller) +{ + m_hapticcontroller = i_hapticcontroller; +} + +CORBA::Boolean HapticControllerService_impl::startHapticController() +{ + return m_hapticcontroller->startHapticController(); +}; + +CORBA::Boolean HapticControllerService_impl::stopHapticController() +{ + return m_hapticcontroller->stopHapticController(); +}; + +CORBA::Boolean HapticControllerService_impl::pauseHapticController() +{ + return m_hapticcontroller->pauseHapticController(); +}; + +CORBA::Boolean HapticControllerService_impl::resumeHapticController() +{ + return m_hapticcontroller->resumeHapticController(); +}; +void HapticControllerService_impl::resetOdom() +{ + m_hapticcontroller->resetOdom(); +}; +// +void HapticControllerService_impl::setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param) +{ + m_hapticcontroller->setParams(i_param); +}; + +void HapticControllerService_impl::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param) +{ + //???????????????????????????????????????????????????????? +// OpenHRP::HapticControllerService::HapticControllerParam tmp; +// m_hapticcontroller->getParams(tmp); +// i_param = tmp; + + //今までこれでいけてた？ idlにsequenceとか可変長を含むとき？ + i_param = new OpenHRP::HapticControllerService::HapticControllerParam(); + m_hapticcontroller->getParams(*i_param); + + // error: no matching function for call to ‘HapticController::getParams(OpenHRP::HapticControllerService::HapticControllerParam_out&)’ +// m_hapticcontroller->getParams(i_param); + }; diff --git a/rtc/HapticController/HapticControllerService_impl.h b/rtc/HapticController/HapticControllerService_impl.h new file mode 100644 index 00000000000..11b9d1f60d9 --- /dev/null +++ b/rtc/HapticController/HapticControllerService_impl.h @@ -0,0 +1,32 @@ +// -*-C++-*- +#ifndef HAPTICCONTROLLERSERVICESVC_IMPL_H +#define HAPTICCONTROLLERSERVICESVC_IMPL_H + +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/HapticControllerService.hh" + +using namespace OpenHRP; + +class HapticController; + +class HapticControllerService_impl + : public virtual POA_OpenHRP::HapticControllerService, + public virtual PortableServer::RefCountServantBase +{ +public: + HapticControllerService_impl(); + virtual ~HapticControllerService_impl(); + CORBA::Boolean startHapticController(); + CORBA::Boolean stopHapticController(); + CORBA::Boolean pauseHapticController(); + CORBA::Boolean resumeHapticController(); + void resetOdom(); + void setParams(const OpenHRP::HapticControllerService::HapticControllerParam& i_param); + void getParams(OpenHRP::HapticControllerService::HapticControllerParam_out i_param); + // + void hapticcontroller(HapticController *i_hapticcontroller); +private: + HapticController *m_hapticcontroller; +}; + +#endif diff --git a/rtc/RobotHardware/defs.h b/rtc/RobotHardware/defs.h index ccfbf4547a5..90a6d1a2fa3 100644 --- a/rtc/RobotHardware/defs.h +++ b/rtc/RobotHardware/defs.h @@ -14,7 +14,7 @@ enum {RL, FB}; #define sgn(x) (((x)>0)?1:-1) #define deg2rad(x) ((x)*M_PI/180) #define rad2deg(x) ((x)*180/M_PI) -#define sqr(x) ((x)*(x)) +//#define sqr(x) ((x)*(x)) //　conflict with boost? #define LPF(dT, omega, x, y) ((y) = (((dT)*(omega)/(1+(dT)*(omega)))*(x)+1/(1+(dT)*(omega))*(y))) template diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 2027dd405db..8f982efec15 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -466,7 +466,12 @@ bool robot::power(int jid, bool turnon) } } else for (unsigned int i=0; i& A, std::vector fb_coeffs(3), ff_coeffs(3); - const double omega = 2 * 3.14159265 * f_cutoff / hz; - const double alpha = std::sin(omega) / (2 * Q); - const double denom = 1 + alpha; - fb_coeffs[0] = 1; - fb_coeffs[1] = -2 * std::cos(omega) / denom; - fb_coeffs[2] = (1 - alpha) / denom; - ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom; - ff_coeffs[1] = (1 - std::cos(omega)) / denom; - ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom; - return this->setParameter(2, fb_coeffs, ff_coeffs); + std::vector fb_coeffs(3), ff_coeffs(3); + const double omega = 2 * 3.14159265 * f_cutoff / hz; + const double alpha = std::sin(omega) / (2 * Q); + const double denom = 1 + alpha; + fb_coeffs[0] = 1; + fb_coeffs[1] = -2 * std::cos(omega) / denom; + fb_coeffs[2] = (1 - alpha) / denom; + ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom; + ff_coeffs[1] = (1 - std::cos(omega)) / denom; + ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom; + return this->setParameter(2, fb_coeffs, ff_coeffs); }; diff --git a/rtc/WholeBodyMasterSlave/CMakeLists.txt b/rtc/WholeBodyMasterSlave/CMakeLists.txt new file mode 100644 index 00000000000..7a6087fd119 --- /dev/null +++ b/rtc/WholeBodyMasterSlave/CMakeLists.txt @@ -0,0 +1,22 @@ +set(comp_sources WholeBodyMasterSlave.cpp WholeBodyMasterSlaveService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp ../TorqueFilter/IIRFilter.cpp) +set(libs ${OpenCV_LIBRARIES} hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +#set(CMAKE_CXX_STANDARD, 11) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +add_library(WholeBodyMasterSlave SHARED ${comp_sources}) +target_link_libraries(WholeBodyMasterSlave ${libs}) +set_target_properties(WholeBodyMasterSlave PROPERTIES PREFIX "") + +add_executable(WholeBodyMasterSlaveComp WholeBodyMasterSlaveComp.cpp ${comp_sources}) +target_link_libraries(WholeBodyMasterSlaveComp ${libs}) + +include_directories(${PROJECT_SOURCE_DIR}/rtc/SequencePlayer) + +set(target WholeBodyMasterSlave WholeBodyMasterSlaveComp ) + +#add_test(testPreviewControllerNoGP testPreviewController --use-gnuplot false) + +install(TARGETS ${target} + RUNTIME DESTINATION bin CONFIGURATIONS Release Debug + LIBRARY DESTINATION lib CONFIGURATIONS Release Debug +) + diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp new file mode 100644 index 00000000000..6cd39422360 --- /dev/null +++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.cpp @@ -0,0 +1,904 @@ +#include "WholeBodyMasterSlave.h" + +#define DEBUGP (loop%200==0) +#define DEBUGP_ONCE (loop==0) + +static const char* WholeBodyMasterSlave_spec[] = { + "implementation_id", "WholeBodyMasterSlave", + "type_name", "WholeBodyMasterSlave", + "description", "wholebodymasterslave component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + "conf.default.debugLevel", "0", + "" +}; + +WholeBodyMasterSlave::WholeBodyMasterSlave(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), + m_qRefIn("qRef", m_qRef),// from sh + m_qActIn("qAct", m_qAct), + m_zmpIn("zmpIn", m_zmp), + m_basePosIn("basePosIn", m_basePos), + m_baseRpyIn("baseRpyIn", m_baseRpy), + m_optionalDataIn("optionalData", m_optionalData), + m_qOut("q", m_qRef),// to abc + m_zmpOut("zmpOut", m_zmp), + m_basePosOut("basePosOut", m_basePos), + m_baseRpyOut("baseRpyOut", m_baseRpy), + m_optionalDataOut("optionalDataOut", m_optionalData), + m_actCPIn("actCapturePoint", m_actCP), + m_actZMPIn("zmp", m_actZMP), + m_exDataIn("exData", m_exData), + m_delayCheckPacketInboundIn("delay_check_packet_inbound", m_delayCheckPacket), + m_delayCheckPacketOutboundOut("delay_check_packet_outbound", m_delayCheckPacket), + m_exDataIndexIn("exDataIndex", m_exDataIndex), + m_WholeBodyMasterSlaveServicePort("WholeBodyMasterSlaveService"), + m_AutoBalancerServicePort("AutoBalancerService"), + m_StabilizerServicePort("StabilizerService"), + m_debugLevel(0) +{ + m_service0.wholebodymasterslave(this); +} + +WholeBodyMasterSlave::~WholeBodyMasterSlave(){} + +RTC::ReturnCode_t WholeBodyMasterSlave::onInitialize(){ + RTC_INFO_STREAM("onInitialize()"); + bindParameter("debugLevel", m_debugLevel, "0"); + addInPort("qRef", m_qRefIn);// from sh + addInPort("qAct", m_qActIn); + addInPort("zmpIn", m_zmpIn); + addInPort("basePosIn", m_basePosIn); + addInPort("baseRpyIn", m_baseRpyIn); + addInPort("optionalData", m_optionalDataIn); + addOutPort("q", m_qOut);// to abc + addOutPort("zmpOut", m_zmpOut); + addOutPort("basePosOut", m_basePosOut); + addOutPort("baseRpyOut", m_baseRpyOut); + addOutPort("optionalDataOut", m_optionalDataOut); + addInPort("actCapturePoint", m_actCPIn); + addInPort("zmp", m_actZMPIn); + addInPort("delay_check_packet_inbound", m_delayCheckPacketInboundIn); + addOutPort("delay_check_packet_outbound", m_delayCheckPacketOutboundOut); + addInPort("exData", m_exDataIn); + addInPort("exDataIndex", m_exDataIndexIn); + m_WholeBodyMasterSlaveServicePort.registerProvider("service0", "WholeBodyMasterSlaveService", m_service0); + addPort(m_WholeBodyMasterSlaveServicePort); + + m_AutoBalancerServicePort.registerConsumer("service0","AutoBalancerService", m_AutoBalancerServiceConsumer); + m_StabilizerServicePort.registerConsumer("service0","StabilizerService", m_StabilizerServiceConsumer); + addPort(m_AutoBalancerServicePort); + addPort(m_StabilizerServicePort); + + RTC::Properties& prop = getProperties(); + coil::stringTo(m_dt, prop["dt"].c_str()); + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ comPos = nameServer.length(); } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + + hrp::BodyPtr robot_for_ik = hrp::BodyPtr(new hrp::Body()); + if (!loadBodyFromModelLoader(robot_for_ik, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){ + RTC_WARN_STREAM("failed to load model[" << prop["model"] << "]"); + return RTC::RTC_ERROR; + } + m_robot_act = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy + m_robot_vsafe = hrp::BodyPtr(new hrp::Body(*robot_for_ik)); //copy + RTC_INFO_STREAM("setup robot model finished"); + + fik = fikPtr(new FullbodyInverseKinematicsSolver(robot_for_ik, std::string(m_profile.instance_name), m_dt)); + setupEEIKConstraintFromConf(ee_ikc_map, robot_for_ik, prop); + RTC_INFO_STREAM("setup fullbody ik finished"); + + wbms = boost::shared_ptr(new WBMSCore(m_dt)); + if(fik->m_robot->name() == "RHP4B"){ + for(int i=0; im_robot->numJoints(); i++){ + if(fik->m_robot->joint(i)->name.find("-linear-joint") == std::string::npos){ + wbms->wp.use_joints.push_back(fik->m_robot->joint(i)->name); + } + } + }else{ + wbms->wp.use_joints = getJointNameAll(fik->m_robot); + } + + if(fik->m_robot->name().find("JAXON") != std::string::npos){ // for demo + wbms->wp.use_targets.push_back("rleg"); + wbms->wp.use_targets.push_back("lleg"); + wbms->wp.use_targets.push_back("rarm"); + wbms->wp.use_targets.push_back("larm"); + wbms->wp.use_targets.push_back("com"); + wbms->wp.use_targets.push_back("head"); + }else{ + wbms->wp.use_targets.push_back("rleg"); + wbms->wp.use_targets.push_back("lleg"); + wbms->wp.use_targets.push_back("rarm"); + wbms->wp.use_targets.push_back("larm"); + wbms->wp.use_targets.push_back("com"); + wbms->wp.use_targets.push_back("head"); + } + wbms->legged = ( has(ee_names,"lleg") || has(ee_names,"rleg") ); + RTC_INFO_STREAM("setup mode as legged robot ? = " << wbms->legged); + + for (int st; stnumSensorTypes(); st++){ + RTC_INFO_STREAM("SensorType : "<numSensors(st); s++){ + RTC_INFO_STREAM("Sensor id : "<sensor(st,s)->putInformation(std::cerr); + } + } + + sccp = boost::shared_ptr(new CapsuleCollisionChecker(fik->m_robot)); + RTC_INFO_STREAM("setup main function class finished"); + + // allocate memory for outPorts + m_qRef.data.length(fik->m_robot->numJoints()); // is really needed? + if(prop["seq_optional_data_dim"].empty()){ + RTC_WARN_STREAM("No seq_optional_data_dim is set ! Exit !!!"); + return RTC::RTC_ERROR; + } + coil::stringTo(optionalDataLength, prop["seq_optional_data_dim"].c_str()); + + output_ratio = 0.0; + t_ip = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + t_ip->clear(); + t_ip->set(&output_ratio); + q_ip = new interpolator(fik->numStates(), m_dt, interpolator::CUBICSPLINE, 1); // or HOFFARBIB, QUINTICSPLINE + q_ip->clear(); + if(fik->m_robot->name().find("JAXON") != std::string::npos){ + avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); + avg_q_vel.head(12).fill(4.0); // leg + }else{ + avg_q_vel = hrp::dvector::Constant(fik->numStates(), 1.0); // all joint max avarage vel = 1.0 rad/s + } + if(!wbms->legged){ avg_q_vel = hrp::dvector::Constant(fik->numStates(), 10.0); } // rapid manip + avg_q_acc = hrp::dvector::Constant(fik->numStates(), 16.0); // all joint max avarage acc = 16.0 rad/s^2 + avg_q_vel.tail(6).fill(std::numeric_limits::max()); // no limit for base link vel + avg_q_acc.tail(6).fill(std::numeric_limits::max()); // no limit for base link acc + RTC_INFO_STREAM("setup interpolator finished"); + + ref_zmp_filter.resize(XYZ); + ref_zmp_filter.setParameter(100, 1/m_dt, Q_BUTTERWORTH); + + tgt_names = ee_names; + tgt_names.push_back("com"); + tgt_names.push_back("head"); + tgt_names.push_back("rhand"); + tgt_names.push_back("lhand"); + tgt_names.push_back("rfloor"); + tgt_names.push_back("lfloor"); + + for (auto ee : ee_names) { + const std::string n = "slave_"+ee+"_wrench"; + m_slaveEEWrenchesOut[ee] = OTDS_Ptr(new RTC::OutPort(n.c_str(), m_slaveEEWrenches[ee])); + registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee]); + RTC_INFO_STREAM(" registerOutPort " << n); + } + for (auto ee : ee_names) { + const std::string n = "master_"+ee+"_wrench"; + m_masterEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort(n.c_str(), m_masterEEWrenches[ee])); + registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee]); + RTC_INFO_STREAM(" registerInPort " << n); + } + /////////////////// + for (auto tgt : tgt_names) { + const std::string n = "master_"+tgt+"_pose"; + m_masterTgtPosesIn[tgt] = ITP3_Ptr(new RTC::InPort(n.c_str(), m_masterTgtPoses[tgt])); + registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt]); + RTC_INFO_STREAM(" registerInPort " << n); + } + for (auto tgt : tgt_names) { + const std::string n = "slave_"+tgt+"_pose"; + m_slaveTgtPosesOut[tgt] = OTP3_Ptr(new RTC::OutPort(n.c_str(), m_slaveTgtPoses[tgt])); + registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt]); + RTC_INFO_STREAM(" registerOutPort " << n); + } + ////////////// + for (auto ee : ee_names) { + const std::string n = "local_"+ee+"_wrench"; + m_localEEWrenchesIn[ee] = ITDS_Ptr(new RTC::InPort(n.c_str(), m_localEEWrenches[ee])); + registerInPort(n.c_str(), *m_localEEWrenchesIn[ee]); + RTC_INFO_STREAM(" registerInPort " << n); + } + + for(auto ee : ee_names){ + ee_f_filter[ee].resize(XYZ); + ee_f_filter[ee].setParameter(1, 1/m_dt, Q_BUTTERWORTH); + } + + RTC_INFO_STREAM("onInitialize() OK"); + loop = 0; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t WholeBodyMasterSlave::setupEEIKConstraintFromConf(std::map& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop){ + coil::vstring ee_conf_all = coil::split(_prop["end_effectors"], ","); + size_t prop_num = 10; // limbname + linkname + basename + pos(3) + angleaxis(4) + if (ee_conf_all.size() > 0) { + size_t ee_num = ee_conf_all.size()/prop_num; + for (size_t i = 0; i < ee_num; i++) { + std::string ee_name, target_link_name, base_name; // e.g. rleg, RLEG_JOINT5, WAIST + coil::stringTo(ee_name, ee_conf_all[i*prop_num].c_str()); + coil::stringTo(target_link_name, ee_conf_all[i*prop_num+1].c_str()); + coil::stringTo(base_name, ee_conf_all[i*prop_num+2].c_str()); + ee_names.push_back(ee_name); + _ee_ikc_map[ee_name].target_link_name = target_link_name; + for (size_t j = 0; j < XYZ; j++){ coil::stringTo(_ee_ikc_map[ee_name].localPos(j), ee_conf_all[i*prop_num+3+j].c_str()); } + double tmp_aa[4]; + for (int j = 0; j < 4; j++ ){ coil::stringTo(tmp_aa[j], ee_conf_all[i*prop_num+6+j].c_str()); } + _ee_ikc_map[ee_name].localR = Eigen::AngleAxis(tmp_aa[3], hrp::Vector3(tmp_aa[0], tmp_aa[1], tmp_aa[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle + if(_robot->link(target_link_name)){ + RTC_INFO_STREAM("End Effector [" << ee_name << "]"); + RTC_INFO_STREAM(" target_link_name = " << _ee_ikc_map[ee_name].target_link_name << ", base = " << base_name); + RTC_INFO_STREAM(" offset_pos = " << _ee_ikc_map[ee_name].localPos.transpose() << "[m]"); + RTC_INFO_STREAM(" has_toe_joint = " << "fix to false now"); + }else{ + RTC_WARN_STREAM("Target link [" << target_link_name << "] not found !"); + for (int i; i<_robot->numJoints(); i++){ RTC_WARN_STREAM( i << " : " << _robot->joint(i)->name); } + for (int i; i<_robot->numLinks(); i++){ RTC_WARN_STREAM( i << " : " << _robot->link(i)->name); } + return RTC::RTC_ERROR; + } + contact_states_index_map.insert(std::pair(ee_name, i));////TODO:要移動? //used for check optional data order + } + } + return RTC::RTC_OK; +} + +RTC::ReturnCode_t WholeBodyMasterSlave::onExecute(RTC::UniqueId ec_id){ + if(DEBUGP_ONCE) RTC_INFO_STREAM("onExecute(" << ec_id << ")"); + time_report_str.clear(); + clock_gettime(CLOCK_REALTIME, &startT); + if(m_qRefIn.isNew() ){ m_qRefIn.read(); } + if(m_qActIn.isNew() ){ m_qActIn.read(); } + if(m_basePosIn.isNew() ){ m_basePosIn.read(); } + if(m_baseRpyIn.isNew() ){ m_baseRpyIn.read(); } + if(m_zmpIn.isNew() ){ m_zmpIn.read(); } + if(m_optionalDataIn.isNew() ){ m_optionalDataIn.read(); } + if(m_delayCheckPacketInboundIn.isNew() ){ m_delayCheckPacketInboundIn.read(); m_delayCheckPacketOutboundOut.write();} + for (auto ee : ee_names) { + if(m_localEEWrenchesIn[ee]->isNew()){ m_localEEWrenchesIn[ee]->read(); } + } + if(wbms->legged){ + wbms->act_rs.act_foot_wrench[0] = hrp::to_dvector(m_localEEWrenches["rleg"].data); + wbms->act_rs.act_foot_wrench[1] = hrp::to_dvector(m_localEEWrenches["lleg"].data); + wbms->act_rs.act_foot_pose[0] = ee_ikc_map["rleg"].getCurrentTargetPose(m_robot_vsafe); + wbms->act_rs.act_foot_pose[1] = ee_ikc_map["lleg"].getCurrentTargetPose(m_robot_vsafe); + } + // calc actual state + std::map to_sname{{"lleg", "lfsensor"}, {"rleg", "rfsensor"}, {"larm", "lhsensor"}, {"rarm", "rhsensor"}}; + hrp::setQAll(m_robot_act, hrp::to_dvector(m_qAct.data)); + m_robot_act->rootLink()->p = m_robot_vsafe->rootLink()->p; + m_robot_act->rootLink()->R = m_robot_vsafe->rootLink()->R; + m_robot_act->calcForwardKinematics(); + for(auto ee : ee_names){ + hrp::ForceSensor* sensor = m_robot_act->sensor(to_sname[ee]); + hrp::dvector6 w_ee_wld = hrp::dvector6::Zero(); + if(sensor){ + hrp::Matrix33 sensorR_wld = sensor->link->R * sensor->localR; + hrp::Matrix33 sensorR_from_base = m_robot_act->rootLink()->R.transpose() * sensorR_wld; + const hrp::Vector3 f_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).head(3); + const hrp::Vector3 t_sensor_wld = sensorR_from_base * hrp::to_dvector(m_localEEWrenches[ee].data).tail(3); + const hrp::Vector3 sensor_to_ee_vec_wld = ee_ikc_map[ee].getCurrentTargetPos(m_robot_act) - sensor->link->p; + w_ee_wld << f_sensor_wld, t_sensor_wld - sensor_to_ee_vec_wld.cross(f_sensor_wld); + } + m_slaveEEWrenches[ee].data = hrp::to_DoubleSeq( w_ee_wld ); + m_slaveEEWrenches[ee].tm = m_qRef.tm; + m_slaveEEWrenchesOut[ee]->write(); + m_slaveTgtPoses[ee].data = hrp::to_Pose3D(ee_ikc_map[ee].getCurrentTargetPose(m_robot_act)); + m_slaveTgtPoses[ee].tm = m_qRef.tm; + m_slaveTgtPosesOut[ee]->write(); + } + m_slaveTgtPoses["com"].data = hrp::to_Pose3D( (hrp::dvector6()<calcCM(),0,0,0).finished()); + m_slaveTgtPoses["com"].tm = m_qRef.tm; + m_slaveTgtPosesOut["com"]->write(); + + static_balancing_com_offset.fill(0); + for(auto ee : {"larm","rarm"}){ + const hrp::Vector3 use_f = hrp::Vector3::UnitZ() * m_slaveEEWrenches[ee].data[Z]; + hrp::Vector3 use_f_filtered = ee_f_filter[ee].passFilter(use_f); + const hrp::Vector3 ee_pos_from_com = ee_ikc_map[ee].getCurrentTargetPos(m_robot_vsafe) - wbms->rp_ref_out.tgt[com].abs.p; + static_balancing_com_offset.head(XY) += - ee_pos_from_com.head(XY) * use_f_filtered(Z) / (-m_robot_vsafe->totalMass() * G); + } + if(loop%500==0)dbgv(static_balancing_com_offset); + + // button func + hrp::dvector ex_data; + std::vector ex_data_index; + if(m_exDataIn.isNew()){ + m_exDataIn.read(); + ex_data = hrp::to_dvector(m_exData.data); + } + if(m_exDataIndexIn.isNew()){ + m_exDataIndexIn.read(); + ex_data_index = hrp::to_string_vector(m_exDataIndex.data); + } + + if( mode.now() != MODE_PAUSE ){ // stop updating input when MODE_PAUSE + for (auto tgt : tgt_names){ + if (m_masterTgtPosesIn[tgt]->isNew()){ + m_masterTgtPosesIn[tgt]->read(); + if(tgt == "lhand" || tgt == "rhand" || tgt == "lfloor" || tgt == "rfloor" ){ + // nothing + }else{ + wbms->hp_wld_raw.stgt(tgt).abs = hrp::to_Pose3(m_masterTgtPoses[tgt].data); + } + } + } + for (auto ee : ee_names){ + if (m_masterEEWrenchesIn[ee]->isNew()){ + m_masterEEWrenchesIn[ee]->read(); + wbms->hp_wld_raw.stgt(ee).w = hrp::to_dvector(m_masterEEWrenches[ee].data); + } + } + if (m_actCPIn.isNew()){ m_actCPIn.read(); rel_act_cp = hrp::to_Vector3(m_actCP.data);} + if (m_actZMPIn.isNew()){ m_actZMPIn.read(); rel_act_zmp = hrp::to_Vector3(m_actZMP.data);} + }else{ + for (auto tgt : {"lhand", "rhand"}){ // PAUSE中もボタン読み込みのためにこれらだけは更新・・・ + if (m_masterTgtPosesIn[tgt]->isNew()){ m_masterTgtPosesIn[tgt]->read(); } + } + } + addTimeReport("InPort"); + + ///// Button start + static int button1_flag_count = 0, button2_flag_count = 0; + if(m_masterTgtPoses["rhand"].data.position.x > 0 && m_masterTgtPoses["lhand"].data.position.x > 0 ){ button1_flag_count++; }else{ button1_flag_count = 0; } + if(m_masterTgtPoses["rhand"].data.position.x > 0 && m_masterTgtPoses["lhand"].data.position.x <= 0 ){ button2_flag_count++; }else{ button2_flag_count = 0; } + if(button1_flag_count > 2.0 / m_dt){// keep pless both button for 2s to start + if(mode.now() == MODE_IDLE){ + RTC_INFO_STREAM("startWholeBodyMasterSlave by Button"); + startWholeBodyMasterSlave(); + } + if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){ + RTC_INFO_STREAM("stopWholeBodyMasterSlave by Button"); + stopWholeBodyMasterSlave(); + } + button1_flag_count = 0; + } + if(button2_flag_count > 2.0 / m_dt){// keep pless both button for 2s to start + if(mode.now() == MODE_WBMS){ + RTC_INFO_STREAM("pauseWholeBodyMasterSlave by Button"); + pauseWholeBodyMasterSlave(); + } + if(mode.now() == MODE_PAUSE ){ + RTC_INFO_STREAM("resumeWholeBodyMasterSlave by Button"); + resumeWholeBodyMasterSlave(); + } + button2_flag_count = 0; + } + + processTransition(); + mode.update(); + + if (mode.isRunning()) { + if(mode.isInitialize()){ + preProcessForWholeBodyMasterSlave(); + idsb.setInitState(fik->m_robot, m_dt);//逆動力学初期化 + } + wbms->update();//////HumanSynchronizerの主要処理 + if(DEBUGP)RTC_INFO_STREAM(wbms->rp_ref_out); + addTimeReport("MainFunc"); + + solveFullbodyIK(wbms->rp_ref_out); + addTimeReport("IK"); + + // RHP finger + if(fik->m_robot->name() == "RHP4B"){ + const double hand_max_vel = M_PI/0.4;//0.4s for 180deg + { + const double trigger_in = MINMAX_LIMITED(m_masterTgtPoses["lhand"].data.position.y, 0, 1); + double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI; + fik->m_robot->link("L_HAND")->q = MINMAX_LIMITED(tgt_hand_q, fik->m_robot->link("L_HAND")->llimit, fik->m_robot->link("L_HAND")->ulimit); + avg_q_vel(fik->m_robot->link("L_HAND")->jointId) = hand_max_vel; + }{ + const double trigger_in = MINMAX_LIMITED(m_masterTgtPoses["rhand"].data.position.y, 0, 1); + double tgt_hand_q = (-29.0 + (124.0-(-29.0))*trigger_in ) /180.0*M_PI; + fik->m_robot->link("R_HAND")->q = MINMAX_LIMITED(tgt_hand_q, fik->m_robot->link("R_HAND")->llimit, fik->m_robot->link("R_HAND")->ulimit); + avg_q_vel(fik->m_robot->link("R_HAND")->jointId) = hand_max_vel; + } + } + + smoothingJointAngles(fik->m_robot, m_robot_vsafe); + + hrp::Vector3 com = m_robot_vsafe->calcCM(); + static hrp::Vector3 com_old = com; + static hrp::Vector3 com_old_old = com_old; + hrp::Vector3 com_acc = (com - 2*com_old + com_old_old)/(m_dt*m_dt); + hrp::Vector3 ref_zmp; ref_zmp << com.head(XY)-(com(Z)/G)*com_acc.head(XY), 0; + ref_zmp.head(XY) -= static_balancing_com_offset.head(XY); +// if(mode.isInitialize()){ ref_zmp_filter.reset(ref_zmp); } +// ref_zmp = ref_zmp_filter.passFilter(ref_zmp); + com_old_old = com_old; + com_old = com; + wbms->act_rs.ref_com = com; + wbms->act_rs.ref_zmp = ref_zmp; + wbms->act_rs.st_zmp = m_robot_vsafe->rootLink()->p + m_robot_vsafe->rootLink()->R * rel_act_zmp; + + // qRef + for (int i = 0; i < m_qRef.data.length(); i++ ){ + if(has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){ + m_qRef.data[i] = output_ratio * m_robot_vsafe->joint(i)->q + (1 - output_ratio) * m_qRef.data[i]; + } + } + // basePos + m_basePos.data = hrp::to_Point3D( output_ratio * m_robot_vsafe->rootLink()->p + (1 - output_ratio) * hrp::to_Vector3(m_basePos.data)); + m_basePos.tm = m_qRef.tm; + // baseRpy + m_baseRpy.data = hrp::to_Orientation3D( output_ratio * hrp::rpyFromRot(m_robot_vsafe->rootLink()->R) + (1 - output_ratio) * hrp::to_Vector3(m_baseRpy.data)); + m_baseRpy.tm = m_qRef.tm; + // zmp + hrp::Vector3 rel_ref_zmp = m_robot_vsafe->rootLink()->R.transpose() * (ref_zmp - m_robot_vsafe->rootLink()->p); + m_zmp.data = hrp::to_Point3D( output_ratio * rel_ref_zmp + (1 - output_ratio) * hrp::to_Vector3(m_zmp.data)); + m_zmp.tm = m_qRef.tm; + // m_optionalData + if(m_optionalData.data.length() < optionalDataLength){ + m_optionalData.data.length(optionalDataLength);//TODO:これいいのか？ + for(int i=0;ilegged){ + m_optionalData.data[contact_states_index_map["rleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["rleg"]] = wbms->rp_ref_out.tgt[rf].is_contact(); + m_optionalData.data[contact_states_index_map["lleg"]] = m_optionalData.data[optionalDataLength/2 + contact_states_index_map["lleg"]] = wbms->rp_ref_out.tgt[lf].is_contact(); + } + addTimeReport("SetOutPut"); + + } + wbms->baselinkpose.p = fik->m_robot->rootLink()->p; + wbms->baselinkpose.R = fik->m_robot->rootLink()->R; + // send back auto detected floor height + + if(wbms->legged){ + for(std::string lr : {"l","r"}){ + const hrp::Vector3 floor_pos = wbms->rp_ref_out.stgt(lr+"leg").cnt.p - wbms->rp_ref_out.stgt(lr+"leg").offs.p; + m_slaveTgtPoses[lr+"floor"].data = hrp::to_Pose3D( (hrp::dvector6()<write(); + } + } + + // write + m_qOut.write(); + m_basePosOut.write(); + m_baseRpyOut.write(); + m_zmpOut.write(); + m_optionalDataOut.write(); + addTimeReport("OutPort"); + if(DEBUGP)RTC_INFO_STREAM(time_report_str); + if(DEBUGP)RTC_INFO_STREAM(wbms->ws); + loop ++; + return RTC::RTC_OK; +} + + +void WholeBodyMasterSlave::processTransition(){ + switch(mode.now()){ + + case MODE_SYNC_TO_WBMS: + if(mode.pre() == MODE_IDLE){ double tmp = 1.0; t_ip->setGoal(&tmp, 3.0, true); } + if (!t_ip->isEmpty() ){ + t_ip->get(&output_ratio, true); + }else{ + mode.setNextMode(MODE_WBMS); + } + break; + + case MODE_SYNC_TO_IDLE: + if(mode.pre() == MODE_WBMS || mode.pre() == MODE_PAUSE){ double tmp = 0.0; t_ip->setGoal(&tmp, 3.0, true); } + if (!t_ip->isEmpty()) { + t_ip->get(&output_ratio, true); + }else{ + mode.setNextMode(MODE_IDLE); + } + break; + } +} + + +void WholeBodyMasterSlave::preProcessForWholeBodyMasterSlave(){ + fik->m_robot->rootLink()->p = hrp::to_Vector3(m_basePos.data); + hrp::setQAll(fik->m_robot, hrp::to_dvector(m_qRef.data)); + fik->m_robot->calcForwardKinematics(); + double current_foot_height_from_world = wbms->legged ? ::min(ee_ikc_map["rleg"].getCurrentTargetPos(fik->m_robot)(Z), ee_ikc_map["lleg"].getCurrentTargetPos(fik->m_robot)(Z)) : 0; + RTC_INFO_STREAM("current_foot_height_from_world = "<m_robot, m_robot_vsafe}){//初期姿勢でBodyをFK + hrp::setRobotStateVec(b, hrp::to_dvector(m_qRef.data), hrp::to_Vector3(m_basePos.data) - hrp::Vector3::UnitZ() * current_foot_height_from_world, hrp::rotFromRpy(hrp::to_Vector3(m_baseRpy.data))); + b->calcForwardKinematics(); + } + fik->q_ref = hrp::getRobotStateVec(fik->m_robot); + q_ip->set(fik->q_ref.data()); + wbms->initializeRequest(fik->m_robot, ee_ikc_map); +} + + +void WholeBodyMasterSlave::solveFullbodyIK(HumanPose& ref){ + std::vector ikc_list; + if(wbms->legged){ // free baselink, lleg, rleg, larm, rarm setting + { + IKConstraint tmp; + tmp.target_link_name = fik->m_robot->rootLink()->name; + tmp.localPos = hrp::Vector3::Zero(); + tmp.localR = hrp::Matrix33::Identity(); + tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec + tmp.targetRpy = ref.stgt("com").abs.rpy(); + tmp.constraint_weight << 0, 0, 0, 0.1, 0.1, 0.1; + tmp.rot_precision = deg2rad(3); + ikc_list.push_back(tmp); + } + for(auto leg : {"lleg","rleg"}){ + if(has(wbms->wp.use_targets, leg)){ + IKConstraint tmp; + tmp.target_link_name = ee_ikc_map[leg].target_link_name; + tmp.localPos = ee_ikc_map[leg].localPos; + tmp.localR = ee_ikc_map[leg].localR; + tmp.targetPos = ref.stgt(leg).abs.p; + tmp.targetRpy = ref.stgt(leg).abs.rpy(); + tmp.constraint_weight = wbms->rp_ref_out.tgt[rf].is_contact() ? hrp::dvector6::Constant(3) : hrp::dvector6::Constant(0.1); + ikc_list.push_back(tmp); + } + } + for(auto arm : {"larm","rarm"}){ + if(has(wbms->wp.use_targets, arm)){ + IKConstraint tmp; + tmp.target_link_name = ee_ikc_map[arm].target_link_name; + tmp.localPos = ee_ikc_map[arm].localPos; + tmp.localR = ee_ikc_map[arm].localR; + tmp.targetPos = ref.stgt(arm).abs.p; + tmp.targetRpy = ref.stgt(arm).abs.rpy(); + tmp.constraint_weight = hrp::dvector6::Constant(0.1); + tmp.pos_precision = 3e-3; + tmp.rot_precision = deg2rad(3); + ikc_list.push_back(tmp); + } + } + if(has(wbms->wp.use_targets, "com")){ + IKConstraint tmp; + tmp.target_link_name = "COM"; + tmp.localPos = hrp::Vector3::Zero(); + tmp.localR = hrp::Matrix33::Identity(); + tmp.targetPos = ref.stgt("com").abs.p + static_balancing_com_offset;// COM height will not be constraint + tmp.targetRpy = hrp::Vector3::Zero();//reference angular momentum + tmp.constraint_weight << 3,3,0.01,0,0,0; + ikc_list.push_back(tmp); + } + }else{ // fixed baselink, larm, rarm setting + { + IKConstraint tmp; + tmp.target_link_name = fik->m_robot->rootLink()->name; + tmp.localPos = hrp::Vector3::Zero(); + tmp.localR = hrp::Matrix33::Identity(); + tmp.targetPos = hrp::to_Vector3(m_basePos.data);// will be ignored by selection_vec + tmp.targetRpy = hrp::Vector3::Zero(); + tmp.constraint_weight << hrp::dvector6::Constant(1); + tmp.rot_precision = deg2rad(3); + ikc_list.push_back(tmp); + } + for(auto arm : {"larm","rarm"}){ + if(has(wbms->wp.use_targets, arm)){ + IKConstraint tmp; + tmp.target_link_name = ee_ikc_map[arm].target_link_name; + tmp.localPos = ee_ikc_map[arm].localPos; + tmp.localR = ee_ikc_map[arm].localR; + tmp.targetPos = ref.stgt(arm).abs.p; + tmp.targetRpy = ref.stgt(arm).abs.rpy(); + tmp.constraint_weight << 1, 1, 1, 0.1, 0.1, 0.1; + tmp.pos_precision = 3e-3; + tmp.rot_precision = deg2rad(3); + ikc_list.push_back(tmp); + } + } + } + // common head setting + if(has(wbms->wp.use_targets, "head")){ + if(fik->m_robot->link("HEAD_JOINT1") != NULL){ + IKConstraint tmp; + tmp.target_link_name = "HEAD_JOINT1"; + tmp.targetRpy = ref.stgt("head").abs.rpy(); + tmp.constraint_weight << 0,0,0,0,0.1,0.1; + tmp.rot_precision = deg2rad(1); + ikc_list.push_back(tmp); + } + if(fik->m_robot->link("HEAD_P") != NULL){ + IKConstraint tmp; + tmp.target_link_name = "HEAD_P"; + tmp.targetRpy = ref.stgt("head").abs.rpy(); + tmp.constraint_weight << 0,0,0,0,0.1,0.1; + tmp.rot_precision = deg2rad(1); + ikc_list.push_back(tmp); + } + } + if(wbms->rp_ref_out.tgt[rf].is_contact()){ + sccp->avoid_priority.head(12).head(6).fill(4); + }else{ + sccp->avoid_priority.head(12).head(6).fill(3); + } + if(wbms->rp_ref_out.tgt[lf].is_contact()){ + sccp->avoid_priority.head(12).tail(6).fill(4); + }else{ + sccp->avoid_priority.head(12).tail(6).fill(3); + } + +// sccp->checkCollision(); + + for(int i=0;icollision_info_list.size();i++){ + IKConstraint tmp; + double val_w = (sccp->collision_info_list[i].dist_safe - sccp->collision_info_list[i].dist_cur)*1e2; + LIMIT_MAX(val_w, 3); + tmp.constraint_weight << val_w,val_w,val_w,0,0,0; +// tmp.constraint_weight << 3,3,3,0,0,0; + double margin = 1e-3; + if(sccp->avoid_priority(sccp->collision_info_list[i].id0) > sccp->avoid_priority(sccp->collision_info_list[i].id1)){ + tmp.localPos = sccp->collision_info_list[i].cp1_local; + tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id1)->name; + tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin); + ikc_list.push_back(tmp); + }else if(sccp->avoid_priority(sccp->collision_info_list[i].id0) < sccp->avoid_priority(sccp->collision_info_list[i].id1)){ + tmp.localPos = sccp->collision_info_list[i].cp0_local; + tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id0)->name; + tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin); + ikc_list.push_back(tmp); + }else{ + tmp.localPos = sccp->collision_info_list[i].cp1_local; + tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id1)->name; + tmp.targetPos = sccp->collision_info_list[i].cp0_wld + (sccp->collision_info_list[i].cp1_wld - sccp->collision_info_list[i].cp0_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin); + ikc_list.push_back(tmp); + tmp.localPos = sccp->collision_info_list[i].cp0_local; + tmp.target_link_name = fik->m_robot->joint(sccp->collision_info_list[i].id0)->name; + tmp.targetPos = sccp->collision_info_list[i].cp1_wld + (sccp->collision_info_list[i].cp0_wld - sccp->collision_info_list[i].cp1_wld).normalized() * (sccp->collision_info_list[i].dist_safe + margin); + ikc_list.push_back(tmp); + } +// ikc_list[3].constraint_weight = hrp::dvector6::Constant(1e-4); +// ikc_list[4].constraint_weight = hrp::dvector6::Constant(1e-4); + } + + if(loop%20==0){ + if(sccp->collision_info_list.size()>0){ + std::cout<<"pair:"<collision_info_list.size();i++){ + std::cout<m_robot->joint(sccp->collision_info_list[i].id0)->name<<" "<m_robot->joint(sccp->collision_info_list[i].id1)->name<m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT0")->jointId) = 1e3;//JAXON + if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT1")->jointId) = 1e3; +// if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 10; + if( fik->m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_JOINT2")->jointId) = 0;//実機修理中 + + if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->m_robot->link("CHEST_JOINT0")->llimit = deg2rad(-8); + if( fik->m_robot->link("CHEST_JOINT0") != NULL) fik->m_robot->link("CHEST_JOINT0")->ulimit = deg2rad(8); + if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->m_robot->link("CHEST_JOINT1")->llimit = deg2rad(1); + if( fik->m_robot->link("CHEST_JOINT1") != NULL) fik->m_robot->link("CHEST_JOINT1")->ulimit = deg2rad(32); + + if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->llimit = deg2rad(-20); + if( fik->m_robot->link("HEAD_JOINT0") != NULL) fik->m_robot->link("HEAD_JOINT0")->ulimit = deg2rad(20); + if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->llimit = deg2rad(-15); + if( fik->m_robot->link("HEAD_JOINT1") != NULL) fik->m_robot->link("HEAD_JOINT1")->ulimit = deg2rad(35); + + if( fik->m_robot->link("RARM_JOINT6") != NULL) fik->m_robot->link("RARM_JOINT6")->llimit = deg2rad(-59); + if( fik->m_robot->link("RARM_JOINT6") != NULL) fik->m_robot->link("RARM_JOINT6")->ulimit = deg2rad(59); + if( fik->m_robot->link("RARM_JOINT7") != NULL) fik->m_robot->link("RARM_JOINT7")->llimit = deg2rad(-61); + if( fik->m_robot->link("RARM_JOINT7") != NULL) fik->m_robot->link("RARM_JOINT7")->ulimit = deg2rad(58); + + if( fik->m_robot->link("LARM_JOINT6") != NULL) fik->m_robot->link("LARM_JOINT6")->llimit = deg2rad(-59); + if( fik->m_robot->link("LARM_JOINT6") != NULL) fik->m_robot->link("LARM_JOINT6")->ulimit = deg2rad(59); + if( fik->m_robot->link("LARM_JOINT7") != NULL) fik->m_robot->link("LARM_JOINT7")->llimit = deg2rad(-61); + if( fik->m_robot->link("LARM_JOINT7") != NULL) fik->m_robot->link("LARM_JOINT7")->ulimit = deg2rad(58); + + if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->ulimit = deg2rad(-45);//脇内側の干渉回避 + if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->llimit = deg2rad(45); + if( fik->m_robot->link("RARM_JOINT2") != NULL) fik->m_robot->link("RARM_JOINT2")->llimit = deg2rad(-89);//肩グルン防止 + if( fik->m_robot->link("LARM_JOINT2") != NULL) fik->m_robot->link("LARM_JOINT2")->ulimit = deg2rad(89); + if( fik->m_robot->link("RARM_JOINT4") != NULL) fik->m_robot->link("RARM_JOINT4")->ulimit = deg2rad(1);//肘逆折れ + if( fik->m_robot->link("LARM_JOINT4") != NULL) fik->m_robot->link("LARM_JOINT4")->ulimit = deg2rad(1); + if( fik->m_robot->link("RLEG_JOINT3") != NULL) fik->m_robot->link("RLEG_JOINT3")->llimit = deg2rad(40);//膝伸びきり防止のため + if( fik->m_robot->link("LLEG_JOINT3") != NULL) fik->m_robot->link("LLEG_JOINT3")->llimit = deg2rad(40); + + if( fik->m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_Y")->jointId) = 10;//K + if( fik->m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(fik->m_robot->link("CHEST_P")->jointId) = 10; + if( fik->m_robot->link("R_KNEE_P") != NULL) fik->m_robot->link("R_KNEE_P")->llimit = deg2rad(30);//膝伸びきり防止 + if( fik->m_robot->link("L_KNEE_P") != NULL) fik->m_robot->link("L_KNEE_P")->llimit = deg2rad(30); + // if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->llimit = deg2rad(-40); + // if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->llimit = deg2rad(-40); + // if( fik->m_robot->link("R_WRIST_R") != NULL) fik->m_robot->link("R_WRIST_R")->ulimit = deg2rad(40); + // if( fik->m_robot->link("L_WRIST_R") != NULL) fik->m_robot->link("L_WRIST_R")->ulimit = deg2rad(40); + // if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-40); + // if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-40); + // if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(20); + // if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(20); + if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->llimit = deg2rad(-80); + if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->llimit = deg2rad(-80); + if( fik->m_robot->link("R_WRIST_P") != NULL) fik->m_robot->link("R_WRIST_P")->ulimit = deg2rad(45); + if( fik->m_robot->link("L_WRIST_P") != NULL) fik->m_robot->link("L_WRIST_P")->ulimit = deg2rad(45); + if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->llimit = deg2rad(-20); + if( fik->m_robot->link("CHEST_Y") != NULL) fik->m_robot->link("CHEST_Y")->ulimit = deg2rad(20); + if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->llimit = deg2rad(0); + if( fik->m_robot->link("CHEST_P") != NULL) fik->m_robot->link("CHEST_P")->ulimit = deg2rad(60); + if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->llimit = deg2rad(-5); + if( fik->m_robot->link("HEAD_Y") != NULL) fik->m_robot->link("HEAD_Y")->ulimit = deg2rad(5); + if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->llimit = deg2rad(0); + if( fik->m_robot->link("HEAD_P") != NULL) fik->m_robot->link("HEAD_P")->ulimit = deg2rad(60); + for(int i=0;im_robot->numJoints();i++){ + LIMIT_MINMAX(fik->m_robot->joint(i)->q, fik->m_robot->joint(i)->llimit, fik->m_robot->joint(i)->ulimit); + } + + fik->q_ref.head(m_qRef.data.length()) = hrp::to_dvector(m_qRef.data);//あえてseqからのbaselink poseは信用しない + + for(int i=0; im_robot->numJoints(); i++){ + if(!has(wbms->wp.use_joints, fik->m_robot->joint(i)->name)){ + fik->dq_weight_all(i) = 0; + fik->m_robot->joint(i)->q = m_qRef.data[i]; + } + } + + + if(fik->m_robot->name().find("JAXON") != std::string::npos){ + for(int i=0; im_robot->numJoints(); i++){ + if(fik->m_robot->joint(i)->name.find("ARM") != std::string::npos){ + fik->q_ref_constraint_weight(i) = 1e-3;//腕だけ + } + } + } + + const int IK_MAX_LOOP = 1; + int loop_result = fik->solveFullbodyIKLoop(ikc_list, IK_MAX_LOOP); +} + +void WholeBodyMasterSlave::smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe){ + double goal_time = 0.0; + const double min_goal_time_offset = 0.3; + + static hrp::dvector ans_state_vel = hrp::dvector::Zero(fik->numStates()); + + const hrp::dvector estimated_times_from_vel_limit = (hrp::getRobotStateVec(_robot) - hrp::getRobotStateVec(_robot_safe)).array().abs() / avg_q_vel.array(); + const hrp::dvector estimated_times_from_acc_limit = ans_state_vel.array().abs() / avg_q_acc.array(); + const double longest_estimated_time_from_vel_limit = estimated_times_from_vel_limit.maxCoeff(); + const double longest_estimated_time_from_acc_limit = estimated_times_from_acc_limit.maxCoeff(); + goal_time = hrp::Vector3(longest_estimated_time_from_vel_limit, longest_estimated_time_from_acc_limit, min_goal_time_offset).maxCoeff(); + + q_ip->setGoal(hrp::getRobotStateVec(_robot).data(), goal_time, true); + double tmp[fik->numStates()], tmpv[fik->numStates()]; + if (!q_ip->isEmpty() ){ q_ip->get(tmp, tmpv, true);} + hrp::dvector ans_state = Eigen::Map(tmp, fik->numStates()); + ans_state_vel = Eigen::Map(tmpv, fik->numStates()); + + hrp::setRobotStateVec(_robot_safe, ans_state); + for(int i=0; i<_robot_safe->numJoints(); i++){ + LIMIT_MINMAX(_robot_safe->joint(i)->q, _robot_safe->joint(i)->llimit, _robot_safe->joint(i)->ulimit); + } + + _robot_safe->calcForwardKinematics(); +} + + +bool WholeBodyMasterSlave::startWholeBodyMasterSlave(){ + if(mode.now() == MODE_IDLE){ + RTC_INFO_STREAM("startWholeBodyMasterSlave"); + mode.setNextMode(MODE_SYNC_TO_WBMS); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to startWholeBodyMasterSlave"); + return false; + } +} + + +bool WholeBodyMasterSlave::pauseWholeBodyMasterSlave(){ + if(mode.now() == MODE_WBMS){ + RTC_INFO_STREAM("pauseWholeBodyMasterSlave"); + mode.setNextMode(MODE_PAUSE); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to pauseWholeBodyMasterSlave"); + return false; + } +} + + +bool WholeBodyMasterSlave::resumeWholeBodyMasterSlave(){ + if(mode.now() == MODE_PAUSE){ + RTC_INFO_STREAM("resumeWholeBodyMasterSlave"); + mode.setNextMode(MODE_WBMS); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to resumeWholeBodyMasterSlave"); + return false; + } +} + + +bool WholeBodyMasterSlave::stopWholeBodyMasterSlave(){ + if(mode.now() == MODE_WBMS || mode.now() == MODE_PAUSE ){ + RTC_INFO_STREAM("stopWholeBodyMasterSlave"); + mode.setNextMode(MODE_SYNC_TO_IDLE); + return true; + }else{ + RTC_WARN_STREAM("Invalid context to stopWholeBodyMasterSlave"); + return false; + } +} +namespace hrp{ +// hrp::Vector2 to_Vector2(const OpenHRP::WholeBodyMasterSlaveService::DblSequence2& in){ return (hrp::Vector2()<< in[0],in[1]).finished(); } +// hrp::Vector2 to_Vector3(const OpenHRP::WholeBodyMasterSlaveService::DblSequence3& in){ return hrp::dvector::Map(in.get_buffer(), in.length()); } +// hrp::Vector2 to_Vector4(const OpenHRP::WholeBodyMasterSlaveService::DblSequence4& in){ return (hrp::Vector4()<< in[0],in[1]).finished(); } + OpenHRP::WholeBodyMasterSlaveService::DblSequence2 to_DblSequence2(const hrp::Vector2& in){ + OpenHRP::WholeBodyMasterSlaveService::DblSequence2 ret; ret.length(in.size()); hrp::Vector2::Map(ret.get_buffer()) = in; return ret; } + OpenHRP::WholeBodyMasterSlaveService::DblSequence3 to_DblSequence3(const hrp::Vector3& in){ + OpenHRP::WholeBodyMasterSlaveService::DblSequence3 ret; ret.length(in.size()); hrp::Vector3::Map(ret.get_buffer()) = in; return ret; } + OpenHRP::WholeBodyMasterSlaveService::DblSequence4 to_DblSequence4(const hrp::Vector4& in){ + OpenHRP::WholeBodyMasterSlaveService::DblSequence4 ret; ret.length(in.size()); hrp::Vector4::Map(ret.get_buffer()) = in; return ret; } +} + +bool WholeBodyMasterSlave::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){ + RTC_INFO_STREAM("setWholeBodyMasterSlaveParam"); + wbms->wp.auto_com_mode = i_param.auto_com_mode; + wbms->wp.auto_floor_h_mode = i_param.auto_floor_h_mode; + wbms->wp.auto_foot_landing_by_act_cp = i_param.auto_foot_landing_by_act_cp; + wbms->wp.auto_foot_landing_by_act_zmp = i_param.auto_foot_landing_by_act_zmp; + wbms->wp.additional_double_support_time = i_param.additional_double_support_time; + wbms->wp.auto_com_foot_move_detect_height = i_param.auto_com_foot_move_detect_height; + wbms->wp.auto_floor_h_detect_fz = i_param.auto_floor_h_detect_fz; + wbms->wp.auto_floor_h_reset_fz = i_param.auto_floor_h_reset_fz; + wbms->wp.base_to_hand_min_distance = i_param.base_to_hand_min_distance; + wbms->wp.capture_point_extend_ratio = i_param.capture_point_extend_ratio; + wbms->wp.com_filter_cutoff_hz = i_param.com_filter_cutoff_hz; + wbms->wp.foot_collision_avoidance_distance = i_param.foot_collision_avoidance_distance; + wbms->wp.foot_landing_vel = i_param.foot_landing_vel; + wbms->wp.force_double_support_com_h = i_param.force_double_support_com_h; + wbms->wp.human_to_robot_ratio = i_param.human_to_robot_ratio; + wbms->wp.max_double_support_width = i_param.max_double_support_width; + wbms->wp.upper_body_rmc_ratio = i_param.upper_body_rmc_ratio; + wbms->wp.single_foot_zmp_safety_distance = i_param.single_foot_zmp_safety_distance; + wbms->wp.swing_foot_height_offset = i_param.swing_foot_height_offset; + wbms->wp.com_offset = hrp::Vector3::Map(i_param.com_offset.get_buffer()); + wbms->wp.actual_foot_vert_fbio = hrp::Vector4::Map(i_param.actual_foot_vert_fbio.get_buffer()); + wbms->wp.safety_foot_vert_fbio = hrp::Vector4::Map(i_param.safety_foot_vert_fbio.get_buffer()); + wbms->wp.use_joints = hrp::to_string_vector(i_param.use_joints); + wbms->wp.use_targets = hrp::to_string_vector(i_param.use_targets); + wbms->wp.CheckSafeLimit(); + return true; +} + + +bool WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param){ + RTC_INFO_STREAM("getWholeBodyMasterSlaveParam"); + i_param.auto_com_mode = wbms->wp.auto_com_mode; + i_param.auto_floor_h_mode = wbms->wp.auto_floor_h_mode; + i_param.auto_foot_landing_by_act_cp = wbms->wp.auto_foot_landing_by_act_cp; + i_param.auto_foot_landing_by_act_zmp = wbms->wp.auto_foot_landing_by_act_zmp; + i_param.additional_double_support_time = wbms->wp.additional_double_support_time; + i_param.auto_com_foot_move_detect_height = wbms->wp.auto_com_foot_move_detect_height; + i_param.auto_floor_h_detect_fz = wbms->wp.auto_floor_h_detect_fz; + i_param.auto_floor_h_reset_fz = wbms->wp.auto_floor_h_reset_fz; + i_param.base_to_hand_min_distance = wbms->wp.base_to_hand_min_distance; + i_param.capture_point_extend_ratio = wbms->wp.capture_point_extend_ratio; + i_param.com_filter_cutoff_hz = wbms->wp.com_filter_cutoff_hz; + i_param.foot_collision_avoidance_distance = wbms->wp.foot_collision_avoidance_distance; + i_param.foot_landing_vel = wbms->wp.foot_landing_vel; + i_param.force_double_support_com_h = wbms->wp.force_double_support_com_h; + i_param.human_to_robot_ratio = wbms->wp.human_to_robot_ratio; + i_param.max_double_support_width = wbms->wp.max_double_support_width; + i_param.upper_body_rmc_ratio = wbms->wp.upper_body_rmc_ratio; + i_param.single_foot_zmp_safety_distance = wbms->wp.single_foot_zmp_safety_distance; + i_param.swing_foot_height_offset = wbms->wp.swing_foot_height_offset; + i_param.com_offset = hrp::to_DblSequence3(wbms->wp.com_offset); + i_param.actual_foot_vert_fbio = hrp::to_DblSequence4(wbms->wp.actual_foot_vert_fbio); + i_param.safety_foot_vert_fbio = hrp::to_DblSequence4(wbms->wp.safety_foot_vert_fbio); + i_param.use_joints = hrp::to_StrSequence(wbms->wp.use_joints); + i_param.use_targets = hrp::to_StrSequence(wbms->wp.use_targets); + return true; +} + + +RTC::ReturnCode_t WholeBodyMasterSlave::onActivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onActivated(" << ec_id << ")"); return RTC::RTC_OK; } +RTC::ReturnCode_t WholeBodyMasterSlave::onDeactivated(RTC::UniqueId ec_id){ RTC_INFO_STREAM("onDeactivated(" << ec_id << ")"); return RTC::RTC_OK; } +RTC::ReturnCode_t WholeBodyMasterSlave::onFinalize(){ return RTC::RTC_OK; } + +extern "C"{ + void WholeBodyMasterSlaveInit(RTC::Manager* manager) { + RTC::Properties profile(WholeBodyMasterSlave_spec); + manager->registerFactory(profile, RTC::Create, RTC::Delete); + } +}; diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h new file mode 100644 index 00000000000..b2306fbf34c --- /dev/null +++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlave.h @@ -0,0 +1,203 @@ +#ifndef WholeBodyMasterSlave_H +#define WholeBodyMasterSlave_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "WholeBodyMasterSlaveService_impl.h" +#include "../Stabilizer/StabilizerService_impl.h" +#include "../AutoBalancer/AutoBalancerService_impl.h" +#include "wbms_core.h" + +//#define USE_DEBUG_PORT + +enum mode_enum{ MODE_IDLE, MODE_SYNC_TO_WBMS, MODE_WBMS, MODE_PAUSE, MODE_SYNC_TO_IDLE}; + +class ControlMode{ + private: + mode_enum current, previous, next; + public: + ControlMode(){ current = previous = next = MODE_IDLE;} + ~ControlMode(){} + bool setNextMode(const mode_enum _request){ + switch(_request){ + case MODE_SYNC_TO_WBMS: + if(current == MODE_IDLE){ next = MODE_SYNC_TO_WBMS; return true; }else{ return false; } + case MODE_WBMS: + if(current == MODE_SYNC_TO_WBMS || current == MODE_PAUSE ){ next = MODE_WBMS; return true; }else{ return false; } + case MODE_PAUSE: + if(current == MODE_WBMS){ next = MODE_PAUSE; return true; }else{ return false; } + case MODE_SYNC_TO_IDLE: + if(current == MODE_WBMS || current == MODE_PAUSE ){ next = MODE_SYNC_TO_IDLE; return true; }else{ return false; } + case MODE_IDLE: + if(current == MODE_SYNC_TO_IDLE ){ next = MODE_IDLE; return true; }else{ return false; } + default: + return false; + } + } + void update(){ previous = current; current = next; } + mode_enum now(){ return current; } + mode_enum pre(){ return previous; } + bool isRunning(){ return (current==MODE_SYNC_TO_WBMS) || (current==MODE_WBMS) || (current==MODE_PAUSE) || (current==MODE_SYNC_TO_IDLE) ;} + bool isInitialize(){ return (previous==MODE_IDLE) && (current==MODE_SYNC_TO_WBMS) ;} +}; + +namespace hrp{ + inline std::vector to_string_vector (const OpenHRP::WholeBodyMasterSlaveService::StrSequence& in) { + std::vector ret(in.length()); for(int i=0; i& in){ + OpenHRP::WholeBodyMasterSlaveService::StrSequence ret; ret.length(in.size()); for(int i=0; i m_qRefIn; + RTC::TimedDoubleSeq m_qAct; + RTC::InPort m_qActIn; + RTC::TimedPoint3D m_basePos; + RTC::InPort m_basePosIn; + RTC::TimedOrientation3D m_baseRpy; + RTC::InPort m_baseRpyIn; + RTC::TimedPoint3D m_zmp; + RTC::InPort m_zmpIn; + RTC::TimedDoubleSeq m_optionalData; + RTC::InPort m_optionalDataIn; + typedef boost::shared_ptr > ITP3_Ptr; + typedef boost::shared_ptr > ITDS_Ptr; + typedef boost::shared_ptr > OTP3_Ptr; + typedef boost::shared_ptr > OTDS_Ptr; + + std::map m_masterTgtPoses; + std::map m_masterTgtPosesIn; + + std::map m_masterEEWrenches; + std::map m_masterEEWrenchesIn; + + std::map m_slaveEEWrenches; + std::map m_slaveEEWrenchesOut; + + std::map m_slaveTgtPoses; + std::map m_slaveTgtPosesOut; + + std::map m_localEEWrenches; + std::map m_localEEWrenchesIn; + + RTC::Time m_delayCheckPacket; + RTC::InPort m_delayCheckPacketInboundIn; + RTC::OutPort m_delayCheckPacketOutboundOut; + + RTC::TimedDoubleSeq m_exData; + RTC::InPort m_exDataIn; + RTC::TimedStringSeq m_exDataIndex; + RTC::InPort m_exDataIndexIn; + + RTC::TimedPoint3D m_actCP; + RTC::InPort m_actCPIn; + RTC::TimedPoint3D m_actZMP; + RTC::InPort m_actZMPIn; + + RTC::OutPort m_qOut; + RTC::OutPort m_zmpOut; + RTC::OutPort m_basePosOut; + RTC::OutPort m_baseRpyOut; + RTC::OutPort m_optionalDataOut; + RTC::CorbaPort m_WholeBodyMasterSlaveServicePort; + + WholeBodyMasterSlaveService_impl m_service0; + + RTC::CorbaPort m_AutoBalancerServicePort; + RTC::CorbaPort m_StabilizerServicePort; + RTC::CorbaConsumer m_AutoBalancerServiceConsumer; + RTC::CorbaConsumer m_StabilizerServiceConsumer; + + + private: + double m_dt; + unsigned int loop; + unsigned int m_debugLevel; + int optionalDataLength; + hrp::BodyPtr m_robot_act; // actual + hrp::BodyPtr m_robot_vsafe; // joint trajectory safe + typedef boost::shared_ptr fikPtr; + fikPtr fik; + std::map ee_ikc_map; // e.g. feet hands head com + std::map contact_states_index_map; + + double output_ratio; + interpolator *t_ip,*q_ip; + hrp::dvector avg_q_vel, avg_q_acc; + + hrp::InvDynStateBuffer idsb; + BiquadIIRFilterVec ref_zmp_filter; + std::map ee_f_filter; + + HumanPose raw_pose; + + boost::shared_ptr wbms; + boost::shared_ptr sccp; + + hrp::Vector3 torso_rot_rmc; + ControlMode mode; + + hrp::Vector3 rel_act_cp; + hrp::Vector3 rel_act_zmp; + struct timespec startT, endT; + std::string time_report_str; + + hrp::Vector3 static_balancing_com_offset; + + std::vector ee_names; + std::vector tgt_names; + + RTC::ReturnCode_t setupEEIKConstraintFromConf(std::map& _ee_ikc_map, hrp::BodyPtr _robot, RTC::Properties& _prop); + void solveFullbodyIK(HumanPose& ref); + void processTransition(); + void preProcessForWholeBodyMasterSlave(); + void processWholeBodyMasterSlave(const HumanPose& ref); + void smoothingJointAngles(hrp::BodyPtr _robot, hrp::BodyPtr _robot_safe); + bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; } + void addTimeReport(const std::string& prefix){ + clock_gettime(CLOCK_REALTIME, &endT); + std::stringstream ss; + ss << prefix << "= " << std::fixed < +#include +#include +#include "WholeBodyMasterSlave.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + WholeBodyMasterSlaveInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("WholeBodyMasterSlave"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference +// RTC::RTObject_var rtobj; +// rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component +// PortList* portlist; +// portlist = rtobj->get_ports(); + + // getting port profiles +// std::cout << "Number of Ports: "; +// std::cout << portlist->length() << std::endl << std::endl; +// for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) +// { +// Port_ptr port; +// port = (*portlist)[i]; +// std::cout << "Port" << i << " (name): "; +// std::cout << port->get_port_profile()->name << std::endl; +// +// RTC::PortInterfaceProfileList iflist; +// iflist = port->get_port_profile()->interfaces; +// std::cout << "---interfaces---" << std::endl; +// for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) +// { +// std::cout << "I/F name: "; +// std::cout << iflist[i].instance_name << std::endl; +// std::cout << "I/F type: "; +// std::cout << iflist[i].type_name << std::endl; +// const char* pol; +// pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; +// std::cout << "Polarity: " << pol << std::endl; +// } +// std::cout << "---properties---" << std::endl; +// NVUtil::dump(port->get_port_profile()->properties); +// std::cout << "----------------" << std::endl << std::endl; +// } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp new file mode 100644 index 00000000000..63ed8dc8ff6 --- /dev/null +++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.cpp @@ -0,0 +1,47 @@ +#include "WholeBodyMasterSlaveService_impl.h" +#include "WholeBodyMasterSlave.h" + +WholeBodyMasterSlaveService_impl::WholeBodyMasterSlaveService_impl() : m_wholebodymasterslave(NULL) +{ +} + +WholeBodyMasterSlaveService_impl::~WholeBodyMasterSlaveService_impl() +{ +} + +void WholeBodyMasterSlaveService_impl::wholebodymasterslave(WholeBodyMasterSlave *i_wholebodymasterslave) +{ + m_wholebodymasterslave = i_wholebodymasterslave; +} + +CORBA::Boolean WholeBodyMasterSlaveService_impl::startWholeBodyMasterSlave() +{ + return m_wholebodymasterslave->startWholeBodyMasterSlave(); +}; + +CORBA::Boolean WholeBodyMasterSlaveService_impl::stopWholeBodyMasterSlave() +{ + return m_wholebodymasterslave->stopWholeBodyMasterSlave(); +}; + +CORBA::Boolean WholeBodyMasterSlaveService_impl::pauseWholeBodyMasterSlave() +{ + return m_wholebodymasterslave->pauseWholeBodyMasterSlave(); +}; + +CORBA::Boolean WholeBodyMasterSlaveService_impl::resumeWholeBodyMasterSlave() +{ + return m_wholebodymasterslave->resumeWholeBodyMasterSlave(); +}; + +void WholeBodyMasterSlaveService_impl::setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param) +{ + m_wholebodymasterslave->setParams(i_param); +}; + +void WholeBodyMasterSlaveService_impl::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param) +{ + i_param = new OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam(); + m_wholebodymasterslave->getParams(*i_param); +// m_wholebodymasterslave->getParams(i_param); // error: no matching function for call to ‘WholeBodyMasterSlave::getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out&)’ +}; diff --git a/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h new file mode 100644 index 00000000000..92026dabb6b --- /dev/null +++ b/rtc/WholeBodyMasterSlave/WholeBodyMasterSlaveService_impl.h @@ -0,0 +1,31 @@ +// -*-C++-*- +#ifndef WholeBodyMasterSlaveSERVICESVC_IMPL_H +#define WholeBodyMasterSlaveSERVICESVC_IMPL_H + +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/WholeBodyMasterSlaveService.hh" + +using namespace OpenHRP; + +class WholeBodyMasterSlave; + +class WholeBodyMasterSlaveService_impl + : public virtual POA_OpenHRP::WholeBodyMasterSlaveService, + public virtual PortableServer::RefCountServantBase +{ +public: + WholeBodyMasterSlaveService_impl(); + virtual ~WholeBodyMasterSlaveService_impl(); + CORBA::Boolean startWholeBodyMasterSlave(); + CORBA::Boolean stopWholeBodyMasterSlave(); + CORBA::Boolean pauseWholeBodyMasterSlave(); + CORBA::Boolean resumeWholeBodyMasterSlave(); + void setParams(const OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam& i_param); + void getParams(OpenHRP::WholeBodyMasterSlaveService::WholeBodyMasterSlaveParam_out i_param); + // + void wholebodymasterslave(WholeBodyMasterSlave *i_wholebodymasterslave); +private: + WholeBodyMasterSlave *m_wholebodymasterslave; +}; + +#endif diff --git a/rtc/WholeBodyMasterSlave/wbms_core.h b/rtc/WholeBodyMasterSlave/wbms_core.h new file mode 100644 index 00000000000..6f16e91b473 --- /dev/null +++ b/rtc/WholeBodyMasterSlave/wbms_core.h @@ -0,0 +1,838 @@ +#ifndef WBMS_CORE_H +#define WBMS_CORE_H + +#include +#include +#include "../SequencePlayer/interpolator.h" +#include "../ImpedanceController/JointPathEx.h" +#include "../AutoBalancer/FullbodyInverseKinematicsSolver.h" +// geometry +#include +#include +#include +#include +#include +//#include +namespace bg = boost::geometry; +typedef bg::model::d2::point_xy bg_point; +typedef bg::model::multi_point bg_multi_point; +typedef bg::model::linestring bg_linestring; +typedef bg::model::polygon bg_polygon; + +//#include +//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix2d) +//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Matrix3d) +//EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d) + + + +#define DEBUG 0 + +inline bg_point to_bg_point(const hrp::Vector2& hrp_point2d){ return bg_point(hrp_point2d(X), hrp_point2d(Y)); } +inline hrp::Vector2 to_Vector2(const bg_point& bg_point2d){ return hrp::Vector2(bg_point2d.x(), bg_point2d.y()); } +inline bg_polygon to_bg_hull(const hrp::dmatrix& hrp_hull){ + if(hrp_hull.rows() != 2){ std::cerr << "Invalid input for to_bg_hull" << std::endl; dbgn(hrp_hull); } + bg_polygon hull_bg; + hull_bg.outer().resize(hrp_hull.cols()); + for(int i=0; i com; + void update(const hrp::Vector3 com_new){ com.pop_back(); com.push_front(com_new); } + hrp::Vector3 vel(){ return (com[1] - com[0]) / dt ; } + hrp::Vector3 DCM(){ return com[0] + vel() * sqrt( com_height / G ); } + hrp::Vector3 CCM(){ return com[0] - vel() * sqrt( com_height / G ); } + hrp::Vector3 acc(){ return (com[2] - 2* com[1] + com[0]) / ( dt * dt ); } +// hrp::Vector3 ZMP(){ return (hrp::Vector3() << com[0].head(XY) - acc().head(XY) * ( com_height / G ), com[0](Z) - com_height).finished; } +}; + +class PoseTGT{ + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + hrp::Pose3 abs, offs, cnt; + hrp::dvector6 w; + bool go_contact; + PoseTGT(){ reset(); } + ~PoseTGT(){} + void reset(){ abs.reset(); offs.reset(); cnt.reset(); w.fill(0); go_contact = false;} + bool is_contact(){ + const double contact_threshold = 0.005; + // return ((abs.p(Z) - offs.p(Z)) < contact_threshold); + return ((abs.p(Z) - cnt.p(Z)) < contact_threshold); + } +}; + +class HumanPose{ + public: + std::vector tgt; + HumanPose(){ tgt.resize(num_pose_tgt); } + ~HumanPose(){cerr<<"HumanPose destructed"<::iterator it = tgt.begin(); it != tgt.end(); it++){ it->reset(); } } + friend ostream& operator<<(ostream& os, const HumanPose& in){ + const std::vector short_ns = {"c","rf","lf","rh","lh","hd","z"}; + os << std::fixed << std::setprecision(1); + for (int i=0; i= 0 + double b = u|v; + double c = v|v; // always >= 0 + double d = u|w; + double e = v|w; + double D = a*c - b*b; // always >= 0 + double sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0 + double tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0 + + // compute the line parameters of the two closest points +#define EPS 1e-8 + if (D < EPS) { // the lines are almost parallel + sN = 0.0; // force using point P0 on segment S1 + sD = 1.0; // to prevent possible division by 0.0 later + tN = e; + tD = c; + } + else { // get the closest points on the infinite lines + sN = (b*e - c*d); + tN = (a*e - b*d); + if (sN < 0.0) { // sc < 0 => the s=0 edge is visible + sN = 0.0; + tN = e; + tD = c; + } + else if (sN > sD) { // sc > 1 => the s=1 edge is visible + sN = sD; + tN = e + b; + tD = c; + } + } + + if (tN < 0.0) { // tc < 0 => the t=0 edge is visible + tN = 0.0; + // recompute sc for this edge + if (-d < 0.0) + sN = 0.0; + else if (-d > a) + sN = sD; + else { + sN = -d; + sD = a; + } + } + else if (tN > tD) { // tc > 1 => the t=1 edge is visible + tN = tD; + // recompute sc for this edge + if ((-d + b) < 0.0) + sN = 0; + else if ((-d + b) > a) + sN = sD; + else { + sN = (-d + b); + sD = a; + } + } + // finally do the division to get sc and tc + sc = (fabs(sN) < EPS ? 0.0 : sN / sD); + tc = (fabs(tN) < EPS ? 0.0 : tN / tD); + + cp0 = u0 + sc * u; + cp1 = v0 + tc * v; + + // get the difference of the two closest points + Point dP = cp0 - cp1; + + return dP.Magnitude(); // return the closest distance +} + +class Capsule{ + public: + hrp::Vector3 p0, p1; + double r; + Capsule(const hrp::Vector3 _p0 = hrp::Vector3::Zero(), const hrp::Vector3 _p1 = hrp::Vector3::Zero(), const double _r = 0){ + p0 = _p0; p1 = _p1; r = _r; + } +}; + +class CollisionInfo{ + public: + int id0, id1; + hrp::Vector3 cp0_local, cp1_local, cp0_wld, cp1_wld; + double dist_safe, dist_cur; + CollisionInfo(const int _id0 = 0, const int _id1 = 0, + const hrp::Vector3 _cp0_local = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_local = hrp::Vector3::Zero(), + const hrp::Vector3 _cp0_wld = hrp::Vector3::Zero(), const hrp::Vector3 _cp1_wld = hrp::Vector3::Zero(), + const double _dist_safe = 0, const double _dist_cur = 0){ + id0 = _id0; id1 = _id1; cp0_local = _cp0_local; cp1_local = _cp1_local; cp0_wld = _cp0_wld; cp1_wld = _cp1_wld; dist_safe = _dist_safe; dist_cur = _dist_cur; + } +}; + +typedef std::vector CapsuleArray; + +class CapsuleCollisionChecker { + private: + const hrp::BodyPtr m_robot; + std::vector capsule_array_list_local, capsule_array_list_wld; + public: + std::vector collision_info_list; + Eigen::MatrixXi check_pair_mat; + hrp::ivector avoid_priority; + bool hasStr(const std::string& str, const std::string key){ return str.find(key) != std::string::npos; } + CapsuleCollisionChecker(hrp::BodyPtr robot): + m_robot(robot){ + capsule_array_list_local.resize(m_robot->numJoints()); + for(int i=0; inumJoints(); i++){ + if(hasStr(m_robot->joint(i)->name,"LEG_JOINT0")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.099)); + } + if(hasStr(m_robot->joint(i)->name,"LLEG_JOINT2")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.095)); + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0.05,0), m_robot->joint(i)->child->b, 0.095)); + } + if(hasStr(m_robot->joint(i)->name,"RLEG_JOINT2")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.095)); + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,-0.05,0), m_robot->joint(i)->child->b, 0.095)); + } + if(hasStr(m_robot->joint(i)->name,"LEEIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Matrix2d)G_JOINT3")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0.05,0,0), m_robot->joint(i)->child->b, 0.095)); + } + if(hasStr(m_robot->joint(i)->name,"ARM_JOINT2") || hasStr(m_robot->joint(i)->name,"ARM_JOINT3") || hasStr(m_robot->joint(i)->name,"ARM_JOINT4") || hasStr(m_robot->joint(i)->name,"ARM_JOINT5") || hasStr(m_robot->joint(i)->name,"ARM_JOINT6")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.09)); + } + if(hasStr(m_robot->joint(i)->name,"ARM_JOINT7")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), hrp::Vector3(0,0,-0.35), 0.1)); + } + if(hasStr(m_robot->joint(i)->name,"CHEST_JOINT1") || hasStr(m_robot->joint(i)->name,"CHEST_JOINT2")){ + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(0,0,0), m_robot->joint(i)->child->b, 0.2)); + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(-0.1,0,0), m_robot->joint(i)->child->b + hrp::Vector3(-0.1,0,0), 0.2)); + capsule_array_list_local[i].push_back(Capsule(hrp::Vector3(-0.2,0,0), m_robot->joint(i)->child->b + hrp::Vector3(-0.2,0,0), 0.2)); + } + }; + + for(int i=0; inumJoints(); i++){ + for(int j=0; jjoint(i)->name<<" capsule["<numJoints();me++){ + for(int you=me; younumJoints();you++){ + if(me == you) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("RLEG_JOINT") != std::string::npos && m_robot->joint(you)->name.find("RLEG_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("LLEG_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LLEG_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("RARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("RARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("LARM_JOINT") != std::string::npos && m_robot->joint(you)->name.find("LARM_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; +// if(m_robot->link(me)->parent == m_robot->link(you) || m_robot->link(you)->parent == m_robot->link(me)) check_pair_mat(me, you) = 0;//実機とシミュで挙動違う？ + if(m_robot->joint(me)->parent == m_robot->joint(you) || m_robot->joint(you)->parent == m_robot->joint(me)) check_pair_mat(me, you) = 0; + + if(m_robot->joint(me)->name.find("LEG_JOINT0") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("LEG_JOINT1") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("LEG_JOINT2") != std::string::npos && m_robot->joint(you)->name.find("CHEST_JOINT") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT0") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT1") != std::string::npos) check_pair_mat(me, you) = 0; + if(m_robot->joint(me)->name.find("CHEST_JOINT") != std::string::npos && m_robot->joint(you)->name.find("ARM_JOINT2") != std::string::npos) check_pair_mat(me, you) = 0; + } + } + dbgn(check_pair_mat); + avoid_priority = hrp::ivector::Zero(m_robot->numJoints()); + + for(int i=0;inumJoints();i++){ + if(m_robot->joint(i)->name.find("ARM_JOINT") != std::string::npos) avoid_priority(i) = 1; + if(m_robot->joint(i)->name.find("CHEST_JOINT") != std::string::npos) avoid_priority(i) = 2; + if(m_robot->joint(i)->name.find("LEG_JOINT") != std::string::npos) avoid_priority(i) = 3; + } + + } + ~CapsuleCollisionChecker(){cerr<<"CapsuleCollisionChecker destructed"<numJoints();i++){ + for(int j=0;jjoint(i)->p + m_robot->joint(i)->R * capsule_array_list_local[i][j].p0; + capsule_array_list_wld[i][j].p1 = m_robot->joint(i)->p + m_robot->joint(i)->R * capsule_array_list_local[i][j].p1; + } + }; + } + Point Vec3ToPoint(const hrp::Vector3& in){ return Point(in(0),in(1),in(2)); } + bool checkCollision(){ + update(); + collision_info_list.clear(); + for(int me=0;menumJoints();me++){ + for(int you=me; younumJoints();you++){ + + for(int mec=0;mec 0 && capsule_array_list_wld[you][youc].r > 0 && check_pair_mat(me,you)){ + Point cp0_ans, cp1_ans; + double dist_ans = SegSegDist2(Vec3ToPoint(capsule_array_list_wld[me][mec].p0), Vec3ToPoint(capsule_array_list_wld[me][mec].p1-capsule_array_list_wld[me][mec].p0), + Vec3ToPoint(capsule_array_list_wld[you][youc].p0), Vec3ToPoint(capsule_array_list_wld[you][youc].p1-capsule_array_list_wld[you][youc].p0), cp0_ans, cp1_ans); + double dist_safe = capsule_array_list_wld[me][mec].r + capsule_array_list_wld[you][youc].r; + if (dist_ans < dist_safe){ + hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z); + hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z); + collision_info_list.push_back(CollisionInfo(me, you, + m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p), + cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans)); + } + } + } + } + + } + }; + return (collision_info_list.size() > 0 ); + } +// bool checkCapsuleArray2CapsuleArray(const Capsule){ +// if(capsule_array_list_wld[me].r > 0 && capsule_array_list_wld[you].r > 0 && check_pair_mat(me,you)){ +// Point cp0_ans, cp1_ans; +// double dist_ans = SegSegDist2(Vec3ToPoint(capsule_array_list_wld[me].p0), Vec3ToPoint(capsule_array_list_wld[me].p1-capsule_array_list_wld[me].p0), +// Vec3ToPoint(capsule_array_list_wld[you].p0), Vec3ToPoint(capsule_array_list_wld[you].p1-capsule_array_list_wld[you].p0), cp0_ans, cp1_ans); +// double dist_safe = capsule_array_list_wld[me].r + capsule_array_list_wld[you].r; +// if (dist_ans < dist_safe){ +// hrp::Vector3 cp0_wld_tmp = hrp::Vector3(cp0_ans.x,cp0_ans.y,cp0_ans.z); +// hrp::Vector3 cp1_wld_tmp = hrp::Vector3(cp1_ans.x,cp1_ans.y,cp1_ans.z); +// collision_info_list.push_back(CollisionInfo(me, you, +// m_robot->joint(me)->R.transpose() * (cp0_wld_tmp - m_robot->joint(me)->p), m_robot->joint(you)->R.transpose() * (cp1_wld_tmp - m_robot->joint(you)->p), +// cp0_wld_tmp, cp1_wld_tmp, dist_safe, dist_ans)); +// } +// } +// } +}; + +class WBMSCore{ + private: + double HZ, DT; + hrp::Vector3 com_old, com_oldold, comacc, com_CP_ref_old; + HumanPose rp_ref_out_old; + unsigned int loop; + bool is_initial_loop; + int zmp_force_go_contact_count[LR]; + BiquadIIRFilterVec acc4zmp_v_filters, com_filter; + double com_filter_cutoff_hz_old; + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + bool legged; + double H_cur; + HumanPose hp_wld_raw, rp_ref_out; + hrp::Pose3 baselinkpose; + hrp::Vector3 cp_dec, cp_acc; + hrp::Vector4 act_foot_vert_fblr[LR], safe_foot_vert_fblr[LR]; + hrp::Vector2 com_forcp_ref,com_vel_forcp_ref; + class WBMSParams { + public: + bool auto_com_mode; + bool auto_floor_h_mode; + bool auto_foot_landing_by_act_cp; + bool auto_foot_landing_by_act_zmp; + double additional_double_support_time; + double auto_com_foot_move_detect_height; + double auto_floor_h_detect_fz; + double auto_floor_h_reset_fz; + double base_to_hand_min_distance; + double capture_point_extend_ratio; + double com_filter_cutoff_hz; + double foot_collision_avoidance_distance; + double foot_landing_vel; + double force_double_support_com_h; + double human_to_robot_ratio; + double max_double_support_width; + double upper_body_rmc_ratio; + double single_foot_zmp_safety_distance; + double swing_foot_height_offset; + hrp::Vector3 com_offset; + hrp::Vector4 actual_foot_vert_fbio; + hrp::Vector4 safety_foot_vert_fbio; + std::vector use_joints; + std::vector use_targets; + WBMSParams(){ + auto_com_mode = true; + auto_floor_h_mode = false; + auto_foot_landing_by_act_cp = false; + auto_foot_landing_by_act_zmp = true; + additional_double_support_time = 0.5;//0.4s以上は必須な気がする + auto_com_foot_move_detect_height = 0.03; + auto_floor_h_detect_fz = 50; + auto_floor_h_reset_fz = 30; + base_to_hand_min_distance = 0.5; + capture_point_extend_ratio = 1.0; + com_filter_cutoff_hz = 1.0; + foot_collision_avoidance_distance = 0.16; + foot_landing_vel = 0.4; + force_double_support_com_h = 0.9; + human_to_robot_ratio = 1.0;//human 1.1m vs jaxon 1.06m + max_double_support_width = 0.4; + upper_body_rmc_ratio = 0.0; + single_foot_zmp_safety_distance = 0.04; + swing_foot_height_offset = 0.02; + com_offset << 0, 0, 0; + actual_foot_vert_fbio << 0.13, -0.10, 0.06, -0.08; + safety_foot_vert_fbio << 0.02, -0.02, 0.02, 0.01; + CheckSafeLimit(); + } + void CheckSafeLimit(){ + LIMIT_MINMAX(additional_double_support_time , 0, 9); + LIMIT_MINMAX(auto_com_foot_move_detect_height , 0, 1); + LIMIT_MINMAX(auto_floor_h_detect_fz , 0, 1000); + LIMIT_MINMAX(auto_floor_h_reset_fz , 0, 1000); + LIMIT_MINMAX(base_to_hand_min_distance , 0, 1); + LIMIT_MINMAX(capture_point_extend_ratio , 0, 10); + LIMIT_MINMAX(com_filter_cutoff_hz , 0, 500); + LIMIT_MINMAX(foot_collision_avoidance_distance , 0, 1); + LIMIT_MINMAX(foot_landing_vel , 0.01, 2); + LIMIT_MINMAX(force_double_support_com_h , 0, 999); + LIMIT_MINMAX(human_to_robot_ratio , 0, 10); + LIMIT_MINMAX(max_double_support_width , 0, 2); + LIMIT_MINMAX(upper_body_rmc_ratio , 0, 1); + LIMIT_MINMAX(single_foot_zmp_safety_distance , 0, 0.2); + LIMIT_MINMAX(swing_foot_height_offset , 0, 0.1); + } + } wp; + struct ActualRobotState { + hrp::Vector3 ref_com, ref_zmp, st_zmp; + hrp::dvector6 act_foot_wrench[LR]; + hrp::Pose3 act_foot_pose[LR]; + } act_rs; + class WBMSStates { + public: + bool lock_foot_by_ref_zmp[LR]; + bool lock_foot_by_act_zmp[LR]; + bool lock_foot_by_ref_cp[LR]; + bool lock_foot_by_act_cp[LR]; + bool lock_both_feet_by_com_h; + int lock_foot_by_act_zmp_count[LR]; + string auto_com_pos_tgt; + WBMSStates(){ + for(int lr=0; lr& _ee_ikc_map){ + const std::string robot_l_names[4] = {"rleg","lleg","rarm","larm"}; + const int human_l_names[4] = {rf,lf,rh,lh}; + for(int i=0;i<4;i++){//HumanSynchronizerの初期姿勢オフセットをセット + if(_ee_ikc_map.count(robot_l_names[i])){ + rp_ref_out.tgt[human_l_names[i]].abs = + rp_ref_out.tgt[human_l_names[i]].offs = + rp_ref_out.tgt[human_l_names[i]].cnt = + _ee_ikc_map[robot_l_names[i]].getCurrentTargetPose(robot_in);// fik_in->getEndEffectorPos(robot_l_names[i]); + } + } + rp_ref_out.tgt[com].offs.p = act_rs.ref_com = act_rs.ref_zmp = act_rs.st_zmp = robot_in->calcCM(); + com_CP_ref_old = rp_ref_out.tgt[com].offs.p; + rp_ref_out.tgt[com].offs.R = robot_in->rootLink()->R; + rp_ref_out.tgt[zmp].offs.p.head(XY) = rp_ref_out.tgt[com].offs.p.head(XY); + rp_ref_out.tgt[zmp].offs.p(Z) = (rp_ref_out.tgt[rf].offs.p(Z) + rp_ref_out.tgt[lf].offs.p(Z)) / 2; + baselinkpose.p = robot_in->rootLink()->p; + baselinkpose.R = robot_in->rootLink()->R; + H_cur = rp_ref_out.tgt[com].offs.p(Z) - std::min((double)rp_ref_out.tgt[rf].offs.p(Z), (double)rp_ref_out.tgt[rf].offs.p(Z)); + } + void initializeRequest(hrp::BodyPtr robot_in, std::map& _ee_ikc_map){ + loop = 0; + is_initial_loop = true; + initializeHumanPoseFromCurrentInput(); + initializeRobotPoseFromHRPBody(robot_in, _ee_ikc_map); + rp_ref_out_old = rp_ref_out; + } + void update(){////////// メインループ //////////// + convertHumanToRobot (hp_wld_raw, rp_ref_out); + if(legged){ + setAutoCOMMode (hp_wld_raw, rp_ref_out_old, rp_ref_out); + + lockSwingFootIfZMPOutOfSupportFoot (rp_ref_out_old, rp_ref_out);// + limitEEWorkspace (rp_ref_out_old, rp_ref_out); + setFootContactPoseByGoContact (rp_ref_out_old, rp_ref_out); + limitFootVelNearGround (rp_ref_out_old, rp_ref_out); + avoidFootSinkIntoFloor (rp_ref_out); + + applyCOMToSupportRegionLimit (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p); + applyCOMToSupportRegionLimit (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old);//これやらないと支持領域の移動によって1ステップ前のCOM位置はもうはみ出てるかもしれないから + com_forcp_ref = rp_ref_out.tgt[com].abs.p.head(XY); + static hrp::Vector2 com_forcp_ref_old; + com_vel_forcp_ref = (com_forcp_ref - com_forcp_ref_old)/DT; + com_forcp_ref_old = com_forcp_ref; + applyCOMStateLimitByCapturePoint (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, com_CP_ref_old, rp_ref_out.tgt[com].abs.p); + applyCOMToSupportRegionLimit (rp_ref_out.tgt[rf].abs, rp_ref_out.tgt[lf].abs, rp_ref_out.tgt[com].abs.p); + applyZMPCalcFromCOM (rp_ref_out.tgt[com].abs.p, rp_ref_out.tgt[zmp].abs.p);//結局STに送るZMPは最終段で計算するからこれ意味ない + H_cur = rp_ref_out.tgt[com].abs.p(Z) - std::min((double)rp_ref_out.tgt[rf].abs.p(Z), (double)rp_ref_out.tgt[lf].abs.p(Z)); + rp_ref_out_old = rp_ref_out; + + ///// COMだけはフィルターいるか・・・ + if(is_initial_loop || wp.com_filter_cutoff_hz != com_filter_cutoff_hz_old){ + com_filter.setParameter(wp.com_filter_cutoff_hz, HZ, Q_NOOVERSHOOT); + com_filter.reset(rp_ref_out.tgt[com].abs.p); + com_filter_cutoff_hz_old = wp.com_filter_cutoff_hz; + } + rp_ref_out.tgt[com].abs.p = com_filter.passFilter(rp_ref_out.tgt[com].abs.p); + } + loop++; + is_initial_loop = false; + } + + private: + void convertHumanToRobot(const HumanPose& in, HumanPose& out){//結局初期指定からの移動量(=Rel)で計算をしてゆく + // out = in;//ダメゼッタイ + for(auto l : {com,rf,lf,rh,lh,head}){ + out.tgt[l].abs.p = wp.human_to_robot_ratio * (in.tgt[l].abs.p - in.tgt[l].offs.p) + out.tgt[l].offs.p; + out.tgt[l].abs.R = in.tgt[l].abs.R * in.tgt[l].offs.R.transpose() * out.tgt[l].offs.R; + } + for(auto l : {rf,lf,rh,lh}){ + out.tgt[l].w = in.tgt[l].w; + out.tgt[l].go_contact = in.tgt[l].go_contact; + } + out.tgt[com].abs.p += wp.com_offset; + out.tgt[zmp].abs = in.tgt[zmp].abs;//最近使わない + } + void setAutoCOMMode(const HumanPose& human, const HumanPose& old, HumanPose& out){ + if(wp.auto_com_mode){ + const double rf_h_from_floor = human.tgt[rf].abs.p(Z) - old.tgt[rf].cnt.p(Z); + const double lf_h_from_floor = human.tgt[lf].abs.p(Z) - old.tgt[lf].cnt.p(Z); + if ( rf_h_from_floor - lf_h_from_floor > wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "LF"; } + else if ( rf_h_from_floor - lf_h_from_floor < -wp.auto_com_foot_move_detect_height) { ws.auto_com_pos_tgt = "RF"; } + else { ws.auto_com_pos_tgt = "MID"; } + for(auto f : {rf,lf}){///// lock if com height is low = half sitting + if(act_rs.ref_com(Z) - old.tgt[f].cnt.p(Z) < wp.force_double_support_com_h) { ws.auto_com_pos_tgt = "MID"; ws.lock_both_feet_by_com_h = true; } + } + if (ws.auto_com_pos_tgt == "LF") { out.tgt[com].abs.p.head(XY) = old.tgt[lf].abs.p.head(XY); } + else if (ws.auto_com_pos_tgt == "RF") { out.tgt[com].abs.p.head(XY) = old.tgt[rf].abs.p.head(XY); } + else if (ws.auto_com_pos_tgt == "MID") { out.tgt[com].abs.p.head(XY) = (old.tgt[rf].abs.p.head(XY) + old.tgt[lf].abs.p.head(XY)) / 2; } + else { std::cerr<< "setAutoCOMMode something wrong!" << std::endl; } + } + } + void lockSwingFootIfZMPOutOfSupportFoot(const HumanPose& old, HumanPose& out){ + hrp::Vector2 to_opposite_foot[LR], ref_zmp_from_foot[LR], act_zmp_from_foot[LR]; + for(int lr=0; lr wp.single_foot_zmp_safety_distance); + ///// lock by act_zmp + if(wp.auto_foot_landing_by_act_zmp){ + act_zmp_from_foot[lr] = act_rs.st_zmp.head(XY) - old.foot(lr).abs.p.head(XY); + if(act_zmp_from_foot[lr].dot(to_opposite_foot[lr].normalized()) > wp.single_foot_zmp_safety_distance){ + ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)] = 0;// reset count + } + if(ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)] < HZ * wp.additional_double_support_time){ + ws.lock_foot_by_act_zmp_count[OPPOSITE(lr)]++;// count up till additional_double_support_time + ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = true;// lock swing foot till reach count limit + }else{ + ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = false;// release swing foot lock after reach count limit + } + }else{ + ws.lock_foot_by_act_zmp[OPPOSITE(lr)] = false; + } + } + for(int lr=0; lr wp.auto_floor_h_detect_fz){ + if( cnt_for_clear[lr] >= HZ * 1.0){ // first contact after detected floor height cleared + out.foot(lr).cnt.p(Z) = act_rs.act_foot_pose[lr].p(Z);// get act contact height as detected floor height + } + cnt_for_clear[lr] = 0;// keep resetting count during act foot force exists + }else{ + if(cnt_for_clear[lr] < HZ * 1.0){ // keep current detected floor height until reach count limit + cnt_for_clear[lr]++; + }else{ // clear detected floor height to zero after finish count up + out.foot(lr).cnt.p(Z) = out.foot(lr).offs.p(Z); + } + } + } + if(out.foot(lr).go_contact){ + out.foot(lr).abs.p.head(XY) = old.foot(lr).abs.p.head(XY); + // out.foot(lr).abs.p(Z) = out.foot(lr).offs.p(Z); + out.foot(lr).abs.p(Z) = out.foot(lr).cnt.p(Z); + out.foot(lr).abs.setRpy(0, 0, old.foot(lr).abs.rpy()(y)); + /////着地時に足を広げすぎないよう制限 + const hrp::Vector2 support_to_swing = out.foot(lr).abs.p.head(XY) - old.foot(OPPOSITE(lr)).abs.p.head(XY); + if(support_to_swing.norm() > wp.max_double_support_width){ + out.foot(lr).abs.p.head(XY) = old.foot(OPPOSITE(lr)).abs.p.head(XY) + support_to_swing.normalized() * wp.max_double_support_width; + } + }else{ + out.foot(lr).cnt.p.head(XY) = out.foot(lr).abs.p.head(XY); + out.foot(lr).cnt.R = hrp::rotFromRpy(0, 0, hrp::rpyFromRot(out.foot(lr).abs.R)(y)); + LIMIT_MIN( out.foot(lr).abs.p(Z), out.foot(lr).cnt.p(Z)+wp.swing_foot_height_offset); + } + // if(loop%500==0){ + // dbg(lr); + // dbg(out.foot(lr).cnt.p(Z)); + // dbg(out.foot(lr).abs.p(Z)); + // dbg(act_rs.act_foot_pose[lr].p(Z)); + // } + } + } + void limitFootVelNearGround(const HumanPose& old, HumanPose& out){ + for(int lr=0; lr0 ){ rf_landing_delay = 0; }//怪しい + // if(com_vel(Y)<0 ){ lf_landing_delay = 0; } + double foot_landing_delay = std::max(rf_landing_delay, lf_landing_delay); + //減速CP条件(現在のCPを常に両足裏で頭打ち) + hrp::Vector2 dcm_ragulated = (com_pos + com_vel * ( sqrt( H_cur / G ) + foot_landing_delay) * wp.capture_point_extend_ratio ); + if(!isPointInHull2D(dcm_ragulated, hull_d)){ + calcCrossPointOnHull(com_pos, dcm_ragulated, hull_d, dcm_ragulated); + com_vel_decel_ok = (dcm_ragulated - com_pos) / ( sqrt( H_cur / G ) + foot_landing_delay); + } + ///// ついでに減速CPによる強制遊脚着地も判定 + hrp::dmatrix foot_vert3d_check_wld[LR], one_foot_hull2d[LR], one_foot_vert_3d_for_check[LR]; + for(int lr=0; lr