PDcontroller.h
Go to the documentation of this file.
00001 // -*- mode: c++; -*-
00010 #ifndef PDcontroller_H
00011 #define PDcontroller_H
00012 
00013 #include <rtm/idl/BasicDataType.hh>
00014 #include <rtm/Manager.h>
00015 #include <rtm/DataFlowComponentBase.h>
00016 #include <rtm/CorbaPort.h>
00017 #include <rtm/CorbaNaming.h>
00018 #include <rtm/DataInPort.h>
00019 #include <rtm/DataOutPort.h>
00020 #include <rtm/idl/BasicDataTypeSkel.h>
00021 
00022 #include <hrpUtil/EigenTypes.h>
00023 #include <hrpModel/ModelLoaderUtil.h>
00024 #include <hrpModel/Body.h>
00025 #include <hrpModel/Link.h>
00026 
00027 // Service implementation headers
00028 // <rtc-template block="service_impl_h">
00029 
00030 // </rtc-template>
00031 
00032 // Service Consumer stub headers
00033 // <rtc-template block="consumer_stub_h">
00034 
00035 // </rtc-template>
00036 
00037 using namespace RTC;
00038 
00039 class PDcontroller
00040   : public RTC::DataFlowComponentBase
00041 {
00042  public:
00043   PDcontroller(RTC::Manager* manager);
00044   ~PDcontroller();
00045 
00046   // The initialize action (on CREATED->ALIVE transition)
00047   // formaer rtc_init_entry() 
00048  virtual RTC::ReturnCode_t onInitialize();
00049 
00050   // The finalize action (on ALIVE->END transition)
00051   // formaer rtc_exiting_entry()
00052   // virtual RTC::ReturnCode_t onFinalize();
00053 
00054   // The startup action when ExecutionContext startup
00055   // former rtc_starting_entry()
00056   // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id);
00057 
00058   // The shutdown action when ExecutionContext stop
00059   // former rtc_stopping_entry()
00060   // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id);
00061 
00062   // The activated action (Active state entry action)
00063   // former rtc_active_entry()
00064   virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
00065 
00066   // The deactivated action (Active state exit action)
00067   // former rtc_active_exit()
00068   virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id);
00069 
00070   // The execution action that is invoked periodically
00071   // former rtc_active_do()
00072   virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
00073 
00074   // The aborting action when main logic error occurred.
00075   // former rtc_aborting_entry()
00076   // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id);
00077 
00078   // The error action in ERROR state
00079   // former rtc_error_do()
00080   // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id);
00081 
00082   // The reset action that is invoked resetting
00083   // This is same but different the former rtc_init_entry()
00084   // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id);
00085   
00086   // The state update action that is invoked after onExecute() action
00087   // no corresponding operation exists in OpenRTm-aist-0.2.0
00088   // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id);
00089 
00090   // The action that is invoked when execution context's rate is changed
00091   // no corresponding operation exists in OpenRTm-aist-0.2.0
00092   // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id);
00093 
00094  protected:
00095   // Configuration variable declaration
00096   // <rtc-template block="config_declare">
00097   
00098   // </rtc-template>
00099 
00100   // DataInPort declaration
00101   // <rtc-template block="inport_declare">
00102   TimedDoubleSeq m_angle;
00103   InPort<TimedDoubleSeq> m_angleIn;
00104   TimedDoubleSeq m_angleRef;
00105   InPort<TimedDoubleSeq> m_angleRefIn;
00106   
00107   // </rtc-template>
00108 
00109   // DataOutPort declaration
00110   // <rtc-template block="outport_declare">
00111   TimedDoubleSeq m_torque;
00112   OutPort<TimedDoubleSeq> m_torqueOut;
00113   
00114   // </rtc-template>
00115 
00116   // CORBA Port declaration
00117   // <rtc-template block="corbaport_declare">
00118   
00119   // </rtc-template>
00120 
00121   // Service declaration
00122   // <rtc-template block="service_declare">
00123   
00124   // </rtc-template>
00125 
00126   // Consumer declaration
00127   // <rtc-template block="consumer_declare">
00128   
00129   // </rtc-template>
00130 
00131  private:
00132   void readGainFile();
00133   hrp::BodyPtr m_robot;
00134   double dt;     // sampling time of pd control
00135   double ref_dt; // sampling time of renference angles
00136   int step;      // current interpolation step
00137   int nstep;     // the number of steps to interpolate references
00138   std::ifstream gain;
00139   std::string gain_fname;
00140   hrp::dvector qold, qold_ref, Pgain, Dgain, tlimit_ratio;
00141   size_t dof, loop;
00142   unsigned int m_debugLevel;
00143   int dummy;
00144 };
00145 
00146 extern "C"
00147 {
00148    void PDcontrollerInit(RTC::Manager* manager);
00149 };
00150 
00151 #endif // PDcontroller_H


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