Stabilizer.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef STABILIZER_COMPONENT_H
00011 #define STABILIZER_COMPONENT_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/idl/ExtendedDataTypes.hh>
00015 #include <rtm/Manager.h>
00016 #include <rtm/DataFlowComponentBase.h>
00017 #include <rtm/CorbaPort.h>
00018 #include <rtm/DataInPort.h>
00019 #include <rtm/DataOutPort.h>
00020 #include <rtm/idl/BasicDataTypeSkel.h>
00021 #include <rtm/idl/ExtendedDataTypesSkel.h>
00022 #include <hrpModel/Body.h>
00023 
00024 // Service implementation headers
00025 // <rtc-template block="service_impl_h">
00026 #include "StabilizerService_impl.h"
00027 #include "TwoDofController.h"
00028 #include "ZMPDistributor.h"
00029 #include "../ImpedanceController/JointPathEx.h"
00030 #include "../ImpedanceController/RatsMatrix.h"
00031 #include "../TorqueFilter/IIRFilter.h"
00032 
00033 // </rtc-template>
00034 
00035 // Service Consumer stub headers
00036 // <rtc-template block="consumer_stub_h">
00037 
00038 // </rtc-template>
00039 
00044 class Stabilizer
00045   : public RTC::DataFlowComponentBase
00046 {
00047  public:
00052   Stabilizer(RTC::Manager* manager);
00056   virtual ~Stabilizer();
00057 
00058   // The initialize action (on CREATED->ALIVE transition)
00059   // formaer rtc_init_entry()
00060   virtual RTC::ReturnCode_t onInitialize();
00061 
00062   // The finalize action (on ALIVE->END transition)
00063   // formaer rtc_exiting_entry()
00064   virtual RTC::ReturnCode_t onFinalize();
00065 
00066   // The startup action when ExecutionContext startup
00067   // former rtc_starting_entry()
00068   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00069 
00070   // The shutdown action when ExecutionContext stop
00071   // former rtc_stopping_entry()
00072   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00073 
00074   // The activated action (Active state entry action)
00075   // former rtc_active_entry()
00076   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00077 
00078   // The deactivated action (Active state exit action)
00079   // former rtc_active_exit()
00080   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00081 
00082   // The execution action that is invoked periodically
00083   // former rtc_active_do()
00084   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00085 
00086   // The aborting action when main logic error occurred.
00087   // former rtc_aborting_entry()
00088   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00089 
00090   // The error action in ERROR state
00091   // former rtc_error_do()
00092   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00093 
00094   // The reset action that is invoked resetting
00095   // This is same but different the former rtc_init_entry()
00096   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00097 
00098   // The state update action that is invoked after onExecute() action
00099   // no corresponding operation exists in OpenRTm-aist-0.2.0
00100   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00101 
00102   // The action that is invoked when execution context's rate is changed
00103   // no corresponding operation exists in OpenRTm-aist-0.2.0
00104   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00105 
00106   void startStabilizer(void);
00107   void stopStabilizer(void);
00108   void getCurrentParameters ();
00109   void getActualParameters ();
00110   void getTargetParameters ();
00111   void calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot);
00112   void sync_2_st ();
00113   void sync_2_idle();
00114   bool calcZMP(hrp::Vector3& ret_zmp, const double zmp_z);
00115   void calcStateForEmergencySignal();
00116   void calcRUNST();
00117   void moveBasePosRotForBodyRPYControl ();
00118   void calcSwingSupportLimbGain();
00119   void calcTPCC();
00120   void calcEEForceMomentControl();
00121   void calcSwingEEModification ();
00122   void limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& ee_p, const std::vector<hrp::Matrix33>& ee_R);
00123   void getParameter(OpenHRP::StabilizerService::stParam& i_stp);
00124   void setParameter(const OpenHRP::StabilizerService::stParam& i_stp);
00125   void setBoolSequenceParam (std::vector<bool>& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name);
00126   void setBoolSequenceParamWithCheckContact (std::vector<bool>& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name);
00127   std::string getStabilizerAlgorithmString (OpenHRP::StabilizerService::STAlgorithm _st_algorithm);
00128   void waitSTTransition();
00129   // funcitons for calc final torque output
00130   void calcContactMatrix (hrp::dmatrix& tm, const std::vector<hrp::Vector3>& contact_p);
00131   void calcTorque ();
00132   void fixLegToCoords (const std::string& leg, const rats::coordinates& coords);
00133   void getFootmidCoords (rats::coordinates& ret);
00134   double calcDampingControl (const double tau_d, const double tau, const double prev_d,
00135                              const double DD, const double TT);
00136   hrp::Vector3 calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT);
00137   double calcDampingControl (const double prev_d, const double TT);
00138   hrp::Vector3 calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d,
00139                                    const hrp::Vector3& DD, const hrp::Vector3& TT);
00140   double vlimit(double value, double llimit_value, double ulimit_value);
00141   hrp::Vector3 vlimit(const hrp::Vector3& value, double llimit_value, double ulimit_value);
00142   hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& limit_value);
00143   hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value);
00144 
00145   inline bool isContact (const size_t idx) // 0 = right, 1 = left
00146   {
00147     return (prev_act_force_z[idx] > 25.0);
00148   };
00149   inline int calcMaxTransitionCount ()
00150   {
00151       return (transition_time / dt);
00152   };
00153   void calcDiffFootOriginExtMoment ();
00154 
00155  protected:
00156   // Configuration variable declaration
00157   // <rtc-template block="config_declare">
00158   
00159   // </rtc-template>
00160   RTC::TimedDoubleSeq m_qCurrent;
00161   RTC::TimedDoubleSeq m_qRef;
00162   RTC::TimedDoubleSeq m_tau;
00163   RTC::TimedOrientation3D m_rpy;
00164   RTC::TimedPoint3D m_zmpRef;
00165   RTC::TimedPoint3D m_zmp;
00166   RTC::TimedPoint3D m_refCP;
00167   RTC::TimedPoint3D m_actCP;
00168   RTC::TimedPoint3D m_diffCP;
00169   RTC::TimedPoint3D m_diffFootOriginExtMoment;
00170   RTC::TimedPoint3D m_basePos;
00171   RTC::TimedOrientation3D m_baseRpy;
00172   RTC::TimedBooleanSeq m_contactStates;
00173   RTC::TimedDoubleSeq m_toeheelRatio;
00174   RTC::TimedDoubleSeq m_controlSwingSupportTime;
00175   std::vector<RTC::TimedPoint3D> m_limbCOPOffset;
00176   RTC::TimedBooleanSeq m_actContactStates;
00177   RTC::TimedDoubleSeq m_COPInfo;
00178   RTC::TimedLong m_emergencySignal;
00179   RTC::TimedDoubleSeq m_qRefSeq;
00180   RTC::TimedBoolean m_walkingStates;
00181   RTC::TimedPoint3D m_sbpCogOffset;
00182   // for debug ouput
00183   RTC::TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp;
00184   RTC::TimedPoint3D m_originActZmp, m_originActCog, m_originActCogVel;
00185   RTC::TimedOrientation3D m_actBaseRpy;
00186   RTC::TimedPoint3D m_currentBasePos;
00187   RTC::TimedOrientation3D m_currentBaseRpy;
00188   RTC::TimedDoubleSeq m_allRefWrench;
00189   RTC::TimedDoubleSeq m_allEEComp;
00190   RTC::TimedDoubleSeq m_debugData;
00191   
00192   // DataInPort declaration
00193   // <rtc-template block="inport_declare">
00194   RTC::InPort<RTC::TimedDoubleSeq> m_qCurrentIn;
00195   RTC::InPort<RTC::TimedDoubleSeq> m_qRefIn;
00196   RTC::InPort<RTC::TimedOrientation3D> m_rpyIn;
00197   RTC::InPort<RTC::TimedPoint3D> m_zmpRefIn;
00198   RTC::InPort<RTC::TimedPoint3D> m_basePosIn;
00199   RTC::InPort<RTC::TimedOrientation3D> m_baseRpyIn;
00200   RTC::InPort<RTC::TimedBooleanSeq> m_contactStatesIn;
00201   RTC::InPort<RTC::TimedDoubleSeq> m_toeheelRatioIn;
00202   RTC::InPort<RTC::TimedDoubleSeq> m_controlSwingSupportTimeIn;
00203   std::vector<RTC::InPort<RTC::TimedPoint3D> *> m_limbCOPOffsetIn;
00204   RTC::InPort<RTC::TimedDoubleSeq> m_qRefSeqIn;
00205   RTC::InPort<RTC::TimedBoolean> m_walkingStatesIn;
00206   RTC::InPort<RTC::TimedPoint3D> m_sbpCogOffsetIn;
00207 
00208   std::vector<RTC::TimedDoubleSeq> m_wrenches;
00209   std::vector<RTC::InPort<RTC::TimedDoubleSeq> *> m_wrenchesIn;
00210   std::vector<RTC::TimedDoubleSeq> m_ref_wrenches;
00211   std::vector<RTC::InPort<RTC::TimedDoubleSeq> *> m_ref_wrenchesIn;
00212   
00213   // </rtc-template>
00214 
00215   // DataOutPort declaration
00216   // <rtc-template block="outport_declare">
00217   RTC::OutPort<RTC::TimedDoubleSeq> m_qRefOut;
00218   RTC::OutPort<RTC::TimedDoubleSeq> m_tauOut;
00219   RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
00220   RTC::OutPort<RTC::TimedPoint3D> m_refCPOut;
00221   RTC::OutPort<RTC::TimedPoint3D> m_actCPOut;
00222   RTC::OutPort<RTC::TimedPoint3D> m_diffCPOut;
00223   RTC::OutPort<RTC::TimedPoint3D> m_diffFootOriginExtMomentOut;
00224   RTC::OutPort<RTC::TimedBooleanSeq> m_actContactStatesOut;
00225   RTC::OutPort<RTC::TimedDoubleSeq> m_COPInfoOut;
00226   RTC::OutPort<RTC::TimedLong> m_emergencySignalOut;
00227   // for debug output
00228   RTC::OutPort<RTC::TimedPoint3D> m_originRefZmpOut, m_originRefCogOut, m_originRefCogVelOut, m_originNewZmpOut;
00229   RTC::OutPort<RTC::TimedPoint3D> m_originActZmpOut, m_originActCogOut, m_originActCogVelOut;
00230   RTC::OutPort<RTC::TimedOrientation3D> m_actBaseRpyOut;
00231   RTC::OutPort<RTC::TimedPoint3D> m_currentBasePosOut;
00232   RTC::OutPort<RTC::TimedOrientation3D> m_currentBaseRpyOut;
00233   RTC::OutPort<RTC::TimedDoubleSeq> m_allRefWrenchOut;
00234   RTC::OutPort<RTC::TimedDoubleSeq> m_allEECompOut;
00235   RTC::OutPort<RTC::TimedDoubleSeq> m_debugDataOut;
00236   
00237   // </rtc-template>
00238 
00239   // CORBA Port declaration
00240   // <rtc-template block="corbaport_declare">
00241   
00242   // </rtc-template>
00243 
00244   // Service declaration
00245   // <rtc-template block="service_declare">
00246   RTC::CorbaPort m_StabilizerServicePort;
00247   
00248   // </rtc-template>
00249 
00250   // Consumer declaration
00251   // <rtc-template block="consumer_declare">
00252   StabilizerService_impl m_service0;
00253   
00254   // </rtc-template>
00255 
00256  private:
00257   // Stabilizer Parameters
00258   struct STIKParam {
00259     std::string target_name; // Name of end link
00260     std::string ee_name; // Name of ee (e.g., rleg, lleg, ...)
00261     std::string sensor_name; // Name of force sensor in the limb
00262     std::string parent_name; // Name of parent ling in the limb
00263     hrp::Vector3 localp; // Position of ee in end link frame (^{l}p_e = R_l^T (p_e - p_l))
00264     hrp::Vector3 localCOPPos; // Position offset of reference COP in end link frame (^{l}p_{cop} = R_l^T (p_{cop} - p_l) - ^{l}p_e)
00265     hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e)
00266     // For eefm
00267     hrp::Vector3 d_foot_pos, d_foot_rpy, ee_d_foot_rpy;
00268     hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const, eefm_swing_rot_spring_gain, eefm_swing_pos_spring_gain, eefm_swing_rot_time_const, eefm_swing_pos_time_const, eefm_ee_moment_limit;
00269     double eefm_pos_compensation_limit, eefm_rot_compensation_limit;
00270     hrp::Vector3 ref_force, ref_moment;
00271     hrp::dvector6 eefm_ee_forcemoment_distribution_weight;
00272     double swing_support_gain, support_time;
00273     // For swing ee modification
00274     boost::shared_ptr<FirstOrderLowPassFilter<hrp::Vector3> > target_ee_diff_p_filter, target_ee_diff_r_filter;
00275     hrp::Vector3 target_ee_diff_p, d_pos_swing, d_rpy_swing, prev_d_pos_swing, prev_d_rpy_swing;
00276     hrp::Matrix33 target_ee_diff_r;
00277     // IK parameter
00278     double avoid_gain, reference_gain, max_limb_length, limb_length_margin;
00279     size_t ik_loop_count;
00280   };
00281   enum cmode {MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR} control_mode;
00282   // members
00283   std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
00284   std::vector<hrp::JointPathExPtr> jpe_v;
00285   hrp::BodyPtr m_robot;
00286   coil::Mutex m_mutex;
00287   unsigned int m_debugLevel;
00288   hrp::dvector transition_joint_q, qorg, qrefv;
00289   std::vector<STIKParam> stikp;
00290   std::map<std::string, size_t> contact_states_index_map;
00291   std::vector<bool> ref_contact_states, prev_ref_contact_states, act_contact_states, is_ik_enable, is_feedback_control_enable, is_zmp_calc_enable;
00292   std::vector<double> toeheel_ratio;
00293   double dt;
00294   int transition_count, loop;
00295   int m_is_falling_counter;
00296   std::vector<int> m_will_fall_counter;
00297   int is_air_counter, detection_count_to_air;
00298   bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_limb_stretch_avoidance, use_zmp_truncation;
00299   bool is_walking, is_estop_while_walking;
00300   hrp::Vector3 current_root_p, target_root_p;
00301   hrp::Matrix33 current_root_R, target_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot, ref_foot_origin_rot;
00302   std::vector <hrp::Vector3> target_ee_p, rel_ee_pos, act_ee_p, projected_normal, act_force, ref_force, ref_moment;
00303   std::vector <hrp::Matrix33> target_ee_R, rel_ee_rot, act_ee_R;
00304   std::vector<std::string> rel_ee_name;
00305   rats::coordinates target_foot_midcoords;
00306   hrp::Vector3 ref_zmp, ref_cog, ref_cp, ref_cogvel, rel_ref_cp, prev_ref_cog, prev_ref_zmp;
00307   hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset, cp_offset, diff_cp;
00308   hrp::Vector3 foot_origin_offset[2];
00309   std::vector<double> prev_act_force_z;
00310   double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], root_rot_compensation_limit[2];
00311   boost::shared_ptr<FirstOrderLowPassFilter<hrp::Vector3> > act_cogvel_filter;
00312   OpenHRP::StabilizerService::STAlgorithm st_algorithm;
00313   SimpleZMPDistributor* szd;
00314   std::vector<std::vector<Eigen::Vector2d> > support_polygon_vetices, margined_support_polygon_vetices;
00315   // TPCC
00316   double k_tpcc_p[2], k_tpcc_x[2], d_rpy[2], k_brot_p[2], k_brot_tc[2];
00317   // RUN ST
00318   TwoDofController m_tau_x[2], m_tau_y[2], m_f_z;
00319   hrp::Vector3 pdr;
00320   double m_torque_k[2], m_torque_d[2]; // 3D-LIP parameters (0: x, 1: y)
00321   double pangx_ref, pangy_ref, pangx, pangy;
00322   double k_run_b[2], d_run_b[2];
00323   double rdx, rdy, rx, ry;
00324   // EEFM ST
00325   double eefm_k1[2], eefm_k2[2], eefm_k3[2], eefm_zmp_delay_time_const[2], eefm_body_attitude_control_gain[2], eefm_body_attitude_control_time_const[2];
00326   double eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_gravitational_acceleration;
00327   std::vector<double> eefm_swing_damping_force_thre, eefm_swing_damping_moment_thre;
00328   hrp::Vector3 new_refzmp, rel_cog, ref_zmp_aux, diff_foot_origin_ext_moment;
00329   hrp::Vector3 pos_ctrl;
00330   hrp::Vector3 ref_total_force, ref_total_moment;
00331   // Total foot moment around the foot origin coords (relative to foot origin coords)
00332   hrp::Vector3 ref_total_foot_origin_moment, act_total_foot_origin_moment;
00333   hrp::Vector3 eefm_swing_pos_damping_gain, eefm_swing_rot_damping_gain;
00334   double total_mass, transition_time, cop_check_margin, contact_decision_threshold;
00335   std::vector<double> cp_check_margin, tilt_margin;
00336   OpenHRP::StabilizerService::EmergencyCheckMode emergency_check_mode;
00337 };
00338 
00339 
00340 extern "C"
00341 {
00342   void StabilizerInit(RTC::Manager* manager);
00343 };
00344 
00345 #endif // STABILIZER_COMPONENT_H


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:56