hrpsys: AutoBalancer.h Source File

Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef AUTOBALANCER_H
00011 #define AUTOBALANCER_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 #include "../ImpedanceController/JointPathEx.h"
00024 #include "../ImpedanceController/RatsMatrix.h"
00025 #include "GaitGenerator.h"
00026 // Service implementation headers
00027 // <rtc-template block="service_impl_h">
00028 #include "AutoBalancerService_impl.h"
00029 #include "interpolator.h"
00030 #include "../TorqueFilter/IIRFilter.h"
00031 #include "SimpleFullbodyInverseKinematicsSolver.h"
00032 
00033 // </rtc-template>
00034 
00035 // Service Consumer stub headers
00036 // <rtc-template block="consumer_stub_h">
00037 
00038 // </rtc-template>
00039 
00040 using namespace RTC;
00041 
00042 class AutoBalancer
00043   : public RTC::DataFlowComponentBase
00044 {
00045  public:
00046   AutoBalancer(RTC::Manager* manager);
00047   virtual ~AutoBalancer();
00048 
00049   // The initialize action (on CREATED->ALIVE transition)
00050   // formaer rtc_init_entry()
00051  virtual RTC::ReturnCode_t onInitialize();
00052 
00053   // The finalize action (on ALIVE->END transition)
00054   // formaer rtc_exiting_entry()
00055   virtual RTC::ReturnCode_t onFinalize();
00056 
00057   // The startup action when ExecutionContext startup
00058   // former rtc_starting_entry()
00059   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00060 
00061   // The shutdown action when ExecutionContext stop
00062   // former rtc_stopping_entry()
00063   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00064 
00065   // The activated action (Active state entry action)
00066   // former rtc_active_entry()
00067   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00068 
00069   // The deactivated action (Active state exit action)
00070   // former rtc_active_exit()
00071   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00072 
00073   // The execution action that is invoked periodically
00074   // former rtc_active_do()
00075   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00076 
00077   // The aborting action when main logic error occurred.
00078   // former rtc_aborting_entry()
00079   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00080 
00081   // The error action in ERROR state
00082   // former rtc_error_do()
00083   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00084 
00085   // The reset action that is invoked resetting
00086   // This is same but different the former rtc_init_entry()
00087   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00088 
00089   // The state update action that is invoked after onExecute() action
00090   // no corresponding operation exists in OpenRTm-aist-0.2.0
00091   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00092 
00093   // The action that is invoked when execution context's rate is changed
00094   // no corresponding operation exists in OpenRTm-aist-0.2.0
00095   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00096   bool goPos(const double& x, const double& y, const double& th);
00097   bool goVelocity(const double& vx, const double& vy, const double& vth);
00098   bool goStop();
00099   bool emergencyStop ();
00100   bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx);
00101   bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx);
00102   bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx);
00103   bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx);
00104   void waitFootSteps();
00105   void waitFootStepsEarly(const double tm);
00106   bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
00107   bool stopAutoBalancer();
00108   bool setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
00109   bool getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param);
00110   bool setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
00111   bool getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param);
00112   bool getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& i_param);
00113   bool adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep);
00114   bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx);
00115   bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep);
00116   bool releaseEmergencyStop();
00117   void distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp);
00118 
00119  protected:
00120   // Configuration variable declaration
00121   // <rtc-template block="config_declare">
00122   
00123   // </rtc-template>
00124 
00125   // DataInPort declaration
00126   // <rtc-template block="inport_declare">
00127   TimedDoubleSeq m_qRef;
00128   InPort<TimedDoubleSeq> m_qRefIn;
00129   TimedPoint3D m_basePos;
00130   InPort<TimedPoint3D> m_basePosIn;
00131   TimedOrientation3D m_baseRpy;
00132   InPort<TimedOrientation3D> m_baseRpyIn;
00133   TimedPoint3D m_zmp;
00134   InPort<TimedPoint3D> m_zmpIn;
00135   TimedDoubleSeq m_optionalData;
00136   InPort<TimedDoubleSeq> m_optionalDataIn;
00137   std::vector<TimedDoubleSeq> m_ref_force;
00138   std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
00139   TimedLong m_emergencySignal;
00140   InPort<TimedLong> m_emergencySignalIn;
00141   TimedPoint3D m_diffCP;
00142   InPort<TimedPoint3D> m_diffCPIn;
00143   TimedBooleanSeq m_actContactStates;
00144   InPort<TimedBooleanSeq> m_actContactStatesIn;
00145   TimedPoint3D m_refFootOriginExtMoment;
00146   InPort<TimedPoint3D> m_refFootOriginExtMomentIn;
00147   TimedBoolean m_refFootOriginExtMomentIsHoldValue;
00148   InPort<TimedBoolean> m_refFootOriginExtMomentIsHoldValueIn;
00149   // for debug
00150   TimedPoint3D m_cog;
00151   
00152   // </rtc-template>
00153 
00154   // DataOutPort declaration
00155   // <rtc-template block="outport_declare">
00156   OutPort<TimedDoubleSeq> m_qOut;
00157   RTC::OutPort<RTC::TimedPoint3D> m_zmpOut;
00158   OutPort<TimedPoint3D> m_basePosOut;
00159   OutPort<TimedOrientation3D> m_baseRpyOut;
00160   TimedDoubleSeq m_baseTform;
00161   OutPort<TimedDoubleSeq> m_baseTformOut;
00162   TimedPose3D m_basePose;
00163   OutPort<TimedPose3D> m_basePoseOut;
00164   TimedAcceleration3D m_accRef;
00165   OutPort<TimedAcceleration3D> m_accRefOut;
00166   TimedBooleanSeq m_contactStates;
00167   OutPort<TimedBooleanSeq> m_contactStatesOut;
00168   TimedDoubleSeq m_toeheelRatio;
00169   OutPort<TimedDoubleSeq> m_toeheelRatioOut;
00170   TimedDoubleSeq m_controlSwingSupportTime;
00171   OutPort<TimedDoubleSeq> m_controlSwingSupportTimeOut;
00172   TimedBoolean m_walkingStates;
00173   OutPort<TimedBoolean> m_walkingStatesOut;
00174   TimedPoint3D m_sbpCogOffset;
00175   OutPort<TimedPoint3D> m_sbpCogOffsetOut;
00176   std::vector<TimedDoubleSeq> m_force;
00177   std::vector<OutPort<TimedDoubleSeq> *> m_ref_forceOut;
00178   std::vector<TimedPoint3D> m_limbCOPOffset;
00179   std::vector<OutPort<TimedPoint3D> *> m_limbCOPOffsetOut;
00180   // for debug
00181   OutPort<TimedPoint3D> m_cogOut;
00182   
00183   // </rtc-template>
00184 
00185   // CORBA Port declaration
00186   // <rtc-template block="corbaport_declare">
00187   RTC::CorbaPort m_AutoBalancerServicePort;
00188 
00189   // </rtc-template>
00190 
00191   // Service declaration
00192   // <rtc-template block="service_declare">
00193   AutoBalancerService_impl m_service0;
00194 
00195   // </rtc-template>
00196 
00197   // Consumer declaration
00198   // <rtc-template block="consumer_declare">
00199   
00200   // </rtc-template>
00201 
00202  private:
00203   struct ABCIKparam {
00204     hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0;
00205     hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0;
00206     hrp::Link* target_link;
00207     bool is_active, has_toe_joint;
00208   };
00209   void getTargetParameters();
00210   void solveFullbodyIK ();
00211   void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs);
00212   void stopABCparam();
00213   void waitABCTransition();
00214   // Functions to calculate parameters for ABC output.
00215   // Output parameters are EE, limbCOPOffset, contactStates, controlSwingSupportTime, toeheelPhaseRatio
00216   void getOutputParametersForWalking ();
00217   void getOutputParametersForABC ();
00218   void getOutputParametersForIDLE ();
00219   void interpolateLegNamesAndZMPOffsets();
00220   void calcFixCoordsForAdjustFootstep (rats::coordinates& tmp_fix_coords);
00221   void rotateRefForcesForFixCoords (rats::coordinates& tmp_fix_coords);
00222   void updateTargetCoordsForHandFixMode (rats::coordinates& tmp_fix_coords);
00223   void calculateOutputRefForces ();
00224   hrp::Vector3 calcFootMidPosUsingZMPWeightMap ();
00225   void updateWalkingVelocityFromHandError (rats::coordinates& tmp_fix_coords);
00226   void calcReferenceJointAnglesForIK ();
00227   hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2);
00228   void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot);
00229   void fixLegToCoords2 (rats::coordinates& tmp_fix_coords);
00230   bool startWalking ();
00231   void stopWalking ();
00232   void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs);
00233   // static balance point offsetting
00234   void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height);
00235   void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height);
00236   hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords);
00237   bool isOptionalDataContact (const std::string& ee_name)
00238   {
00239       return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false;
00240   };
00241   bool calc_inital_support_legs(const double& y, std::vector<rats::coordinates>& initial_support_legs_coords, std::vector<rats::leg_type>& initial_support_legs, rats::coordinates& start_ref_coords);
00242   std::string getUseForceModeString ();
00243 
00244   // for gg
00245   typedef boost::shared_ptr<rats::gait_generator> ggPtr;
00246   ggPtr gg;
00247   bool gg_is_walking, gg_solved;
00248   // for abc
00249   typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> fikPtr;
00250   fikPtr fik;
00251   hrp::Vector3 ref_cog, ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset;
00252   enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type;
00253   enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode;
00254   std::map<std::string, ABCIKparam> ikp;
00255   std::map<std::string, size_t> contact_states_index_map;
00256   std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
00257   std::vector<std::string> sensor_names, leg_names, ee_vec;
00258   hrp::Vector3 target_root_p;
00259   hrp::Matrix33 target_root_R;
00260   rats::coordinates fix_leg_coords, fix_leg_coords2;
00261   std::vector<hrp::Vector3> default_zmp_offsets;
00262   double m_dt;
00263   hrp::BodyPtr m_robot;
00264   coil::Mutex m_mutex;
00265   double d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2];
00266   bool use_limb_stretch_avoidance;
00267 
00268   double transition_interpolator_ratio, transition_time, zmp_transition_time, adjust_footstep_transition_time, leg_names_interpolator_ratio;
00269   interpolator *zmp_offset_interpolator;
00270   interpolator *transition_interpolator;
00271   interpolator *adjust_footstep_interpolator;
00272   interpolator *leg_names_interpolator;
00273   hrp::Vector3 input_zmp, input_basePos;
00274   hrp::Matrix33 input_baseRot;
00275 
00276   // static balance point offsetting
00277   hrp::Vector3 sbp_offset, sbp_cog_offset;
00278   enum {MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT} use_force;
00279   std::vector<hrp::Vector3> ref_forces, ref_moments;
00280 
00281   unsigned int m_debugLevel;
00282   bool is_legged_robot, is_stop_mode, is_hand_fix_mode, is_hand_fix_initial;
00283   int loop;
00284   bool graspless_manip_mode;
00285   std::string graspless_manip_arm;
00286   hrp::Vector3 graspless_manip_p_gain;
00287   rats::coordinates graspless_manip_reference_trans_coords;
00288 
00289   hrp::InvDynStateBuffer idsb;
00290   std::vector<IIRFilter> invdyn_zmp_filters;
00291 
00292   // Used for ref force balancing.
00293   hrp::Link* additional_force_applied_link;
00294   hrp::Vector3 additional_force_applied_point_offset;
00295 };
00296 
00297 
00298 extern "C"
00299 {
00300   void AutoBalancerInit(RTC::Manager* manager);
00301 };
00302 
00303 #endif // IMPEDANCE_H