ImpedanceController.h
Go to the documentation of this file.
00001 // -*- C++ -*-
00010 #ifndef IMPEDANCE_H
00011 #define IMPEDANCE_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 "JointPathEx.h"
00024 #include "RatsMatrix.h"
00025 #include "ImpedanceOutputGenerator.h"
00026 // Service implementation headers
00027 // <rtc-template block="service_impl_h">
00028 #include "ImpedanceControllerService_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 ImpedanceController
00040   : public RTC::DataFlowComponentBase
00041 {
00042  public:
00043   ImpedanceController(RTC::Manager* manager);
00044   virtual ~ImpedanceController();
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   bool startImpedanceController(const std::string& i_name_);
00095   bool startImpedanceControllerNoWait(const std::string& i_name_);
00096   bool stopImpedanceController(const std::string& i_name_);
00097   bool stopImpedanceControllerNoWait(const std::string& i_name_);
00098   bool setImpedanceControllerParam(const std::string& i_name_, OpenHRP::ImpedanceControllerService::impedanceParam i_param_);
00099   bool getImpedanceControllerParam(const std::string& i_name_, OpenHRP::ImpedanceControllerService::impedanceParam& i_param_);
00100   void waitImpedanceControllerTransition(std::string i_name_);
00101 
00102  protected:
00103   // Configuration variable declaration
00104   // <rtc-template block="config_declare">
00105   
00106   // </rtc-template>
00107 
00108   // DataInPort declaration
00109   // <rtc-template block="inport_declare">
00110   TimedDoubleSeq m_qCurrent;
00111   InPort<TimedDoubleSeq> m_qCurrentIn;
00112   TimedDoubleSeq m_qRef;
00113   InPort<TimedDoubleSeq> m_qRefIn;
00114   TimedPoint3D m_basePos;
00115   InPort<TimedPoint3D> m_basePosIn;
00116   TimedOrientation3D m_baseRpy;
00117   InPort<TimedOrientation3D> m_baseRpyIn;
00118   std::vector<TimedDoubleSeq> m_force;
00119   std::vector<InPort<TimedDoubleSeq> *> m_forceIn;
00120   std::vector<TimedDoubleSeq> m_ref_force;
00121   std::vector<InPort<TimedDoubleSeq> *> m_ref_forceIn;
00122   TimedOrientation3D m_rpy;
00123   InPort<TimedOrientation3D> m_rpyIn;
00124   
00125   // </rtc-template>
00126 
00127   // DataOutPort declaration
00128   // <rtc-template block="outport_declare">
00129   TimedDoubleSeq m_q;
00130   OutPort<TimedDoubleSeq> m_qOut;
00131   
00132   // </rtc-template>
00133 
00134   // CORBA Port declaration
00135   // <rtc-template block="corbaport_declare">
00136   RTC::CorbaPort m_ImpedanceControllerServicePort;
00137 
00138   // </rtc-template>
00139 
00140   // Service declaration
00141   // <rtc-template block="service_declare">
00142   ImpedanceControllerService_impl m_service0;
00143 
00144   // </rtc-template>
00145 
00146   // Consumer declaration
00147   // <rtc-template block="consumer_declare">
00148   
00149   // </rtc-template>
00150 
00151  private:
00152 
00153   struct ImpedanceParam : public ImpedanceOutputGenerator {
00154     std::string sensor_name;
00155     hrp::Vector3 ref_force, ref_moment;
00156     double sr_gain, avoid_gain, reference_gain, manipulability_limit;
00157     int transition_count; // negative value when initing and positive value when deleting
00158     hrp::dvector transition_joint_q;
00159     hrp::JointPathExPtr manip;
00160     bool is_active;
00161 
00162     ImpedanceParam ()
00163       : ImpedanceOutputGenerator(),
00164         ref_force(hrp::Vector3::Zero()), ref_moment(hrp::Vector3::Zero()),
00165         sr_gain(1.0), avoid_gain(0.001), reference_gain(0.01), manipulability_limit(0.1), transition_count(0), is_active(false)
00166     {};
00167   };
00168   struct ee_trans {
00169     std::string target_name;
00170     hrp::Vector3 localPos;
00171     hrp::Matrix33 localR;
00172   };
00173 
00174   void copyImpedanceParam (OpenHRP::ImpedanceControllerService::impedanceParam& i_param_, const ImpedanceParam& param);
00175   void updateRootLinkPosRot (TimedOrientation3D tmprpy);
00176   void getTargetParameters ();
00177   void calcImpedanceControl ();
00178   void calcForceMoment();
00179 
00180   std::map<std::string, ImpedanceParam> m_impedance_param;
00181   std::map<std::string, ee_trans> ee_map;
00182   std::map<std::string, hrp::VirtualForceSensorParam> m_vfs;
00183   std::map<std::string, hrp::Vector3> abs_forces, abs_moments, abs_ref_forces, abs_ref_moments;
00184   double m_dt;
00185   hrp::BodyPtr m_robot;
00186   coil::Mutex m_mutex;
00187   hrp::dvector qrefv;
00188   unsigned int m_debugLevel;
00189   int dummy;
00190   int loop;
00191   bool use_sh_base_pos_rpy;
00192 };
00193 
00194 
00195 extern "C"
00196 {
00197   void ImpedanceControllerInit(RTC::Manager* manager);
00198 };
00199 
00200 #endif // IMPEDANCE_H


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