TorqueController.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef TORQUE_CONTROLLER_H
00011 #define TPRQUE_CONTROLLER_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/DataInPort.h>
00018 #include <rtm/DataOutPort.h>
00019 #include <rtm/idl/BasicDataTypeSkel.h>
00020 
00021 #include <hrpModel/Body.h>
00022 #include <hrpModel/Link.h>
00023 #include <hrpModel/JointPath.h>
00024 
00025 #include "MotorTorqueController.h"
00026 
00027 // Service implementation headers
00028 // <rtc-template block="service_impl_h">
00029 
00030 #include "TorqueControllerService_impl.h"
00031 
00032 // </rtc-template>
00033 
00034 // Service Consumer stub headers
00035 // <rtc-template block="consumer_stub_h">
00036 
00037 // </rtc-template>
00038 
00039 using namespace RTC;
00040 
00044 class TorqueController
00045   : public RTC::DataFlowComponentBase
00046 {
00047 public:
00052   TorqueController(RTC::Manager* manager);
00056   virtual ~TorqueController();
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   bool enableTorqueController(std::string jname);
00106   bool enableMultipleTorqueControllers(const OpenHRP::TorqueControllerService::StrSequence& jnames);
00107   bool disableTorqueController(std::string jname);
00108   bool disableMultipleTorqueControllers(const OpenHRP::TorqueControllerService::StrSequence& jnames);
00109   bool startTorqueControl(std::string jname);
00110   bool startMultipleTorqueControls(const OpenHRP::TorqueControllerService::StrSequence& jnames);
00111   bool stopTorqueControl(std::string jname);
00112   bool stopMultipleTorqueControls(const OpenHRP::TorqueControllerService::StrSequence& jnames);
00113   bool setReferenceTorque(std::string jname, double tauRef);
00114   bool setMultipleReferenceTorques(const OpenHRP::TorqueControllerService::StrSequence& jnames, const OpenHRP::TorqueControllerService::dSequence& tauRefs);
00115   bool setTorqueControllerParam(const std::string jname, const OpenHRP::TorqueControllerService::torqueControllerParam& i_param);
00116   bool getTorqueControllerParam(const std::string jname, OpenHRP::TorqueControllerService::torqueControllerParam& i_param);
00117   
00118 protected:
00119 // Configuration variable declaration
00120 // <rtc-template block="config_declare">
00121   
00122 // </rtc-template>
00123   TimedDoubleSeq m_tauCurrentIn;
00124   TimedDoubleSeq m_tauMaxIn;
00125   TimedDoubleSeq m_qCurrentIn;
00126   TimedDoubleSeq m_qRefIn;
00127 
00128   TimedDoubleSeq m_qRefOut;
00129 
00130 // DataInPort declaration
00131 // <rtc-template block="inport_declare">
00132   InPort<TimedDoubleSeq> m_tauCurrentInIn;
00133   InPort<TimedDoubleSeq> m_tauMaxInIn;
00134   InPort<TimedDoubleSeq> m_qCurrentInIn;
00135   InPort<TimedDoubleSeq> m_qRefInIn;
00136   
00137 // </rtc-template>
00138 
00139 // DataOutPort declaration
00140 // <rtc-template block="outport_declare">
00141   OutPort<TimedDoubleSeq> m_qRefOutOut;
00142   
00143 // </rtc-template>
00144 
00145 // CORBA Port declaration
00146 // <rtc-template block="corbaport_declare">
00147   
00148 // </rtc-template>
00149 
00150 // Service declaration
00151 // <rtc-template block="service_declare">
00152   RTC::CorbaPort m_TorqueControllerServicePort;
00153   
00154 // </rtc-template>
00155 
00156 // Consumer declaration
00157 // <rtc-template block="consumer_declare">
00158   TorqueControllerService_impl m_service0;
00159   
00160 // </rtc-template>
00161   
00162 private:
00163   double m_dt;
00164   unsigned int m_debugLevel;
00165   long long m_loop;
00166   hrp::BodyPtr m_robot;
00167   std::vector<MotorTorqueController> m_motorTorqueControllers;
00168   coil::Mutex m_mutex;
00169   void executeTorqueControl(hrp::dvector &dq);
00170   void updateParam(double &val, double &val_new);
00171   bool isDebug(int cycle = 20);
00172 };
00173 
00174 
00175 extern "C"
00176 {
00177   void TorqueControllerInit(RTC::Manager* manager);
00178 };
00179 
00180 #endif // NULL_COMPONENT_H


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